//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2::meta::make_integer"

#include <boost/mpl/placeholders.hpp>
#include <nt2/sdk/config/types.hpp>
#include <nt2/sdk/meta/make_integer.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test make_integer output
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(make_integer)
{
  using nt2::meta::make_integer;
  using boost::is_same;
  using namespace nt2;

  NT2_TEST( (is_same<nt2::int8_t   , make_integer<1, signed  >::type >::value)  );
  NT2_TEST( (is_same<nt2::int16_t  , make_integer<2, signed  >::type >::value)  );
  NT2_TEST( (is_same<nt2::int32_t  , make_integer<4, signed  >::type >::value)  );
  NT2_TEST( (is_same<nt2::int64_t  , make_integer<8, signed  >::type >::value)  );
  NT2_TEST( (is_same<nt2::uint8_t  , make_integer<1,unsigned >::type >::value)  );
  NT2_TEST( (is_same<nt2::uint16_t , make_integer<2,unsigned >::type >::value)  );
  NT2_TEST( (is_same<nt2::uint32_t , make_integer<4,unsigned >::type >::value)  );
  NT2_TEST( (is_same<nt2::uint64_t , make_integer<8,unsigned >::type >::value)  );
}

////////////////////////////////////////////////////////////////////////////////
// Test make_integer transformed output
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(make_integer_transform)
{
  using nt2::meta::make_integer;
  using boost::add_pointer;
  using boost::is_same;
  using boost::mpl::_;
  using namespace nt2;

  NT2_TEST( (is_same<nt2::int8_t*  , make_integer<1, signed , add_pointer<_> >::type >::value)  );
  NT2_TEST( (is_same<nt2::int16_t* , make_integer<2, signed , add_pointer<_> >::type >::value)  );
  NT2_TEST( (is_same<nt2::int32_t* , make_integer<4, signed , add_pointer<_> >::type >::value)  );
  NT2_TEST( (is_same<nt2::int64_t* , make_integer<8, signed , add_pointer<_> >::type >::value)  );
  NT2_TEST( (is_same<nt2::uint8_t* , make_integer<1,unsigned, add_pointer<_> >::type >::value)  );
  NT2_TEST( (is_same<nt2::uint16_t*, make_integer<2,unsigned, add_pointer<_> >::type >::value)  );
  NT2_TEST( (is_same<nt2::uint32_t*, make_integer<4,unsigned, add_pointer<_> >::type >::value)  );
  NT2_TEST( (is_same<nt2::uint64_t*, make_integer<8,unsigned, add_pointer<_> >::type >::value)  );
}


#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../jngen.h"

#include <sstream>

using jngen::GenericGraph;

BOOST_AUTO_TEST_SUITE(generic_graph)

BOOST_AUTO_TEST_CASE(basics) {
    GenericGraph gg;

    BOOST_TEST(gg.n() == 0);
    BOOST_TEST(gg.m() == 0);
    BOOST_TEST(gg.isConnected());
    BOOST_TEST(gg.edges().empty());

    gg.addEdge(0, 2);
    gg.addEdge(1, 3);
    BOOST_TEST(gg.edges().size() == 2);
    BOOST_CHECK(gg.edges()[0] == std::make_pair(0, 2));
    BOOST_CHECK(gg.edges()[1] == std::make_pair(1, 3));
    BOOST_TEST(gg.n() == 4);
    BOOST_TEST(gg.m() == 2);
    BOOST_TEST(!gg.isConnected());

    gg.addEdge(0, 3);
    BOOST_TEST(gg.isConnected());
    gg.addEdge(0, 3);
    BOOST_TEST(gg.m() == 4);

    auto g2 = gg;
    BOOST_CHECK(gg == g2);
    g2.addEdge(1, 2);
    BOOST_CHECK(!(gg == g2));
}

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright (c) 2005 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
//=======================================================================
#include <cstdlib>
#include <iostream>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <ctime>
#include <boost/random.hpp>

#include <boost/graph/max_cardinality_matching.hpp>

using namespace boost;

typedef adjacency_list< vecS, vecS, undirectedS,
    property< vertex_index_t, int > >
    undirected_graph;

typedef property_map< undirected_graph, vertex_index_t >::type
    vertex_index_map_t;
typedef vector_property_map<
    graph_traits< undirected_graph >::vertex_descriptor, vertex_index_map_t >
    mate_t;
typedef graph_traits< undirected_graph >::vertex_iterator vertex_iterator_t;
typedef graph_traits< undirected_graph >::vertex_descriptor vertex_descriptor_t;
typedef graph_traits< undirected_graph >::vertices_size_type v_size_t;

int main(int argc, char** argv)
{
    if (argc < 3)
    {
        std::cout << "Usage: " << argv[0] << " n m" << std::endl
                  << "Tests the checked matching on a random graph w/ n "
                     "vertices and m edges"
                  << std::endl;
        exit(-1);
    }

    int n = atoi(argv[1]);
    int m = atoi(argv[2]);

    undirected_graph g(n);

    typedef boost::mt19937 base_generator_type;
    base_generator_type generator(static_cast< unsigned int >(std::time(0)));
    boost::uniform_int<> distribution(0, n - 1);
    boost::variate_generator< base_generator_type&, boost::uniform_int<> >
        rand_num(generator, distribution);

    int num_edges = 0;
    bool success;

    while (num_edges < m)
    {
        vertex_descriptor_t u = random_vertex(g, rand_num);
        vertex_descriptor_t v = random_vertex(g, rand_num);
        if (u != v)
        {
            if (!edge(u, v, g).second)
                boost::tie(tuples::ignore, success) = add_edge(u, v, g);
            else
                success = false;

            if (success)
                num_edges++;
        }
    }

    mate_t mate(n);
    bool random_graph_result
        = checked_edmonds_maximum_cardinality_matching(g, mate);

    if (!random_graph_result)
    {
        std::cout << "TEST 1 FAILED!!!" << std::endl << std::endl;

        std::cout << "Graph has edges: ";
        typedef graph_traits< undirected_graph >::edge_iterator edge_iterator_t;
        edge_iterator_t ei, ei_end;
        for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
            std::cout << *ei << ", ";
        std::cout << std::endl;

        std::cout << "Matching is: ";
        vertex_iterator_t vi, vi_end;
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
            if (mate[*vi] != graph_traits< undirected_graph >::null_vertex()
                && *vi < mate[*vi])
                std::cout << "{" << *vi << "," << mate[*vi] << "}, ";
        std::cout << std::endl;
    }

    // Now remove an edge from the random_mate matching.
    vertex_iterator_t vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        if (mate[*vi] != graph_traits< undirected_graph >::null_vertex())
            break;

    mate[mate[*vi]] = graph_traits< undirected_graph >::null_vertex();
    mate[*vi] = graph_traits< undirected_graph >::null_vertex();

    //...and run the matching verifier - it should tell us that the matching
    // isn't a maximum matching.
    bool modified_random_verification_result
        = maximum_cardinality_matching_verifier< undirected_graph, mate_t,
            vertex_index_map_t >::verify_matching(g, mate,
            get(vertex_index, g));

    if (modified_random_verification_result)
    {
        std::cout << "TEST 2 FAILED!!!" << std::endl;
    }

    // find a greedy matching on the graph
    mate_t greedy_mate(n);
    greedy_matching< undirected_graph, mate_t >::find_matching(g, greedy_mate);

    if (matching_size(g, mate) > matching_size(g, greedy_mate)
        && maximum_cardinality_matching_verifier< undirected_graph, mate_t,
            vertex_index_map_t >::verify_matching(g, greedy_mate,
            get(vertex_index, g)))
        std::cout << "TEST 3 FAILED!!!" << std::endl;

    return 0;
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Mengali, G., and A.A. Quarta, Fondamenti di Meccanica del volo Spaziale.
 *      Noomen, R., Lambert targeter Excel file.
 *
 *    Notes
 *      DISCLAIMER: At the moment, the Lambert targeter only converges for about half of the cases.
 *      This is not evident from the tests below, but it was observed during simulations carried
 *      out by the author. The reason might very well be an erroneous definition of the starters.
 *
 *      The elliptical case was taken from Example 6.1, page 159-162 of ( Mengali, Quarta ). The
 *      hyperbolic case was taken from ( Noomen, R. ).
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

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

#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"

#include "Tudat/Astrodynamics/MissionSegments/lambertTargeterGooding.h"

namespace tudat
{
namespace unit_tests
{

using namespace unit_conversions;

//! Test the Gooding Lambert targeting algorithm code.
BOOST_AUTO_TEST_SUITE( test_lambert_targeter_gooding )

//! Test hyperbolic case.
BOOST_AUTO_TEST_CASE( testHyperbolicCase )
{
    // Expected test result in meters.
    // Hyperbolic test case (results from excel file [1]).
    const double expectedValueOfSemiMajorAxisHyperbola = -1270129.3602e3;
    const double expectedValueOfRadialVelocityAtDepartureHyperbola = -0.74546e3;
    const double expectedValueOfRadialVelocityAtArrivalHyperbola = 0.69321e3;
    const double expectedValueOfTransverseVelocityAtDepartureHyperbola = 0.15674e3;
    const double expectedValueOfTransverseVelocityAtArrivalHyperbola = 0.10450e3;
    const Eigen::Vector3d expectedInertialVelocityAtDeparture( -745.457, 156.743, 0.0 );
    const Eigen::Vector3d expectedInertialVelocityAtArrival( 104.495, -693.209, 0.0 );

    // Tolerances.
    const double toleranceSemiMajorAxisHyperbola = 1.0e-7;
    const double toleranceVelocity = 1.0e-4;

    // Time conversions.
    const double timeOfFlightInDaysHyperbola = 100.0;
    const double timeOfFlightHyperbola = convertJulianDaysToSeconds(
            timeOfFlightInDaysHyperbola );

    // Central body gravitational parameter.
    const double earthGravitationalParameter = 398600.4418e9;

    // The starting point is twice as far as L1 and L2, which is not really
    // realistic, but it is not about the case, but about the verification.
    const Eigen::Vector3d positionAtDepartureHyperbola( convertAstronomicalUnitsToMeters( 0.02 ),
                                                        0.0, 0.0 ),
            positionAtArrivalHyperbola( 0.0, convertAstronomicalUnitsToMeters( -0.03 ), 0.0 );

    // Hyperbolic orbit case.
    mission_segments::LambertTargeterGooding lambertTargeterHyperbola(
                positionAtDepartureHyperbola, positionAtArrivalHyperbola, timeOfFlightHyperbola,
                earthGravitationalParameter );

    // Create local vectors for position and velocity.
    const Eigen::Vector3d positionDepartureHyperbola = positionAtDepartureHyperbola;
    const Eigen::Vector3d velocityDepartureHyperbola =
            lambertTargeterHyperbola.getInertialVelocityAtDeparture( );

    // Test if the computed semi-major axis corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getSemiMajorAxis( ),
                                expectedValueOfSemiMajorAxisHyperbola,
                                toleranceSemiMajorAxisHyperbola );

    // Test if the computed velocity components corresponds to the expected value within the
    // specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getRadialVelocityAtDeparture( ),
                                expectedValueOfRadialVelocityAtDepartureHyperbola,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getRadialVelocityAtArrival( ),
                                expectedValueOfRadialVelocityAtArrivalHyperbola,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getTransverseVelocityAtDeparture( ),
                                expectedValueOfTransverseVelocityAtDepartureHyperbola,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getTransverseVelocityAtArrival( ),
                                expectedValueOfTransverseVelocityAtArrivalHyperbola,
                                toleranceVelocity );

    // Check that velocities match expected values within the defined tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.x( ),
                                lambertTargeterHyperbola.getInertialVelocityAtDeparture( ).x( ),
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.y( ),
                                lambertTargeterHyperbola.getInertialVelocityAtDeparture( ).y( ),
                                toleranceVelocity );
    BOOST_CHECK_SMALL( lambertTargeterHyperbola.getInertialVelocityAtDeparture( ).z( ),
                       toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.x( ),
                                lambertTargeterHyperbola.getInertialVelocityAtArrival( ).x( ),
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.y( ),
                                lambertTargeterHyperbola.getInertialVelocityAtArrival( ).y( ),
                                toleranceVelocity );
    BOOST_CHECK_SMALL( lambertTargeterHyperbola.getInertialVelocityAtArrival( ).z( ),
                       toleranceVelocity );

    // Test if the computed solution is anti-clockwise, if the z-component of the angular momentum
    // (h = r \times v) is positive.
    BOOST_CHECK_GT( positionDepartureHyperbola.cross( velocityDepartureHyperbola ).z( ),
                    std::numeric_limits< double >::epsilon( ) );
}

//! Test elliptical case.
BOOST_AUTO_TEST_CASE( testEllipticalCase )
{
    // Elliptical test case (results from example 6.1 page 159-162 [2]).
    // Set canonical units for Earth (see page 29 [2]).
    const double distanceUnit = 6.378136e6;
    const double timeUnit = 806.78;

    // Expected test result in meters.
    const double expectedValueOfSemiMajorAxisEllipse = 5.4214 * distanceUnit;
    const double expectedValueOfRadialVelocityAtDepartureEllipse = 2.73580e3;
    const double expectedValueOfRadialVelocityAtArrivalEllipse = 2.97503e3;
    const double expectedValueOfTransverseVelocityAtDepartureEllipse = 6.59430e3;
    const double expectedValueOfTransverseVelocityAtArrivalEllipse = 3.29715e3;
    const Eigen::Vector3d expectedInertialVelocityAtDeparture( 2735.8, 6594.3, 0.0 );
    const Eigen::Vector3d expectedInertialVelocityAtArrival( -1367.9, 4225.03, 0.0 );

    // Tolerance.
    const double toleranceSemiMajorAxisEllipse = 1.0e-3;
    const double toleranceVelocity = 1.0e-6;

    // Time conversions.
    const double timeOfFlightEllipse = 5.0 * timeUnit;

    // Central body gravitational parameter.
    const double earthGravitationalParameter = 398600.4418e9;

    const Eigen::Vector3d positionAtDepartureEllipse( 2.0 * distanceUnit, 0.0, 0.0 ),
            positionAtArrivalEllipse( 2.0 * distanceUnit, 2.0 * sqrt( 3.0 ) * distanceUnit, 0.0 );

    // Elliptical orbit case.
    mission_segments::LambertTargeterGooding lambertTargeterEllipse(
                positionAtDepartureEllipse, positionAtArrivalEllipse, timeOfFlightEllipse,
                earthGravitationalParameter );

    // Create local vectors for position and velocity.
    const Eigen::Vector3d positionDepartureEllipse = positionAtDepartureEllipse;
    const Eigen::Vector3d velocityDepartureEllipse =
            lambertTargeterEllipse.getInertialVelocityAtDeparture( );

    // Test if the computed semi-major axis corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getSemiMajorAxis( ),
                                expectedValueOfSemiMajorAxisEllipse,
                                toleranceSemiMajorAxisEllipse );

    // Test if the computed velocity components corresponds to the expected value within the
    // specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getRadialVelocityAtDeparture( ),
                                expectedValueOfRadialVelocityAtDepartureEllipse,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getRadialVelocityAtArrival( ),
                                expectedValueOfRadialVelocityAtArrivalEllipse,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getTransverseVelocityAtDeparture( ),
                                expectedValueOfTransverseVelocityAtDepartureEllipse,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getTransverseVelocityAtArrival( ),
                                expectedValueOfTransverseVelocityAtArrivalEllipse,
                                toleranceVelocity );

    // Check that velocities match expected values within the defined tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.x( ),
                                lambertTargeterEllipse.getInertialVelocityAtDeparture( ).x( ),
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.y( ),
                                lambertTargeterEllipse.getInertialVelocityAtDeparture( ).y( ),
                                toleranceVelocity );
    BOOST_CHECK_SMALL( lambertTargeterEllipse.getInertialVelocityAtDeparture( ).z( ),
                       toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.x( ),
                                lambertTargeterEllipse.getInertialVelocityAtArrival( ).x( ),
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.y( ),
                                lambertTargeterEllipse.getInertialVelocityAtArrival( ).y( ),
                                toleranceVelocity );
    BOOST_CHECK_SMALL( lambertTargeterEllipse.getInertialVelocityAtArrival( ).z( ),
                       toleranceVelocity );

    // Test if the computed solution is anti-clockwise, if the z-component of the angular momentum
    // (h = r \times v) is positive.
    BOOST_CHECK_GT( positionDepartureEllipse.cross( velocityDepartureEllipse ).z( ),
                    std::numeric_limits< double >::epsilon( ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#pragma once
#pragma ide diagnostic ignored "modernize-use-nodiscard"
#pragma ide diagnostic ignored "NotImplementedFunctions"
#pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection"

#include <map>

#include <Eigen/Geometry>
#include "transform3_with_covariance.hpp"

// Used for specializing the from/to methods on CameraInfo
namespace cv
{
  template<typename T, int M, int N>
  class Matx;

  template<typename T, int N>
  class Vec;
}

namespace fvlam
{
// ==============================================================================
// Camera class
// ==============================================================================

  class Camera
  {
  private:
    // The id of the camera
    std::uint64_t id_;

    std::string imager_frame_id_;

    // The pose of the camera in some world frame. Which frame is world depends on the context.
    Transform3WithCovariance t_map_camera_;

  public:
    Camera() = delete;

    Camera(std::uint64_t id, std::string imager_frame_id, Transform3WithCovariance t_map_camera) :
      id_(id), imager_frame_id_{std::move(imager_frame_id)}, t_map_camera_(std::move(t_map_camera))
    {}

    auto id() const
    { return id_; }

    const auto &t_map_camera() const
    { return t_map_camera_; }

    const auto &imager_frame_id() const
    { return imager_frame_id_; }

    template<class T>
    static Camera from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    bool equals(const Camera &other, double tol = 1.0e-9, bool check_relative_also = true) const;
  };

// ==============================================================================
// CameraInfo class
// ==============================================================================

// This should really be called an ImagerInfo structure. But in the case where there
// is only one imager calling it a CameraInfo is equivalent. Cases that have multiple
// imagers will have to remember that this structure is named incorrectly.
  class CameraInfo
  {
  public:
    using CameraMatrix = Eigen::Matrix3d;
    using DistCoeffs = Eigen::Matrix<double, 5, 1>; // k1, k2, p1, p2 [, k3 [, k4, k5, k6]]

  private:
    std::string imager_frame_id_;
    std::uint32_t width_;
    std::uint32_t height_;
    CameraMatrix camera_matrix_;
    DistCoeffs dist_coeffs_;
    Transform3 t_camera_imager_;

  public:
    CameraInfo() :
      imager_frame_id_{},
      width_{0}, height_{0},
      camera_matrix_{CameraMatrix::Identity()},
      dist_coeffs_{DistCoeffs::Zero()},
      t_camera_imager_{}
    {}

    CameraInfo(std::uint32_t width, std::uint32_t height, CameraMatrix camera_matrix, DistCoeffs dist_coeffs) :
      imager_frame_id_{},
      width_{width}, height_{height},
      camera_matrix_{std::move(camera_matrix)},
      dist_coeffs_{std::move(dist_coeffs)},
      t_camera_imager_{}
    {}

    CameraInfo(double fx, double fy, double s, double u0, double v0) :
      imager_frame_id_{},
      width_{std::uint32_t(std::ceil(u0 * 2))}, height_{std::uint32_t(std::ceil(v0 * 2))},
      camera_matrix_{(CameraMatrix() << fx, s, u0, 0.0, fy, v0, 0.0, 0.0, 1.0).finished()},
      dist_coeffs_{DistCoeffs::Zero()},
      t_camera_imager_{}
    {}

    CameraInfo(double fx, double fy, double s, double u0, double v0,
               double k1, double k2, double p1, double p2, double k3) :
      imager_frame_id_{},
      width_{std::uint32_t(std::ceil(u0 * 2))}, height_{std::uint32_t(std::ceil(v0 * 2))},
      camera_matrix_{(CameraMatrix() << fx, s, u0, 0.0, fy, v0, 0.0, 0.0, 1.0).finished()},
      dist_coeffs_{(DistCoeffs() << k1, k2, p1, p2, k3).finished()},
      t_camera_imager_{}
    {}

    CameraInfo(std::string imager_frame_id,
               std::uint32_t width, std::uint32_t height,
               CameraMatrix camera_matrix, DistCoeffs dist_coeffs,
               Transform3 t_camera_imager) :
      imager_frame_id_{std::move(imager_frame_id)},
      width_{width}, height_{height},
      camera_matrix_{std::move(camera_matrix)},
      dist_coeffs_{std::move(dist_coeffs)},
      t_camera_imager_{std::move(t_camera_imager)}
    {}

    CameraInfo(std::string imager_frame_id,
               const CameraInfo &base,
               Transform3 t_camera_imager) :
      imager_frame_id_{std::move(imager_frame_id)},
      width_{base.width_}, height_{base.height_},
      camera_matrix_{base.camera_matrix_},
      dist_coeffs_{base.dist_coeffs_},
      t_camera_imager_{std::move(t_camera_imager)}
    {}

    auto &imager_frame_id() const
    { return imager_frame_id_; }

    auto &width() const
    { return width_; }

    auto &height() const
    { return height_; }

    const auto &camera_matrix() const
    { return camera_matrix_; }

    const auto &dist_coeffs() const
    { return dist_coeffs_; }

    const auto &t_camera_imager() const
    { return t_camera_imager_; }

    template<class T>
    static CameraInfo from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    std::string to_string() const;

    bool equals(const CameraInfo &other, double tol = 1.0e-9, bool check_relative_also = true) const;
  };

// ==============================================================================
// CameraInfo class
// ==============================================================================

  class CameraInfoMap
  {
    std::map<std::string, CameraInfo> m_{};

  public:
    auto &m_mutable()
    { return m_; }

    auto &m() const
    { return m_; }

    auto size() const
    { return m_.size(); }

    auto empty() const
    { return m_.empty(); }

    auto &first() const
    {
      assert(!m_.empty());
      return m_.begin()->second;
    }

    template<class T>
    static CameraInfoMap from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    std::string to_string() const;

    bool equals(const CameraInfoMap &other, double tol = 1.0e-9, bool check_relative_also = true) const;
  };

// Used for specializing the from/to methods on CameraInfo
  using CvCameraCalibration = std::pair<cv::Matx<double, 3, 3>, cv::Vec<double, 5>>;
}


#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <cstring>
#include <vector>
#include <cmath>
#include <string>
#include <math.h>
#include <signal.h>
#include <Eigen/Geometry>
#include <fstream>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <nav_msgs/Odometry.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/io.h>
#include <pcl/io/obj_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/extract_indices.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/tf.h>

#include <icp_map_merging/cbshot.h>

std::string PATH = "/home/argsubt/ros_docker_ws/localization_comp/SDC Localization Competition Data/map/nuscenes_map.pcd";

int main(int argc, char** argv){
    ros::init(argc, argv, "load_map");

    ros::NodeHandle nh;
    ros::Publisher map_filtered_pub;
    pcl::PointCloud<pcl::PointXYZI>::Ptr icp_map;
    pcl::PointCloud<pcl::PointXYZI>::Ptr icp_map_voxel;

    sensor_msgs::PointCloud2 ros_cloud_msg;

    pcl::VoxelGrid<pcl::PointXYZI> sor;

    std::string filePath;
    std::string fileName;
    double leafSize;
    nh.getParam("/map_node/file_path", filePath);
    nh.getParam("/map_node/file_name", fileName);
    nh.getParam("/map_node/leaf_size", leafSize);

    PATH = filePath+"/"+fileName;

    map_filtered_pub = nh.advertise<sensor_msgs::PointCloud2> ("/map_filtered", 1, true);

    icp_map.reset(new pcl::PointCloud<pcl::PointXYZI>());
    icp_map_voxel.reset(new pcl::PointCloud<pcl::PointXYZI>());

    std::cerr << "Loading Map " << PATH << std::endl;
    pcl::io::loadPCDFile<pcl::PointXYZI>(PATH, *icp_map);
    std::cerr << "Loaded" << std::endl;

    std::cerr << "PointCloud before filtering: " << icp_map->width * icp_map->height
        << " data points (" << pcl::getFieldsList (*icp_map) << ")." << std::endl;
    sor.setInputCloud(icp_map);
    sor.setLeafSize(leafSize, leafSize, leafSize);
    sor.filter(*icp_map_voxel);
    std::cerr << "PointCloud after filtering: " << icp_map_voxel->width * icp_map_voxel->height
        << " data points (" << pcl::getFieldsList (*icp_map_voxel) << ")." << std::endl;

    toROSMsg(*icp_map_voxel, ros_cloud_msg);
    ros_cloud_msg.header.frame_id = "world";
    map_filtered_pub.publish(ros_cloud_msg);

    std::cerr << "Published" << std::endl;

    ros::spin();
}


//
//  Camera.hpp
//  KinFu
//
//  Created by Dave on 19/05/2016.
//  Copyright © 2016 Sindesso. All rights reserved.
//

#ifndef Camera_hpp
#define Camera_hpp

#include <Eigen/Dense>
#include <deque>
#include <cstdint>

class Camera {
private:
    // The intrinsic matrix
    Eigen::Matrix3f     m_k;

    // The inverse of K, precalculated
    Eigen::Matrix3f     m_k_inverse;

    // The pose of the camera in global coordinate space
    Eigen::Matrix4f     m_pose;

    // The inverse pose of the camera
    Eigen::Matrix4f     m_pose_inverse;

    /**
     * Common construction code
     */
    void init( );

public:

    /**
     * @return the default Kinect IR camera parmaeters
     */
    static Camera * default_depth_camera( ) {
        // Default kinect settings
        return new Camera{ 591.1f, 590.1f, 331.0f, 234.6f };
    }

    /**
     * Construct a camera with the given intrinsic parameters
     * @param focal_x The focal length in the x direction in pixels
     * @param focal_y The focal length in the y direction in pixels
     * @param centre_x The horizontal centre of the image in pixels
     * @param centre_y The vertical centre of the image in pixels
     */
    explicit Camera( const float focal_x, const float focal_y, const float centre_x, const float centre_y );

    /**
     * Construct a camera with the given intrinsic parameters as a 3x3 matrix of the form
     * fx  0   cx
     * 0   fy  cy
     * 0   0   1
     *
     * @param k The intrinsic paramters
     */
    Camera( const Eigen::Matrix3f & k );

    /**
     * Construct a camera given the image dimensions and field of view
     * @param image_width The width of the image in pixels
     * @param image_height The height of the image in pixels
     * @param fov_x The horizontal field of view in mm
     * @param fov_y The vertical field of view in mm
     */
    explicit Camera( const int image_width, const int image_height, const float fov_x, const float fov_y );

#pragma mark - K and Kinv
    /**
     * @return the intrinsic parameter matrix K
     */
    const Eigen::Matrix3f k() const;

    /**
     * @return the inverse of k
     */
    const Eigen::Matrix3f kinv() const;

#pragma mark - Pose
    /**
     * @return the pose of the camera as a 4x4 matrix
     */
    const Eigen::Matrix4f & pose( ) const;

    /**
    * @return the inverse pose of the camera as a 4x4 matrix
    */
    const Eigen::Matrix4f & inverse_pose( ) const;

    /**
     * @param The new pose of the camera as a 4x4 matrix
     */
    void set_pose( const Eigen::Matrix4f & pose );

    /**
     * Move the camera and set it's orientation based on
     * 7 float parameters (as provided by TUM groundtruth data)
     * @param vars 0, 1 and 2 are a translation
     * @param vars 3,4,5,6 are x,y,z, w components of a quaternion dewscribing the facing
     */
    void set_pose( float vars[7] );

    /**
     * Move the camera to the given global coordinates
     * @param world_coordinate The 3D world coordinate
     */
    void move_to( const Eigen::Vector3f & world_coordinate );

    /**
     * Move the camera to the given global coordinates
     * @param wx World X coordinate
     * @param wy World Y coordinate
     * @param wz World Z coordinate
     */
    void move_to( float wx, float wy, float wz );

    /**
     * Adjust the camera pose so that it faces the given point
     * assumes that 'up' is in the direction of the +ve Y axis
     * @param world_coordinate The 3D world coordinate
     */
    void look_at( const Eigen::Vector3f & world_coordinate );

    /**
     * Adjust the camera pose so that it faces the given point
     * assumes that 'up' is in the direction of the +ve Y axis
     * @param wx World X coordinate
     * @param wy World Y coordinate
     * @param wz World Z coordinate
     */
    void look_at( float wx, float wy, float wz );

    /**
     * @return the position of the camera as a vector
     */
    Eigen::Vector3f position(  ) const;

#pragma mark - Camera coordinate methods
    /**
     * Convert from pixel coordinates to camera image plane coordinates
     * @param image_coordinate The 2D coordinate in the image space
     * @return camera_coordinate The 2D coordinate in camera image plane
     */
    Eigen::Vector2f pixel_to_image_plane( const Eigen::Vector2i & image_coordinate ) const;

    /**
     * Convert from pixel coordinates to camera image plane coordinates
     * @param image_x The x coordinate in the image space
     * @param image_y The y coordinate in the image space
     * @return camera_coordinate The 2D coordinate in camera image plane
     */
    Eigen::Vector2f  pixel_to_image_plane( const uint16_t x, const uint16_t y ) const;

    /**
     * Convert from image plane to pixel coordinates
     * @param camera_coordinate The 2D coordinate in camera image plane
     * @return image_coordinate The 2D coordinate in the image space
     */
    Eigen::Vector2i image_plane_to_pixel( const Eigen::Vector2f & camera_coordinate ) const;

    /**
     * Convert the camera coordinate into world space
     * Multiply by pose
     * @param camera_coordinate The 3D pointin camera space
     * @return world_coordinate The 3D point in world space
     */
    Eigen::Vector3f camera_to_world( const Eigen::Vector3f & camera_coordinate ) const;

    /**
     * Convert the normal in world coords into camera coords
     * @param world_normal The 3D normal in world space
     * @return camera_normal The 3D normal camera space
     */
    Eigen::Vector3f  world_to_camera_normal( const Eigen::Vector3f & world_normal ) const;

    /**
     * Convert the global coordinates into camera space
     * Multiply by pose.inverse()
     * @param world_coordinate The 3D point in world space
     * @return camera_coordinate The 3D pointin camera space
     */
    Eigen::Vector3f world_to_camera( const Eigen::Vector3f & world_coordinate ) const;

    /**
     * Convert global coordinates into pixel coordinates
     * Multiply by pose.inverse(), then K
     * @param world_coordinate The 3D point in world space
     * @param pixel_coordinate The 2D point in pixel space
     */
    Eigen::Vector2i world_to_pixel( const Eigen::Vector3f & world_coordinate ) const;

#pragma mark - Depth map methods
    /**
     * Convert from a depth image to 3D camera space coordinates
     * @param depth_image A width x height array of uint16_t depth values
     * @param vertices A width x height array of Vector3f representing the vertices in the depth image in camera space
     * @param normals A width x height array of Vector3f representing the vertices in the depth image in camera space
     */
    void depth_image_to_vertices_and_normals(const uint16_t * depth_image, const uint32_t width, const uint32_t height,
            Eigen::Matrix<float, 3, Eigen::Dynamic>& vertices,
            Eigen::Matrix<float, 3, Eigen::Dynamic>& normals ) const;
};

#endif /* Camera_hpp */


/* Author: Abner Salgado, Texas A&M University 2009               */

/*    $Id: step-35.cc 28376 2013-02-13 15:19:38Z heister $       */
/*                                                                */
/*    Copyright (C) 2009-2012 by 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.                        */

// @sect3{Include files}

// We start by including all the necessary deal.II header files and some C++
// related ones. Each one of them has been discussed in previous tutorial
// programs, so we will not get into details here.
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/point.h>
#include <deal.II/base/function.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/multithread_info.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/base/parallel.h>
#include <deal.II/base/utilities.h>
#include <deal.II/base/conditional_ostream.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/sparse_ilu.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/lac/constraint_matrix.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/tria_boundary_lib.h>
#include <deal.II/grid/grid_in.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/dofs/dof_renumbering.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_tools.h>
#include <deal.II/fe/fe_system.h>

#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/data_out.h>

#include <fstream>
#include <cmath>
#include <iostream>

// Finally this is as in all previous programs:
namespace Step35
{
  using namespace dealii;

  // @sect3{Run time parameters}
  //
  // Since our method has several parameters that can be fine-tuned we put
  // them into an external file, so that they can be determined at run-time.
  //
  // This includes, in particular, the formulation of the equation for the
  // auxiliary variable $\phi$, for which we declare an enum.
  // Next, we declare a class that is going to read and store all the
  // parameters that our program needs to run.
  namespace RunTimeParameters
  {
    enum MethodFormulation
    {
      METHOD_STANDARD,
      METHOD_ROTATIONAL
    };

    class Data_Storage
    {
    public:
      Data_Storage();
      ~Data_Storage();
      void read_data (const char *filename);
      MethodFormulation form;
      double initial_time,
             final_time,
             Reynolds;
      double dt;
      unsigned int n_global_refines,
               pressure_degree;
      unsigned int vel_max_iterations,
               vel_Krylov_size,
               vel_off_diagonals,
               vel_update_prec;
      double vel_eps,
             vel_diag_strength;
      bool verbose;
      unsigned int output_interval;
    protected:
      ParameterHandler prm;
    };

    // In the constructor of this class we declare all the parameters. The
    // details of how this works have been discussed elsewhere, for example in
    // step-19 and step-29.
    Data_Storage::Data_Storage()
    {
      prm.declare_entry ("Method_Form", "rotational",
                         Patterns::Selection ("rotational|standard"),
                         " Used to select the type of method that we are going "
                         "to use. ");
      prm.enter_subsection ("Physical data");
      {
        prm.declare_entry ("initial_time", "0.",
                           Patterns::Double (0.),
                           " The initial time of the simulation. ");
        prm.declare_entry ("final_time", "1.",
                           Patterns::Double (0.),
                           " The final time of the simulation. ");
        prm.declare_entry ("Reynolds", "1.",
                           Patterns::Double (0.),
                           " The Reynolds number. ");
      }
      prm.leave_subsection();

      prm.enter_subsection ("Time step data");
      {
        prm.declare_entry ("dt", "5e-4",
                           Patterns::Double (0.),
                           " The time step size. ");
      }
      prm.leave_subsection();

      prm.enter_subsection ("Space discretization");
      {
        prm.declare_entry ("n_of_refines", "0",
                           Patterns::Integer (0, 15),
                           " The number of global refines we do on the mesh. ");
        prm.declare_entry ("pressure_fe_degree", "1",
                           Patterns::Integer (1, 5),
                           " The polynomial degree for the pressure space. ");
      }
      prm.leave_subsection();

      prm.enter_subsection ("Data solve velocity");
      {
        prm.declare_entry ("max_iterations", "1000",
                           Patterns::Integer (1, 1000),
                           " The maximal number of iterations GMRES must make. ");
        prm.declare_entry ("eps", "1e-12",
                           Patterns::Double (0.),
                           " The stopping criterion. ");
        prm.declare_entry ("Krylov_size", "30",
                           Patterns::Integer(1),
                           " The size of the Krylov subspace to be used. ");
        prm.declare_entry ("off_diagonals", "60",
                           Patterns::Integer(0),
                           " The number of off-diagonal elements ILU must "
                           "compute. ");
        prm.declare_entry ("diag_strength", "0.01",
                           Patterns::Double (0.),
                           " Diagonal strengthening coefficient. ");
        prm.declare_entry ("update_prec", "15",
                           Patterns::Integer(1),
                           " This number indicates how often we need to "
                           "update the preconditioner");
      }
      prm.leave_subsection();

      prm.declare_entry ("verbose", "true",
                         Patterns::Bool(),
                         " This indicates whether the output of the solution "
                         "process should be verbose. ");

      prm.declare_entry ("output_interval", "1",
                         Patterns::Integer(1),
                         " This indicates between how many time steps we print "
                         "the solution. ");
    }

    Data_Storage::~Data_Storage()
    {}

    void Data_Storage::read_data (const char *filename)
    {
      std::ifstream file (filename);
      AssertThrow (file, ExcFileNotOpen (filename));

      prm.read_input (file);

      if (prm.get ("Method_Form") == std::string ("rotational"))
        form = METHOD_ROTATIONAL;
      else
        form = METHOD_STANDARD;

      prm.enter_subsection ("Physical data");
      {
        initial_time = prm.get_double ("initial_time");
        final_time   = prm.get_double ("final_time");
        Reynolds     = prm.get_double ("Reynolds");
      }
      prm.leave_subsection();

      prm.enter_subsection ("Time step data");
      {
        dt = prm.get_double ("dt");
      }
      prm.leave_subsection();

      prm.enter_subsection ("Space discretization");
      {
        n_global_refines = prm.get_integer ("n_of_refines");
        pressure_degree     = prm.get_integer ("pressure_fe_degree");
      }
      prm.leave_subsection();

      prm.enter_subsection ("Data solve velocity");
      {
        vel_max_iterations = prm.get_integer ("max_iterations");
        vel_eps            = prm.get_double ("eps");
        vel_Krylov_size    = prm.get_integer ("Krylov_size");
        vel_off_diagonals  = prm.get_integer ("off_diagonals");
        vel_diag_strength  = prm.get_double ("diag_strength");
        vel_update_prec    = prm.get_integer ("update_prec");
      }
      prm.leave_subsection();

      verbose = prm.get_bool ("verbose");

      output_interval = prm.get_integer ("output_interval");
    }
  }

  // @sect3{Equation data}

  // In the next namespace, we declare the initial and boundary conditions:
  namespace EquationData
  {
    // As we have chosen a completely decoupled formulation, we will not take
    // advantage of deal.II's capabilities to handle vector valued
    // problems. We do, however, want to use an interface for the equation
    // data that is somehow dimension independent. To be able to do that, our
    // functions should be able to know on which spatial component we are
    // currently working, and we should be able to have a common interface to
    // do that. The following class is an attempt in that direction.
    template <int dim>
    class MultiComponentFunction: public Function<dim>
    {
    public:
      MultiComponentFunction (const double initial_time = 0.);
      void set_component (const unsigned int d);
    protected:
      unsigned int comp;
    };

    template <int dim>
    MultiComponentFunction<dim>::
    MultiComponentFunction (const double initial_time)
      :
      Function<dim> (1, initial_time), comp(0)
    {}

    template <int dim>
    void MultiComponentFunction<dim>::set_component(const unsigned int d)
    {
      Assert (d<dim, ExcIndexRange (d, 0, dim));
      comp = d;
    }

    // With this class defined, we declare classes that describe the boundary
    // conditions for velocity and pressure:
    template <int dim>
    class Velocity : public MultiComponentFunction<dim>
    {
    public:
      Velocity (const double initial_time = 0.0);

      virtual double value (const Point<dim> &p,
                            const unsigned int component = 0) const;

      virtual void value_list (const std::vector< Point<dim> > &points,
                               std::vector<double> &values,
                               const unsigned int component = 0) const;
    };

    template <int dim>
    Velocity<dim>::Velocity (const double initial_time)
      :
      MultiComponentFunction<dim> (initial_time)
    {}

    template <int dim>
    void Velocity<dim>::value_list (const std::vector<Point<dim> > &points,
                                    std::vector<double> &values,
                                    const unsigned int) const
    {
      const unsigned int n_points = points.size();
      Assert (values.size() == n_points,
              ExcDimensionMismatch (values.size(), n_points));
      for (unsigned int i=0; i<n_points; ++i)
        values[i] = Velocity<dim>::value (points[i]);
    }

    template <int dim>
    double Velocity<dim>::value (const Point<dim> &p,
                                 const unsigned int) const
    {
      if (this->comp == 0)
        {
          const double Um = 1.5;
          const double H  = 4.1;
          return 4.*Um*p(1)*(H - p(1))/(H*H);
        }
      else
        return 0.;
    }

    template <int dim>
    class Pressure: public Function<dim>
    {
    public:
      Pressure (const double initial_time = 0.0);

      virtual double value (const Point<dim> &p,
                            const unsigned int component = 0) const;

      virtual void value_list (const std::vector< Point<dim> > &points,
                               std::vector<double> &values,
                               const unsigned int component = 0) const;
    };

    template <int dim>
    Pressure<dim>::Pressure (const double initial_time)
      :
      Function<dim> (1, initial_time)
    {}

    template <int dim>
    double Pressure<dim>::value (const Point<dim> &p,
                                 const unsigned int) const
    {
      return 25.-p(0);
    }

    template <int dim>
    void Pressure<dim>::value_list (const std::vector<Point<dim> > &points,
                                    std::vector<double> &values,
                                    const unsigned int) const
    {
      const unsigned int n_points = points.size();
      Assert (values.size() == n_points, ExcDimensionMismatch (values.size(), n_points));
      for (unsigned int i=0; i<n_points; ++i)
        values[i] = Pressure<dim>::value (points[i]);
    }
  }

  // @sect3{The NavierStokesProjection class}

  // Now for the main class of the program. It implements the various versions
  // of the projection method for Navier-Stokes equations.  The names for all
  // the methods and member variables should be self-explanatory, taking into
  // account the implementation details given in the introduction.
  template <int dim>
  class NavierStokesProjection
  {
  public:
    NavierStokesProjection (const RunTimeParameters::Data_Storage &data);

    void run (const bool         verbose    = false,
              const unsigned int n_plots = 10);
  protected:
    RunTimeParameters::MethodFormulation type;

    const unsigned int deg;
    const double       dt;
    const double       t_0, T, Re;

    EquationData::Velocity<dim>       vel_exact;
    std::map<unsigned int, double>    boundary_values;
    std::vector<types::boundary_id> boundary_indicators;

    Triangulation<dim> triangulation;

    FE_Q<dim>          fe_velocity;
    FE_Q<dim>          fe_pressure;

    DoFHandler<dim>    dof_handler_velocity;
    DoFHandler<dim>    dof_handler_pressure;

    QGauss<dim>        quadrature_pressure;
    QGauss<dim>        quadrature_velocity;

    SparsityPattern    sparsity_pattern_velocity;
    SparsityPattern    sparsity_pattern_pressure;
    SparsityPattern    sparsity_pattern_pres_vel;

    SparseMatrix<double> vel_Laplace_plus_Mass;
    SparseMatrix<double> vel_it_matrix[dim];
    SparseMatrix<double> vel_Mass;
    SparseMatrix<double> vel_Laplace;
    SparseMatrix<double> vel_Advection;
    SparseMatrix<double> pres_Laplace;
    SparseMatrix<double> pres_Mass;
    SparseMatrix<double> pres_Diff[dim];
    SparseMatrix<double> pres_iterative;

    Vector<double> pres_n;
    Vector<double> pres_n_minus_1;
    Vector<double> phi_n;
    Vector<double> phi_n_minus_1;
    Vector<double> u_n[dim];
    Vector<double> u_n_minus_1[dim];
    Vector<double> u_star[dim];
    Vector<double> force[dim];
    Vector<double> v_tmp;
    Vector<double> pres_tmp;
    Vector<double> rot_u;

    SparseILU<double> prec_velocity[dim];
    SparseILU<double> prec_pres_Laplace;
    SparseDirectUMFPACK prec_mass;
    SparseDirectUMFPACK prec_vel_mass;

    DeclException2 (ExcInvalidTimeStep,
                    double, double,
                    << " The time step " << arg1 << " is out of range."
                    << std::endl
                    << " The permitted range is (0," << arg2 << "]");

    void create_triangulation_and_dofs (const unsigned int n_refines);

    void initialize();

    void interpolate_velocity ();

    void diffusion_step (const bool reinit_prec);

    void projection_step (const bool reinit_prec);

    void update_pressure (const bool reinit_prec);

  private:
    unsigned int vel_max_its;
    unsigned int vel_Krylov_size;
    unsigned int vel_off_diagonals;
    unsigned int vel_update_prec;
    double       vel_eps;
    double       vel_diag_strength;

    void initialize_velocity_matrices();

    void initialize_pressure_matrices();

    // The next few structures and functions are for doing various things in
    // parallel. They follow the scheme laid out in @ref threads, using the
    // WorkStream class. As explained there, this requires us to declare two
    // structures for each of the assemblers, a per-task data and a scratch
    // data structure. These are then handed over to functions that assemble
    // local contributions and that copy these local contributions to the
    // global objects.
    //
    // One of the things that are specific to this program is that we don't
    // just have a single DoFHandler object that represents both the
    // velocities and the pressure, but we use individual DoFHandler objects
    // for these two kinds of variables. We pay for this optimization when we
    // want to assemble terms that involve both variables, such as the
    // divergence of the velocity and the gradient of the pressure, times the
    // respective test functions. When doing so, we can't just anymore use a
    // single FEValues object, but rather we need two, and they need to be
    // initialized with cell iterators that point to the same cell in the
    // triangulation but different DoFHandlers.
    //
    // To do this in practice, we declare a "synchronous" iterator -- an
    // object that internally consists of several (in our case two) iterators,
    // and each time the synchronous iteration is moved up one step, each of
    // the iterators stored internally is moved up one step as well, thereby
    // always staying in sync. As it so happens, there is a deal.II class that
    // facilitates this sort of thing.
    typedef std_cxx1x::tuple< typename DoFHandler<dim>::active_cell_iterator,
            typename DoFHandler<dim>::active_cell_iterator
            > IteratorTuple;

    typedef SynchronousIterators<IteratorTuple> IteratorPair;

    void initialize_gradient_operator();

    struct InitGradPerTaskData
    {
      unsigned int              d;
      unsigned int              vel_dpc;
      unsigned int              pres_dpc;
      FullMatrix<double>        local_grad;
      std::vector<unsigned int> vel_local_dof_indices;
      std::vector<unsigned int> pres_local_dof_indices;

      InitGradPerTaskData (const unsigned int dd,
                           const unsigned int vdpc,
                           const unsigned int pdpc)
        :
        d(dd),
        vel_dpc (vdpc),
        pres_dpc (pdpc),
        local_grad (vdpc, pdpc),
        vel_local_dof_indices (vdpc),
        pres_local_dof_indices (pdpc)
      {}
    };

    struct InitGradScratchData
    {
      unsigned int  nqp;
      FEValues<dim> fe_val_vel;
      FEValues<dim> fe_val_pres;
      InitGradScratchData (const FE_Q<dim> &fe_v,
                           const FE_Q<dim> &fe_p,
                           const QGauss<dim> &quad,
                           const UpdateFlags flags_v,
                           const UpdateFlags flags_p)
        :
        nqp (quad.size()),
        fe_val_vel (fe_v, quad, flags_v),
        fe_val_pres (fe_p, quad, flags_p)
      {}
      InitGradScratchData (const InitGradScratchData &data)
        :
        nqp (data.nqp),
        fe_val_vel (data.fe_val_vel.get_fe(),
                    data.fe_val_vel.get_quadrature(),
                    data.fe_val_vel.get_update_flags()),
        fe_val_pres (data.fe_val_pres.get_fe(),
                     data.fe_val_pres.get_quadrature(),
                     data.fe_val_pres.get_update_flags())
      {}
    };

    void assemble_one_cell_of_gradient (const IteratorPair &SI,
                                        InitGradScratchData &scratch,
                                        InitGradPerTaskData &data);

    void copy_gradient_local_to_global (const InitGradPerTaskData &data);

    // The same general layout also applies to the following classes and
    // functions implementing the assembly of the advection term:
    void assemble_advection_term();

    struct AdvectionPerTaskData
    {
      FullMatrix<double>        local_advection;
      std::vector<unsigned int> local_dof_indices;
      AdvectionPerTaskData (const unsigned int dpc)
        :
        local_advection (dpc, dpc),
        local_dof_indices (dpc)
      {}
    };

    struct AdvectionScratchData
    {
      unsigned int                 nqp;
      unsigned int                 dpc;
      std::vector< Point<dim> >    u_star_local;
      std::vector< Tensor<1,dim> > grad_u_star;
      std::vector<double>          u_star_tmp;
      FEValues<dim>                fe_val;
      AdvectionScratchData (const FE_Q<dim> &fe,
                            const QGauss<dim> &quad,
                            const UpdateFlags flags)
        :
        nqp (quad.size()),
        dpc (fe.dofs_per_cell),
        u_star_local (nqp),
        grad_u_star (nqp),
        u_star_tmp (nqp),
        fe_val (fe, quad, flags)
      {}

      AdvectionScratchData (const AdvectionScratchData &data)
        :
        nqp (data.nqp),
        dpc (data.dpc),
        u_star_local (nqp),
        grad_u_star (nqp),
        u_star_tmp (nqp),
        fe_val (data.fe_val.get_fe(),
                data.fe_val.get_quadrature(),
                data.fe_val.get_update_flags())
      {}
    };

    void assemble_one_cell_of_advection (const typename DoFHandler<dim>::active_cell_iterator &cell,
                                         AdvectionScratchData &scratch,
                                         AdvectionPerTaskData &data);

    void copy_advection_local_to_global (const AdvectionPerTaskData &data);

    // The final few functions implement the diffusion solve as well as
    // postprocessing the output, including computing the curl of the
    // velocity:
    void diffusion_component_solve (const unsigned int d);

    void output_results (const unsigned int step);

    void assemble_vorticity (const bool reinit_prec);
  };

  // @sect4{ NavierStokesProjection::NavierStokesProjection }

  // In the constructor, we just read all the data from the
  // Data_Storage object that is passed as an argument, verify
  // that the data we read is reasonable and, finally, create the
  // triangulation and load the initial data.
  template <int dim>
  NavierStokesProjection<dim>::NavierStokesProjection(const RunTimeParameters::Data_Storage &data)
    :
    type (data.form),
    deg (data.pressure_degree),
    dt (data.dt),
    t_0 (data.initial_time),
    T (data.final_time),
    Re (data.Reynolds),
    vel_exact (data.initial_time),
    fe_velocity (deg+1),
    fe_pressure (deg),
    dof_handler_velocity (triangulation),
    dof_handler_pressure (triangulation),
    quadrature_pressure (deg+1),
    quadrature_velocity (deg+2),
    vel_max_its (data.vel_max_iterations),
    vel_Krylov_size (data.vel_Krylov_size),
    vel_off_diagonals (data.vel_off_diagonals),
    vel_update_prec (data.vel_update_prec),
    vel_eps (data.vel_eps),
    vel_diag_strength (data.vel_diag_strength)
  {
    if (deg < 1)
      std::cout << " WARNING: The chosen pair of finite element spaces is not stable."
                << std::endl
                << " The obtained results will be nonsense"
                << std::endl;

    AssertThrow (!  ( (dt <= 0.) || (dt > .5*T)), ExcInvalidTimeStep (dt, .5*T));

    create_triangulation_and_dofs (data.n_global_refines);
    initialize();
  }

  // @sect4{ NavierStokesProjection::create_triangulation_and_dofs }

  // The method that creates the triangulation and refines it the needed
  // number of times.  After creating the triangulation, it creates the mesh
  // dependent data, i.e. it distributes degrees of freedom and renumbers
  // them, and initializes the matrices and vectors that we will use.
  template <int dim>
  void
  NavierStokesProjection<dim>::
  create_triangulation_and_dofs (const unsigned int n_refines)
  {
    GridIn<dim> grid_in;
    grid_in.attach_triangulation (triangulation);

    {
      std::string filename = "nsbench2.inp";
      std::ifstream file (filename.c_str());
      Assert (file, ExcFileNotOpen (filename.c_str()));
      grid_in.read_ucd (file);
    }

    std::cout << "Number of refines = " << n_refines
              << std::endl;
    triangulation.refine_global (n_refines);
    std::cout << "Number of active cells: " << triangulation.n_active_cells()
              << std::endl;

    boundary_indicators = triangulation.get_boundary_indicators();

    dof_handler_velocity.distribute_dofs (fe_velocity);
    DoFRenumbering::boost::Cuthill_McKee (dof_handler_velocity);
    dof_handler_pressure.distribute_dofs (fe_pressure);
    DoFRenumbering::boost::Cuthill_McKee (dof_handler_pressure);

    initialize_velocity_matrices();
    initialize_pressure_matrices();
    initialize_gradient_operator();

    pres_n.reinit (dof_handler_pressure.n_dofs());
    pres_n_minus_1.reinit (dof_handler_pressure.n_dofs());
    phi_n.reinit (dof_handler_pressure.n_dofs());
    phi_n_minus_1.reinit (dof_handler_pressure.n_dofs());
    pres_tmp.reinit (dof_handler_pressure.n_dofs());
    for (unsigned int d=0; d<dim; ++d)
      {
        u_n[d].reinit (dof_handler_velocity.n_dofs());
        u_n_minus_1[d].reinit (dof_handler_velocity.n_dofs());
        u_star[d].reinit (dof_handler_velocity.n_dofs());
        force[d].reinit (dof_handler_velocity.n_dofs());
      }
    v_tmp.reinit (dof_handler_velocity.n_dofs());
    rot_u.reinit (dof_handler_velocity.n_dofs());

    std::cout << "dim (X_h) = " << (dof_handler_velocity.n_dofs()*dim)
              << std::endl
              << "dim (M_h) = " << dof_handler_pressure.n_dofs()
              << std::endl
              << "Re        = " << Re
              << std::endl
              << std::endl;
  }

  // @sect4{ NavierStokesProjection::initialize }

  // This method creates the constant matrices and loads the initial data
  template <int dim>
  void
  NavierStokesProjection<dim>::initialize()
  {
    vel_Laplace_plus_Mass = 0.;
    vel_Laplace_plus_Mass.add (1./Re, vel_Laplace);
    vel_Laplace_plus_Mass.add (1.5/dt, vel_Mass);

    EquationData::Pressure<dim> pres (t_0);
    VectorTools::interpolate (dof_handler_pressure, pres, pres_n_minus_1);
    pres.advance_time (dt);
    VectorTools::interpolate (dof_handler_pressure, pres, pres_n);
    phi_n = 0.;
    phi_n_minus_1 = 0.;
    for (unsigned int d=0; d<dim; ++d)
      {
        vel_exact.set_time (t_0);
        vel_exact.set_component(d);
        VectorTools::interpolate (dof_handler_velocity, ZeroFunction<dim>(), u_n_minus_1[d]);
        vel_exact.advance_time (dt);
        VectorTools::interpolate (dof_handler_velocity, ZeroFunction<dim>(), u_n[d]);
      }
  }

  // @sect4{ The NavierStokesProjection::initialize_*_matrices methods }

  // In this set of methods we initialize the sparsity patterns, the
  // constraints (if any) and assemble the matrices that do not depend on the
  // timestep dt. Note that for the Laplace and mass matrices, we
  // can use functions in the library that do this. Because the expensive
  // operations of this function -- creating the two matrices -- are entirely
  // independent, we could in principle mark them as tasks that can be worked
  // on in %parallel using the Threads::new_task functions. We won't do that
  // here since these functions internally already are parallelized, and in
  // particular because the current function is only called once per program
  // run and so does not incur a cost in each time step. The necessary
  // modifications would be quite straightforward, however.
  template <int dim>
  void
  NavierStokesProjection<dim>::initialize_velocity_matrices()
  {
    sparsity_pattern_velocity.reinit (dof_handler_velocity.n_dofs(),
                                      dof_handler_velocity.n_dofs(),
                                      dof_handler_velocity.max_couplings_between_dofs());
    DoFTools::make_sparsity_pattern (dof_handler_velocity,
                                     sparsity_pattern_velocity);
    sparsity_pattern_velocity.compress();

    vel_Laplace_plus_Mass.reinit (sparsity_pattern_velocity);
    for (unsigned int d=0; d<dim; ++d)
      vel_it_matrix[d].reinit (sparsity_pattern_velocity);
    vel_Mass.reinit (sparsity_pattern_velocity);
    vel_Laplace.reinit (sparsity_pattern_velocity);
    vel_Advection.reinit (sparsity_pattern_velocity);

    MatrixCreator::create_mass_matrix (dof_handler_velocity,
                                       quadrature_velocity,
                                       vel_Mass);
    MatrixCreator::create_laplace_matrix (dof_handler_velocity,
                                          quadrature_velocity,
                                          vel_Laplace);
  }

  // The initialization of the matrices that act on the pressure space is
  // similar to the ones that act on the velocity space.
  template <int dim>
  void
  NavierStokesProjection<dim>::initialize_pressure_matrices()
  {
    sparsity_pattern_pressure.reinit (dof_handler_pressure.n_dofs(), dof_handler_pressure.n_dofs(),
                                      dof_handler_pressure.max_couplings_between_dofs());
    DoFTools::make_sparsity_pattern (dof_handler_pressure, sparsity_pattern_pressure);

    sparsity_pattern_pressure.compress();

    pres_Laplace.reinit (sparsity_pattern_pressure);
    pres_iterative.reinit (sparsity_pattern_pressure);
    pres_Mass.reinit (sparsity_pattern_pressure);

    MatrixCreator::create_laplace_matrix (dof_handler_pressure,
                                          quadrature_pressure,
                                          pres_Laplace);
    MatrixCreator::create_mass_matrix (dof_handler_pressure,
                                       quadrature_pressure,
                                       pres_Mass);
  }

  // For the gradient operator, we start by initializing the sparsity pattern
  // and compressing it.  It is important to notice here that the gradient
  // operator acts from the pressure space into the velocity space, so we have
  // to deal with two different finite element spaces. To keep the loops
  // synchronized, we use the typedef's that we have defined
  // before, namely PairedIterators and
  // IteratorPair.
  template <int dim>
  void
  NavierStokesProjection<dim>::initialize_gradient_operator()
  {
    sparsity_pattern_pres_vel.reinit (dof_handler_velocity.n_dofs(),
                                      dof_handler_pressure.n_dofs(),
                                      dof_handler_velocity.max_couplings_between_dofs());
    DoFTools::make_sparsity_pattern (dof_handler_velocity,
                                     dof_handler_pressure,
                                     sparsity_pattern_pres_vel);
    sparsity_pattern_pres_vel.compress();

    InitGradPerTaskData per_task_data (0, fe_velocity.dofs_per_cell,
                                       fe_pressure.dofs_per_cell);
    InitGradScratchData scratch_data (fe_velocity,
                                      fe_pressure,
                                      quadrature_velocity,
                                      update_gradients | update_JxW_values,
                                      update_values);

    for (unsigned int d=0; d<dim; ++d)
      {
        pres_Diff[d].reinit (sparsity_pattern_pres_vel);
        per_task_data.d = d;
        WorkStream::run (IteratorPair (IteratorTuple (dof_handler_velocity.begin_active(),
                                                      dof_handler_pressure.begin_active()
                                                     )
                                      ),
                         IteratorPair (IteratorTuple (dof_handler_velocity.end(),
                                                      dof_handler_pressure.end()
                                                     )
                                      ),
                         *this,
                         &NavierStokesProjection<dim>::assemble_one_cell_of_gradient,
                         &NavierStokesProjection<dim>::copy_gradient_local_to_global,
                         scratch_data,
                         per_task_data
                        );
      }
  }

  template <int dim>
  void
  NavierStokesProjection<dim>::
  assemble_one_cell_of_gradient (const IteratorPair &SI,
                                 InitGradScratchData &scratch,
                                 InitGradPerTaskData &data)
  {
    scratch.fe_val_vel.reinit (std_cxx1x::get<0> (SI.iterators));
    scratch.fe_val_pres.reinit (std_cxx1x::get<1> (SI.iterators));

    std_cxx1x::get<0> (SI.iterators)->get_dof_indices (data.vel_local_dof_indices);
    std_cxx1x::get<1> (SI.iterators)->get_dof_indices (data.pres_local_dof_indices);

    data.local_grad = 0.;
    for (unsigned int q=0; q<scratch.nqp; ++q)
      {
        for (unsigned int i=0; i<data.vel_dpc; ++i)
          for (unsigned int j=0; j<data.pres_dpc; ++j)
            data.local_grad (i, j) += -scratch.fe_val_vel.JxW(q) *
                                      scratch.fe_val_vel.shape_grad (i, q)[data.d] *
                                      scratch.fe_val_pres.shape_value (j, q);
      }
  }

  template <int dim>
  void
  NavierStokesProjection<dim>::
  copy_gradient_local_to_global(const InitGradPerTaskData &data)
  {
    for (unsigned int i=0; i<data.vel_dpc; ++i)
      for (unsigned int j=0; j<data.pres_dpc; ++j)
        pres_Diff[data.d].add (data.vel_local_dof_indices[i], data.pres_local_dof_indices[j],
                               data.local_grad (i, j) );
  }

  // @sect4{ NavierStokesProjection::run }

  // This is the time marching function, which starting at t_0
  // advances in time using the projection method with time step
  // dt until T.
  //
  // Its second parameter, verbose indicates whether the function
  // should output information what it is doing at any given moment: for
  // example, it will say whether we are working on the diffusion, projection
  // substep; updating preconditioners etc. Rather than implementing this
  // output using code like
  // @code
  //   if (verbose) std::cout << "something";
  // @endcode
  // we use the ConditionalOStream class to do that for us. That
  // class takes an output stream and a condition that indicates whether the
  // things you pass to it should be passed through to the given output
  // stream, or should just be ignored. This way, above code simply becomes
  // @code
  //   verbose_cout << "something";
  // @endcode
  // and does the right thing in either case.
  template <int dim>
  void
  NavierStokesProjection<dim>::run (const bool verbose,
                                    const unsigned int output_interval)
  {
    ConditionalOStream verbose_cout (std::cout, verbose);

    const unsigned int n_steps =  static_cast<unsigned int>((T - t_0)/dt);
    vel_exact.set_time (2.*dt);
    output_results(1);
    for (unsigned int n = 2; n<=n_steps; ++n)
      {
        if (n % output_interval == 0)
          {
            verbose_cout << "Plotting Solution" << std::endl;
            output_results(n);
          }
        std::cout << "Step = " << n << " Time = " << (n*dt) << std::endl;
        verbose_cout << "  Interpolating the velocity " << std::endl;

        interpolate_velocity();
        verbose_cout << "  Diffusion Step" << std::endl;
        if (n % vel_update_prec == 0)
          verbose_cout << "    With reinitialization of the preconditioner"
                       << std::endl;
        diffusion_step ((n%vel_update_prec == 0) || (n == 2));
        verbose_cout << "  Projection Step" << std::endl;
        projection_step ( (n == 2));
        verbose_cout << "  Updating the Pressure" << std::endl;
        update_pressure ( (n == 2));
        vel_exact.advance_time(dt);
      }
    output_results (n_steps);
  }

  template <int dim>
  void
  NavierStokesProjection<dim>::interpolate_velocity()
  {
    for (unsigned int d=0; d<dim; ++d)
      u_star[d].equ (2., u_n[d], -1, u_n_minus_1[d]);
  }

  // @sect4{NavierStokesProjection::diffusion_step}

  // The implementation of a diffusion step. Note that the expensive operation
  // is the diffusion solve at the end of the function, which we have to do
  // once for each velocity component. To accellerate things a bit, we allow
  // to do this in %parallel, using the Threads::new_task function which makes
  // sure that the dim solves are all taken care of and are
  // scheduled to available processors: if your machine has more than one
  // processor core and no other parts of this program are using resources
  // currently, then the diffusion solves will run in %parallel. On the other
  // hand, if your system has only one processor core then running things in
  // %parallel would be inefficient (since it leads, for example, to cache
  // congestion) and things will be executed sequentially.
  template <int dim>
  void
  NavierStokesProjection<dim>::diffusion_step (const bool reinit_prec)
  {
    pres_tmp.equ (-1., pres_n, -4./3., phi_n, 1./3., phi_n_minus_1);

    assemble_advection_term();

    for (unsigned int d=0; d<dim; ++d)
      {
        force[d] = 0.;
        v_tmp.equ (2./dt,u_n[d],-.5/dt,u_n_minus_1[d]);
        vel_Mass.vmult_add (force[d], v_tmp);

        pres_Diff[d].vmult_add (force[d], pres_tmp);
        u_n_minus_1[d] = u_n[d];

        vel_it_matrix[d].copy_from (vel_Laplace_plus_Mass);
        vel_it_matrix[d].add (1., vel_Advection);

        vel_exact.set_component(d);
        boundary_values.clear();
        for (std::vector<types::boundary_id>::const_iterator
             boundaries = boundary_indicators.begin();
             boundaries != boundary_indicators.end();
             ++boundaries)
          {
            switch (*boundaries)
              {
              case 1:
                VectorTools::
                interpolate_boundary_values (dof_handler_velocity,
                                             *boundaries,
                                             ZeroFunction<dim>(),
                                             boundary_values);
                break;
              case 2:
                VectorTools::
                interpolate_boundary_values (dof_handler_velocity,
                                             *boundaries,
                                             vel_exact,
                                             boundary_values);
                break;
              case 3:
                if (d != 0)
                  VectorTools::
                  interpolate_boundary_values (dof_handler_velocity,
                                               *boundaries,
                                               ZeroFunction<dim>(),
                                               boundary_values);
                break;
              case 4:
                VectorTools::
                interpolate_boundary_values (dof_handler_velocity,
                                             *boundaries,
                                             ZeroFunction<dim>(),
                                             boundary_values);
                break;
              default:
                Assert (false, ExcNotImplemented());
              }
          }
        MatrixTools::apply_boundary_values (boundary_values,
                                            vel_it_matrix[d],
                                            u_n[d],
                                            force[d]);
      }

    Threads::TaskGroup<void> tasks;
    for (unsigned int d=0; d<dim; ++d)
      {
        if (reinit_prec)
          prec_velocity[d].initialize (vel_it_matrix[d],
                                       SparseILU<double>::
                                       AdditionalData (vel_diag_strength,
                                                       vel_off_diagonals));
        tasks += Threads::new_task (&NavierStokesProjection<dim>::
                                    diffusion_component_solve,
                                    *this, d);
      }
    tasks.join_all();
  }

  template <int dim>
  void
  NavierStokesProjection<dim>::diffusion_component_solve (const unsigned int d)
  {
    SolverControl solver_control (vel_max_its, vel_eps*force[d].l2_norm());
    SolverGMRES<> gmres (solver_control,
                         SolverGMRES<>::AdditionalData (vel_Krylov_size));
    gmres.solve (vel_it_matrix[d], u_n[d], force[d], prec_velocity[d]);
  }

  // @sect4{ The NavierStokesProjection::assemble_advection_term method and related}

  // The following few functions deal with assembling the advection terms,
  // which is the part of the system matrix for the diffusion step that
  // changes at every time step. As mentioned above, we will run the assembly
  // loop over all cells in %parallel, using the WorkStream class and other
  // facilities as described in the documentation module on @ref threads.
  template <int dim>
  void
  NavierStokesProjection<dim>::assemble_advection_term()
  {
    vel_Advection = 0.;
    AdvectionPerTaskData data (fe_velocity.dofs_per_cell);
    AdvectionScratchData scratch (fe_velocity, quadrature_velocity,
                                  update_values |
                                  update_JxW_values |
                                  update_gradients);
    WorkStream::run (dof_handler_velocity.begin_active(),
                     dof_handler_velocity.end(), *this,
                     &NavierStokesProjection<dim>::assemble_one_cell_of_advection,
                     &NavierStokesProjection<dim>::copy_advection_local_to_global,
                     scratch,
                     data);
  }

  template <int dim>
  void
  NavierStokesProjection<dim>::
  assemble_one_cell_of_advection(const typename DoFHandler<dim>::active_cell_iterator &cell,
                                 AdvectionScratchData &scratch,
                                 AdvectionPerTaskData &data)
  {
    scratch.fe_val.reinit(cell);
    cell->get_dof_indices (data.local_dof_indices);
    for (unsigned int d=0; d<dim; ++d)
      {
        scratch.fe_val.get_function_values (u_star[d], scratch.u_star_tmp);
        for (unsigned int q=0; q<scratch.nqp; ++q)
          scratch.u_star_local[q](d) = scratch.u_star_tmp[q];
      }

    for (unsigned int d=0; d<dim; ++d)
      {
        scratch.fe_val.get_function_gradients (u_star[d], scratch.grad_u_star);
        for (unsigned int q=0; q<scratch.nqp; ++q)
          {
            if (d==0)
              scratch.u_star_tmp[q] = 0.;
            scratch.u_star_tmp[q] += scratch.grad_u_star[q][d];
          }
      }

    data.local_advection = 0.;
    for (unsigned int q=0; q<scratch.nqp; ++q)
      for (unsigned int i=0; i<scratch.dpc; ++i)
        for (unsigned int j=0; j<scratch.dpc; ++j)
          data.local_advection(i,j) += (scratch.u_star_local[q] *
                                        scratch.fe_val.shape_grad (j, q) *
                                        scratch.fe_val.shape_value (i, q)
                                        +
                                        0.5 *
                                        scratch.u_star_tmp[q] *
                                        scratch.fe_val.shape_value (i, q) *
                                        scratch.fe_val.shape_value (j, q))
                                       *
                                       scratch.fe_val.JxW(q) ;
  }

  template <int dim>
  void
  NavierStokesProjection<dim>::
  copy_advection_local_to_global(const AdvectionPerTaskData &data)
  {
    for (unsigned int i=0; i<fe_velocity.dofs_per_cell; ++i)
      for (unsigned int j=0; j<fe_velocity.dofs_per_cell; ++j)
        vel_Advection.add (data.local_dof_indices[i],
                           data.local_dof_indices[j],
                           data.local_advection(i,j));
  }

  // @sect4{NavierStokesProjection::projection_step}

  // This implements the projection step:
  template <int dim>
  void
  NavierStokesProjection<dim>::projection_step (const bool reinit_prec)
  {
    pres_iterative.copy_from (pres_Laplace);

    pres_tmp = 0.;
    for (unsigned d=0; d<dim; ++d)
      pres_Diff[d].Tvmult_add (pres_tmp, u_n[d]);

    phi_n_minus_1 = phi_n;

    static std::map<unsigned int, double> bval;
    if (reinit_prec)
      VectorTools::interpolate_boundary_values (dof_handler_pressure, 3,
                                                ZeroFunction<dim>(), bval);

    MatrixTools::apply_boundary_values (bval, pres_iterative, phi_n, pres_tmp);

    if (reinit_prec)
      prec_pres_Laplace.initialize(pres_iterative,
                                   SparseILU<double>::AdditionalData (vel_diag_strength,
                                       vel_off_diagonals) );

    SolverControl solvercontrol (vel_max_its, vel_eps*pres_tmp.l2_norm());
    SolverCG<> cg (solvercontrol);
    cg.solve (pres_iterative, phi_n, pres_tmp, prec_pres_Laplace);

    phi_n *= 1.5/dt;
  }

  // @sect4{ NavierStokesProjection::update_pressure }

  // This is the pressure update step of the projection method. It implements
  // the standard formulation of the method, that is @f[ p^{n+1} = p^n +
  // \phi^{n+1}, @f] or the rotational form, which is @f[ p^{n+1} = p^n +
  // \phi^{n+1} - \frac{1}{Re} \nabla\cdot u^{n+1}.  @f]
  template <int dim>
  void
  NavierStokesProjection<dim>::update_pressure (const bool reinit_prec)
  {
    pres_n_minus_1 = pres_n;
    switch (type)
      {
      case RunTimeParameters::METHOD_STANDARD:
        pres_n += phi_n;
        break;
      case RunTimeParameters::METHOD_ROTATIONAL:
        if (reinit_prec)
          prec_mass.initialize (pres_Mass);
        pres_n = pres_tmp;
        prec_mass.solve (pres_n);
        pres_n.sadd(1./Re, 1., pres_n_minus_1, 1., phi_n);
        break;
      default:
        Assert (false, ExcNotImplemented());
      };
  }

  // @sect4{ NavierStokesProjection::output_results }

  // This method plots the current solution. The main difficulty is that we
  // want to create a single output file that contains the data for all
  // velocity components, the pressure, and also the vorticity of the flow. On
  // the other hand, velocities and the pressure live on separate DoFHandler
  // objects, and so can't be written to the same file using a single DataOut
  // object. As a consequence, we have to work a bit harder to get the various
  // pieces of data into a single DoFHandler object, and then use that to
  // drive graphical output.
  //
  // We will not elaborate on this process here, but rather refer to step-31
  // and step-32, where a similar procedure is used (and is documented) to
  // create a joint DoFHandler object for all variables.
  //
  // Let us also note that we here compute the vorticity as a scalar quantity
  // in a separate function, using the $L^2$ projection of the quantity
  // $\text{curl} u$ onto the finite element space used for the components of
  // the velocity. In principle, however, we could also have computed as a
  // pointwise quantity from the velocity, and do so through the
  // DataPostprocessor mechanism discussed in step-29 and step-33.
  template <int dim>
  void NavierStokesProjection<dim>::output_results (const unsigned int step)
  {
    assemble_vorticity ( (step == 1));
    const FESystem<dim> joint_fe (fe_velocity, dim,
                                  fe_pressure, 1,
                                  fe_velocity, 1);
    DoFHandler<dim> joint_dof_handler (triangulation);
    joint_dof_handler.distribute_dofs (joint_fe);
    Assert (joint_dof_handler.n_dofs() ==
            ((dim + 1)*dof_handler_velocity.n_dofs() +
             dof_handler_pressure.n_dofs()),
            ExcInternalError());
    static Vector<double> joint_solution (joint_dof_handler.n_dofs());
    std::vector<unsigned int> loc_joint_dof_indices (joint_fe.dofs_per_cell),
        loc_vel_dof_indices (fe_velocity.dofs_per_cell),
        loc_pres_dof_indices (fe_pressure.dofs_per_cell);
    typename DoFHandler<dim>::active_cell_iterator
    joint_cell = joint_dof_handler.begin_active(),
    joint_endc = joint_dof_handler.end(),
    vel_cell   = dof_handler_velocity.begin_active(),
    pres_cell  = dof_handler_pressure.begin_active();
    for (; joint_cell != joint_endc; ++joint_cell, ++vel_cell, ++pres_cell)
      {
        joint_cell->get_dof_indices (loc_joint_dof_indices);
        vel_cell->get_dof_indices (loc_vel_dof_indices),
                 pres_cell->get_dof_indices (loc_pres_dof_indices);
        for (unsigned int i=0; i<joint_fe.dofs_per_cell; ++i)
          switch (joint_fe.system_to_base_index(i).first.first)
            {
            case 0:
              Assert (joint_fe.system_to_base_index(i).first.second < dim,
                      ExcInternalError());
              joint_solution (loc_joint_dof_indices[i]) =
                u_n[ joint_fe.system_to_base_index(i).first.second ]
                (loc_vel_dof_indices[ joint_fe.system_to_base_index(i).second ]);
              break;
            case 1:
              Assert (joint_fe.system_to_base_index(i).first.second == 0,
                      ExcInternalError());
              joint_solution (loc_joint_dof_indices[i]) =
                pres_n (loc_pres_dof_indices[ joint_fe.system_to_base_index(i).second ]);
              break;
            case 2:
              Assert (joint_fe.system_to_base_index(i).first.second == 0,
                      ExcInternalError());
              joint_solution (loc_joint_dof_indices[i]) =
                rot_u (loc_vel_dof_indices[ joint_fe.system_to_base_index(i).second ]);
              break;
            default:
              Assert (false, ExcInternalError());
            }
      }
    std::vector<std::string> joint_solution_names (dim, "v");
    joint_solution_names.push_back ("p");
    joint_solution_names.push_back ("rot_u");
    DataOut<dim> data_out;
    data_out.attach_dof_handler (joint_dof_handler);
    std::vector< DataComponentInterpretation::DataComponentInterpretation >
    component_interpretation (dim+2,
                              DataComponentInterpretation::component_is_part_of_vector);
    component_interpretation[dim]
      = DataComponentInterpretation::component_is_scalar;
    component_interpretation[dim+1]
      = DataComponentInterpretation::component_is_scalar;
    data_out.add_data_vector (joint_solution,
                              joint_solution_names,
                              DataOut<dim>::type_dof_data,
                              component_interpretation);
    data_out.build_patches (deg + 1);
    std::ofstream output (("solution-" +
                           Utilities::int_to_string (step, 5) +
                           ".vtk").c_str());
    data_out.write_vtk (output);
  }

  // Following is the helper function that computes the vorticity by
  // projecting the term $\text{curl} u$ onto the finite element space used
  // for the components of the velocity. The function is only called whenever
  // we generate graphical output, so not very often, and as a consequence we
  // didn't bother parallelizing it using the WorkStream concept as we do for
  // the other assembly functions. That should not be overly complicated,
  // however, if needed. Moreover, the implementation that we have here only
  // works for 2d, so we bail if that is not the case.
  template <int dim>
  void NavierStokesProjection<dim>::assemble_vorticity (const bool reinit_prec)
  {
    Assert (dim == 2, ExcNotImplemented());
    if (reinit_prec)
      prec_vel_mass.initialize (vel_Mass);

    FEValues<dim> fe_val_vel (fe_velocity, quadrature_velocity,
                              update_gradients |
                              update_JxW_values |
                              update_values);
    const unsigned int dpc = fe_velocity.dofs_per_cell,
                       nqp = quadrature_velocity.size();
    std::vector<unsigned int> ldi (dpc);
    Vector<double> loc_rot (dpc);

    std::vector< Tensor<1,dim> > grad_u1 (nqp), grad_u2 (nqp);
    rot_u = 0.;

    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler_velocity.begin_active(),
    end  = dof_handler_velocity.end();
    for (; cell != end; ++cell)
      {
        fe_val_vel.reinit (cell);
        cell->get_dof_indices (ldi);
        fe_val_vel.get_function_gradients (u_n[0], grad_u1);
        fe_val_vel.get_function_gradients (u_n[1], grad_u2);
        loc_rot = 0.;
        for (unsigned int q=0; q<nqp; ++q)
          for (unsigned int i=0; i<dpc; ++i)
            loc_rot(i) += (grad_u2[q][0] - grad_u1[q][1]) *
                          fe_val_vel.shape_value (i, q) *
                          fe_val_vel.JxW(q);

        for (unsigned int i=0; i<dpc; ++i)
          rot_u (ldi[i]) += loc_rot(i);
      }

    prec_vel_mass.solve (rot_u);
  }
}

// @sect3{ The main function }

// The main function looks very much like in all the other tutorial programs,
// so there is little to comment on here:
int main()
{
  try
    {
      using namespace dealii;
      using namespace Step35;

      RunTimeParameters::Data_Storage data;
      data.read_data ("parameter-file.prm");

      deallog.depth_console (data.verbose ? 2 : 0);

      NavierStokesProjection<2> test (data);
      test.run (data.verbose, data.output_interval);
    }
  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;
    }
  std::cout << "----------------------------------------------------"
            << std::endl
            << "Apparently everything went fine!"
            << std::endl
            << "Don't forget to brush your teeth :-)"
            << std::endl << std::endl;
  return 0;
}