#ifndef __TRANSFORM_UTILS__
#define __TRANSFORM_UTILS__

/**
 * Copyright (c) 2018, University Osnabrück
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University Osnabrück nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * TransformUtils.hpp
 *
 *  @date August 08, 2019
 *  @author Thomas Wiemann
 */

#include "lvr2/types/MatrixTypes.hpp"
#include "lvr2/io/Model.hpp"
#include "lvr2/io/CoordinateTransform.hpp"

#include <Eigen/Dense>

namespace lvr2
{

/**
 * @brief   Computes a Euler representation from the given transformation matrix
 *
 * @param  position     Will contain the position
 * @param  angles       Will contain the rotation angles in radians
 * @param  mat          The transformation matrix
 */
template<typename T>
void getPoseFromMatrix(BaseVector<T>& position, BaseVector<T>& angles, const Transform<T>& mat);

/**
 * @brief Transforms a registration matrix according to the given
 *        transformation matrix, i.e., applies @ref transform to @ref registration
 *
 * @param transform             A transformation matrix
 * @param registration          A matrix representing a registration (i.e. transformation) that
 * @return Transform<T>         The transformed registration matrix
 */
template<typename T>
Transform<T> transformRegistration(const Transform<T>& transform, const Transform<T>& registration);

/**
 * @brief   Transforms an slam6d transformation matrix into an Eigen 4x4 matrix.
 */
template<typename T>
Transform<T> buildTransformation(T* alignxf);

/**
 * @brief   Transforms (scale and switch coordinates) and reduces a model
 *          containing point cloud data using a modulo filter. Use this
 *          function the convert between different coordinate systems.
 *
 * @param   model       A model containing point cloud data
 * @param   modulo      The reduction factor for the modulo filter. Set to
 *                      1 to keep the original resolution.
 * @param   sx          Scaling factor in x direction
 * @param   sy          Scaling factor in y direction
 * @param   sz          Scaling factor in z direction
 * @param   xPos        Position of the x position in the input data, i.e,
 *                      "which array position has the x coordinate that is written
 *                      to the output data in the input data"
 * @param   yPos        Same as with xPos for y.
 * @param   zPos        Same as with xPos for z.
 */
template<typename T>
void transformAndReducePointCloud(ModelPtr model, int modulo,
        const T& sx, const T& sy, const T& sz,
        const unsigned char& xPos,
        const unsigned char& yPos,
        const unsigned char& zPos);

/**
 * @brief  Transforms (scale and switch coordinates) and reduces a model
 *         containing point cloud data using a modulo filter. Use this
 *         function the convert between different coordinate systems.
 *
 * @param model         A model containing point cloud data
 * @param modulo        The reduction factor for the modulo filter. Set to
 *                      1 to keep the original resolution.
 * @param c             The coordinate transformation applied to the \ref model
 */
template<typename T>
void transformAndReducePointCloud(ModelPtr& model, int modulo, const CoordinateTransform<T>& c);

/**
 * @brief   Transforms a model containing a point cloud according to the given
 *          transformation (usually from a .frames file)
 * @param   A model containing point cloud data.
 * @param   A transformation.
 */
template<typename T>
void transformPointCloud(ModelPtr model, const Transform<T>& transformation);

/**
 * @brief   Transforms the given source frame according to the given coordinate
 *          transform struct
 *
 * @param   frame           Source frame
 * @param   ct               Coordinate system transformation
 * @return                  The transformed frame
 */
template<typename T>
Transform<T> transformFrame(const Transform<T>& frame, const CoordinateTransform<T>& ct);

/**
 * @brief    Computes the inverse transformation from the given
 *          transformation matrix, which means if transform encodes
 *          the transformation A->B, the return will transform from
 *          B to A.
 *
 * @param transform             A transformation matrix
 * @return Transform<T>         The inverse transformation
 */
template<typename T>
Transform<T> inverseTransform(const Transform<T>& transform);

/**
 * @brief   Converts a Pose to a Matrix.
 *
 * @param position  The position of the Pose
 * @param rotation  The rotation in radians
 * @return          The Matrix representation of the Pose
 */
template<typename T>
Transform<T> poseToMatrix(const Vector3<T>& position, const Vector3<T>& rotation);

/**
 * @brief   Extracts the Pose from a Matrix
 *
 * @param pose      A Matrix representing a Pose
 * @param position  Output for the position of the Pose
 * @param rotation  Output for the rotation in radians
 */
template<typename T>
void matrixToPose(const Transform<T>& mat, Vector3<T>& position, Vector3<T>& rotation);

/**
 * @brief Converts a transformation matrix that is used in riegl coordinate system into
 *        a transformation matrix that is used in slam6d coordinate system.
 *
 * @param    in    The transformation matrix in riegl coordinate system
 *
 * @return The transformation matrix in slam6d coordinate system
 */
template <typename T>
static Transform<T> rieglToSLAM6DTransform(const Transform<T>& in)
{
       Transform<T> ret;

        ret(0) = in(5);
        ret(1) = -in(9);
        ret(2) = -in(1);
        ret(3) = -in(13);
        ret(4) = -in(6);
        ret(5) = in(10);
        ret(6) = in(2);
        ret(7) = in(14);
        ret(8) = -in(4);
        ret(9) = in(8);
        ret(10) = in(0);
        ret(11) = in(12);
        ret(12) = -100*in(7);
        ret(13) = 100*in(11);
        ret(14) = 100*in(3);
        ret(15) = in(15);

        return ret;
}

/**
 * @brief Converts a transformation matrix that is used in slam6d coordinate system into
 *        a transformation matrix that is used in riegl coordinate system.
 *
 * @param    in    The transformation matrix in slam6d coordinate system
 *
 * @return The transformation matrix in riegl coordinate system
 */
template <typename T>
static Transform<T> slam6dToRieglTransform(const Transform<T> &in)
{
    Transform<T> ret;

    ret(0) = in(10);
    ret(1) = -in(2);
    ret(2) = in(6);
    ret(3) = in(14)/100.0;
    ret(4) = -in(8);
    ret(5) = in(0);
    ret(6) = -in(4);
    ret(7) = -in(12)/100.0;
    ret(8) = in(9);
    ret(9) = -in(1);
    ret(10) = in(5);
    ret(11) = in(13)/100.0;
    ret(12) = in(11);
    ret(13) = -in(3);
    ret(14) = in(7);
    ret(15) = in(15);

    return ret;
}

template <typename T>
static Vector3<T> slam6DToRieglPoint(const Vector3<T> &in)
{
    return Vector3<T>(
        in.coeff(2)   / static_cast<T>(100.0),
        - in.coeff(0) / static_cast<T>(100.0),
        in.coeff(1)   / static_cast<T>(100.0)
    );
}

/////////////////////////////////
// Change Coodinate Systems
/////////////////////////////////

///////////////////////
// 1) CS -> LVR
//////////////

/**
 * @brief Slam6D to LVR coordinate change: Point
 *
 *  Slam6D
 *
 *     y  z
 *     | /
 *     |/___ x
 *
 *  - x: right
 *  - y: up
 *  - z: front
 *  - scale: cm
 *
 *  LVR / ROS
 *        z  x
 *        | /
 *   y ___|/
 *
 *  - x: front
 *  - y: left
 *  - z: up
 *  - scale: m
 *
 */
template <typename T>
static Vector3<T> slam6dToLvr(const Vector3<T>& in)
{
    return Vector3<T>(
        in.coeff(2) / static_cast<T>(100.0),
        - in.coeff(0) / static_cast<T>(100.0),
        in.coeff(1) / static_cast<T>(100.0)
    );
}

template<typename T>
static Rotation<T> slam6dToLvr(const Rotation<T>& in)
{
    Rotation<T> ret;

    // use coeff functions to access elements without range check
    // should be faster?

    // OLD CODE: why transpose?
    // ret.coeffRef(0, 0) =  in.coeff(2, 2);
    // ret.coeffRef(0, 1) = -in.coeff(0, 2);
    // ret.coeffRef(0, 2) =  in.coeff(1, 2);
    // ret.coeffRef(1, 0) = -in.coeff(2, 0);
    // ret.coeffRef(1, 1) =  in.coeff(0, 0);
    // ret.coeffRef(1, 2) = -in.coeff(1, 0);
    // ret.coeffRef(2, 0) =  in.coeff(2, 1);
    // ret.coeffRef(2, 1) = -in.coeff(0, 1);
    // ret.coeffRef(2, 2) =  in.coeff(1, 1);

    ret.coeffRef(0, 0) =  in.coeff(2, 2);
    ret.coeffRef(1, 0) = -in.coeff(0, 2);
    ret.coeffRef(2, 0) =  in.coeff(1, 2);
    ret.coeffRef(0, 1) = -in.coeff(2, 0);
    ret.coeffRef(1, 1) =  in.coeff(0, 0);
    ret.coeffRef(2, 1) = -in.coeff(1, 0);
    ret.coeffRef(0, 2) =  in.coeff(2, 1);
    ret.coeffRef(1, 2) = -in.coeff(0, 1);
    ret.coeffRef(2, 2) =  in.coeff(1, 1);

    return ret;
}

template <typename T>
static Transform<T> slam6dToLvr(const Transform<T> &in)
{
    Transform<T> ret;

    // Rotation
    const Rotation<T> R = in.template block<3,3>(0,0);
    ret.template block<3,3>(0,0) = slam6dToLvr(R);

    // Translation
    ret.coeffRef(0, 3) =  in.coeff(2, 3)/static_cast<T>(100.0);
    ret.coeffRef(1, 3) = -in.coeff(0, 3)/static_cast<T>(100.0);
    ret.coeffRef(2, 3) =  in.coeff(1, 3)/static_cast<T>(100.0);
    ret.coeffRef(3, 0) =  in.coeff(3, 2)/static_cast<T>(100.0);
    ret.coeffRef(3, 1) = -in.coeff(3, 0)/static_cast<T>(100.0);
    ret.coeffRef(3, 2) =  in.coeff(3, 1)/static_cast<T>(100.0);
    ret.coeffRef(3, 3) =  in.coeff(3, 3);

    return ret;
}

/**
 * @brief OpenCV to Lvr coordinate change: Point
 *
 *  OpenCV
 *
 *        z
 *       /
 *      /___ x
 *     |
 *     |
 *     y
 *
 *  - x: right
 *  - y: down
 *  - z: front
 *  - scale: m
 *
 *  LVR / ROS
 *        z  x
 *        | /
 *   y ___|/
 *
 *  - x: front
 *  - y: left
 *  - z: up
 *  - scale: m
 *
 */
template <typename T>
static Vector3<T> openCvToLvr(const Vector3<T>& in)
{
    return Vector3<T>(
        in.coeff(2),
        -in.coeff(0),
        -in.coeff(1)
    );
}

template <typename T>
static Rotation<T> openCvToLvr(const Rotation<T> &in)
{
    Rotation<T> ret;

    ret.coeffRef(0,0) =  in.coeff(2,2);
    ret.coeffRef(0,1) = -in.coeff(2,0);
    ret.coeffRef(0,2) = -in.coeff(2,1);
    ret.coeffRef(1,0) = -in.coeff(0,2);
    ret.coeffRef(1,1) =  in.coeff(0,0);
    ret.coeffRef(1,2) =  in.coeff(0,1);
    ret.coeffRef(2,0) = -in.coeff(1,2);
    ret.coeffRef(2,1) =  in.coeff(1,0);
    ret.coeffRef(2,2) =  in.coeff(1,1);

    return ret;
}

template <typename T>
static Transform<T> openCvToLvr(const Transform<T> &in)
{
    Transform<T> ret;

    const Rotation<T> R = in.template block<3,3>(0,0);
    ret.template block<3,3>(0,0) = openCvToLvr(R);

    ret.coeffRef(0,3) =  in.coeff(2,3);
    ret.coeffRef(1,3) = -in.coeff(0,3);
    ret.coeffRef(2,3) = -in.coeff(1,3);

    ret.coeffRef(3,0) =  in.coeff(3,2);
    ret.coeffRef(3,1) = -in.coeff(3,0);
    ret.coeffRef(3,2) = -in.coeff(3,1);

    ret.coeffRef(3,3) =  in.coeff(3,3);

    return ret;
}

////////////////
// 2) Lvr -> CS
////////////////

/**
 * @brief Lvr to Slam6D coordinate change: Point
 *
 *  Slam6D
 *
 *     y  z
 *     | /
 *     |/___ x
 *
 *  - x: right
 *  - y: up
 *  - z: front
 *  - scale: cm
 *
 *  LVR / ROS
 *        z  x
 *        | /
 *   y ___|/
 *
 *  - x: front
 *  - y: left
 *  - z: up
 *  - scale: m
 */
template <typename T>
static Vector3<T> lvrToSlam6d(const Vector3<T>& in)
{
    return Vector3<T>(
        -in.coeff(1) * static_cast<T>(100.0),
        in.coeff(2) * static_cast<T>(100.0),
        in.coeff(0) * static_cast<T>(100.0)
    );
}

template<typename T>
static Rotation<T> lvrToSlam6d(const Rotation<T>& in)
{
    Rotation<T> ret;

    // use coeff functions to access elements without range check
    // should be faster?

    // OLD CODE: why transpose?
    // ret.coeffRef(0, 0) =  in.coeff(1, 1); // in.coeff(1, 1)
    // ret.coeffRef(0, 1) = -in.coeff(2, 1); // in.coeff(1, 2)
    // ret.coeffRef(0, 2) = -in.coeff(0, 1); // in.coeff(1, 0)
    // ret.coeffRef(1, 0) = -in.coeff(1, 2);
    // ret.coeffRef(1, 1) =  in.coeff(2, 2);
    // ret.coeffRef(1, 2) =  in.coeff(0, 2);
    // ret.coeffRef(2, 0) = -in.coeff(1, 0);
    // ret.coeffRef(2, 1) =  in.coeff(2, 0);
    // ret.coeffRef(2, 2) =  in.coeff(0, 0);

    ret.coeffRef(0, 0) =  in.coeff(1, 1); // in.coeff(1, 1)
    ret.coeffRef(1, 0) = -in.coeff(2, 1); // in.coeff(1, 2)
    ret.coeffRef(2, 0) = -in.coeff(0, 1); // in.coeff(1, 0)
    ret.coeffRef(0, 1) = -in.coeff(1, 2);
    ret.coeffRef(1, 1) =  in.coeff(2, 2);
    ret.coeffRef(2, 1) =  in.coeff(0, 2);
    ret.coeffRef(0, 2) = -in.coeff(1, 0);
    ret.coeffRef(1, 2) =  in.coeff(2, 0);
    ret.coeffRef(2, 2) =  in.coeff(0, 0);

    return ret;
}

template <typename T>
static Transform<T> lvrToSlam6d(const Transform<T> &in)
{
    Transform<T> ret;

    // Rotation
    const Rotation<T> R = in.template block<3,3>(0,0);
    ret.template block<3,3>(0,0) = lvrToSlam6d(R);

    // Translation
    ret.coeffRef(0, 3) = -static_cast<T>(100.0) * in.coeff(1, 3);
    ret.coeffRef(1, 3) =  static_cast<T>(100.0) * in.coeff(2, 3);
    ret.coeffRef(2, 3) =  static_cast<T>(100.0) * in.coeff(0, 3);
    ret.coeffRef(3, 0) = -static_cast<T>(100.0) * in.coeff(3, 1);
    ret.coeffRef(3, 1) =  static_cast<T>(100.0) * in.coeff(3, 2);
    ret.coeffRef(3, 2) =  static_cast<T>(100.0) * in.coeff(3, 0);

    ret.coeffRef(3, 3) = in.coeff(3, 3);

    return ret;
}

/**
 * @brief Lvr to OpenCV coordinate change: Point
 *
 *  OpenCV
 *
 *        z
 *       /
 *      /___ x
 *     |
 *     |
 *     y
 *
 * - x: right
 * - y: down
 * - z: front
 * - scale: m
 *
 *  LVR / ROS
 *        z  x
 *        | /
 *   y ___|/
 *
 *  - x: front
 *  - y: left
 *  - z: up
 *  - scale: m
 *
 */
template <typename T>
static Vector3<T> lvrToOpenCv(const Vector3<T>& in)
{
    return Vector3<T>(
        -in.coeff(1),
        -in.coeff(2),
        in.coeff(0)
    );
}

template <typename T>
static Rotation<T> lvrToOpenCv(const Rotation<T>& in)
{
    Rotation<T> ret;

    ret.coeffRef(0,0) =  in.coeff(1,1);
    ret.coeffRef(0,1) =  in.coeff(1,2);
    ret.coeffRef(0,2) = -in.coeff(1,0);
    ret.coeffRef(1,0) =  in.coeff(2,1);
    ret.coeffRef(1,1) =  in.coeff(2,2);
    ret.coeffRef(1,2) = -in.coeff(2,0);
    ret.coeffRef(2,0) = -in.coeff(0,1);
    ret.coeffRef(2,1) = -in.coeff(0,2);
    ret.coeffRef(2,2) =  in.coeff(0,0);

    return ret;
}

template <typename T>
static Transform<T> lvrToOpenCv(const Transform<T> &in)
{
    Transform<T> ret;

    const Rotation<T> R = in.template block<3,3>(0,0);
    ret.template block<3,3>(0,0) = lvrToOpenCv(R);

    ret.coeffRef(0,3) = -in.coeff(1,3);
    ret.coeffRef(1,3) = -in.coeff(2,3);
    ret.coeffRef(2,3) =  in.coeff(0,3);

    ret.coeffRef(3,0) = -in.coeff(3,1);
    ret.coeffRef(3,1) = -in.coeff(3,2);
    ret.coeffRef(3,2) =  in.coeff(3,0);

    ret.coeffRef(3,3) =  in.coeff(3,3);

    return ret;
}

template<typename T>
void extrinsicsToEuler(Extrinsics<T> mat, T* pose)
{
    T *m = mat.data();
    if (pose != 0)
    {
        float _trX, _trY;
        if (m[0] > 0.0)
        {
            pose[4] = asin(m[8]);
        }
        else
        {
            pose[4] = (float)M_PI - asin(m[8]);
        }
        // rPosTheta[1] =  asin( m[8]);      // Calculate Y-axis angle

        float C = cos(pose[4]);
        if (fabs(C) > 0.005)
        {                     // Gimball lock?
            _trX = m[10] / C; // No, so get X-axis angle
            _trY = -m[9] / C;
            pose[3] = atan2(_trY, _trX);
            _trX = m[0] / C; // Get Z-axis angle
            _trY = -m[4] / C;
            pose[5] = atan2(_trY, _trX);
        }
        else
        {                  // Gimball lock has occurred
            pose[3] = 0.0; // Set X-axis angle to zero
            _trX = m[5];   //1          // And calculate Z-axis angle
            _trY = m[1];   //2
            pose[5] = atan2(_trY, _trX);
        }

        // cout << pose[3] << " " << pose[4] << " " << pose[5] << endl;

        pose[0] = m[12];
        pose[1] = m[13];
        pose[2] = m[14];
    }
}

template<typename T>
void eigenToEuler(Transform<T>& mat, T* pose)
{
    T *m = mat.data();
    if (pose != 0)
    {
        float _trX, _trY;
        if (m[0] > 0.0)
        {
            pose[4] = asin(m[8]);
        }
        else
        {
            pose[4] = (float)M_PI - asin(m[8]);
        }
        // rPosTheta[1] =  asin( m[8]);      // Calculate Y-axis angle

        float C = cos(pose[4]);
        if (fabs(C) > 0.005)
        {                     // Gimball lock?
            _trX = m[10] / C; // No, so get X-axis angle
            _trY = -m[9] / C;
            pose[3] = atan2(_trY, _trX);
            _trX = m[0] / C; // Get Z-axis angle
            _trY = -m[4] / C;
            pose[5] = atan2(_trY, _trX);
        }
        else
        {                  // Gimball lock has occurred
            pose[3] = 0.0; // Set X-axis angle to zero
            _trX = m[5];   //1          // And calculate Z-axis angle
            _trY = m[1];   //2
            pose[5] = atan2(_trY, _trX);
        }

        // cout << pose[3] << " " << pose[4] << " " << pose[5] << endl;

        pose[0] = m[12];
        pose[1] = m[13];
        pose[2] = m[14];
    }
}

} // namespace lvr2

#include "TransformUtils.tcc"

#endif


//=======================================================================
// Copyright (C) 2012 Flavio De Lorenzi (fdlorenzi@gmail.com)
// Copyright (C) 2013 Jakob Lykke Andersen, University of Southern Denmark
// (jlandersen@imada.sdu.dk)
//
// The algorithm implemented here is derived from original ideas by
// Pasquale Foggia and colaborators. For further information see
// e.g. Cordella et al. 2001, 2004.
//
// 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)
//=======================================================================

// Revision History:
//   8 April 2013: Fixed a typo in vf2_print_callback. (Flavio De Lorenzi)

#ifndef BOOST_VF2_SUB_GRAPH_ISO_HPP
#define BOOST_VF2_SUB_GRAPH_ISO_HPP

#include <iostream>
#include <iomanip>
#include <iterator>
#include <vector>
#include <utility>

#include <boost/assert.hpp>
#include <boost/concept/assert.hpp>
#include <boost/concept_check.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/mcgregor_common_subgraphs.hpp> // for always_equivalent
#include <boost/graph/named_function_params.hpp>
#include <boost/type_traits/has_less.hpp>
#include <boost/mpl/int.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/utility/enable_if.hpp>

#ifndef BOOST_GRAPH_ITERATION_MACROS_HPP
#define BOOST_ISO_INCLUDED_ITER_MACROS // local macro, see bottom of file
#include <boost/graph/iteration_macros.hpp>
#endif

namespace boost
{

// Default print_callback
template < typename Graph1, typename Graph2 > struct vf2_print_callback
{

    vf2_print_callback(const Graph1& graph1, const Graph2& graph2)
    : graph1_(graph1), graph2_(graph2)
    {
    }

    template < typename CorrespondenceMap1To2, typename CorrespondenceMap2To1 >
    bool operator()(CorrespondenceMap1To2 f, CorrespondenceMap2To1) const
    {

        // Print (sub)graph isomorphism map
        BGL_FORALL_VERTICES_T(v, graph1_, Graph1)
        std::cout << '(' << get(vertex_index_t(), graph1_, v) << ", "
                  << get(vertex_index_t(), graph2_, get(f, v)) << ") ";

        std::cout << std::endl;

        return true;
    }

private:
    const Graph1& graph1_;
    const Graph2& graph2_;
};

namespace detail
{

    // State associated with a single graph (graph_this)
    template < typename GraphThis, typename GraphOther, typename IndexMapThis,
        typename IndexMapOther >
    class base_state
    {

        typedef typename graph_traits< GraphThis >::vertex_descriptor
            vertex_this_type;
        typedef typename graph_traits< GraphOther >::vertex_descriptor
            vertex_other_type;

        typedef
            typename graph_traits< GraphThis >::vertices_size_type size_type;

        const GraphThis& graph_this_;
        const GraphOther& graph_other_;

        IndexMapThis index_map_this_;
        IndexMapOther index_map_other_;

        std::vector< vertex_other_type > core_vec_;
        typedef iterator_property_map<
            typename std::vector< vertex_other_type >::iterator, IndexMapThis,
            vertex_other_type, vertex_other_type& >
            core_map_type;
        core_map_type core_;

        std::vector< size_type > in_vec_, out_vec_;
        typedef iterator_property_map<
            typename std::vector< size_type >::iterator, IndexMapThis,
            size_type, size_type& >
            in_out_map_type;
        in_out_map_type in_, out_;

        size_type term_in_count_, term_out_count_, term_both_count_,
            core_count_;

        // Forbidden
        base_state(const base_state&);
        base_state& operator=(const base_state&);

    public:
        base_state(const GraphThis& graph_this, const GraphOther& graph_other,
            IndexMapThis index_map_this, IndexMapOther index_map_other)
        : graph_this_(graph_this)
        , graph_other_(graph_other)
        , index_map_this_(index_map_this)
        , index_map_other_(index_map_other)
        , core_vec_(num_vertices(graph_this_),
              graph_traits< GraphOther >::null_vertex())
        , core_(core_vec_.begin(), index_map_this_)
        , in_vec_(num_vertices(graph_this_), 0)
        , out_vec_(num_vertices(graph_this_), 0)
        , in_(in_vec_.begin(), index_map_this_)
        , out_(out_vec_.begin(), index_map_this_)
        , term_in_count_(0)
        , term_out_count_(0)
        , term_both_count_(0)
        , core_count_(0)
        {
        }

        // Adds a vertex pair to the state of graph graph_this
        void push(
            const vertex_this_type& v_this, const vertex_other_type& v_other)
        {

            ++core_count_;

            put(core_, v_this, v_other);

            if (!get(in_, v_this))
            {
                put(in_, v_this, core_count_);
                ++term_in_count_;
                if (get(out_, v_this))
                    ++term_both_count_;
            }

            if (!get(out_, v_this))
            {
                put(out_, v_this, core_count_);
                ++term_out_count_;
                if (get(in_, v_this))
                    ++term_both_count_;
            }

            BGL_FORALL_INEDGES_T(v_this, e, graph_this_, GraphThis)
            {
                vertex_this_type w = source(e, graph_this_);
                if (!get(in_, w))
                {
                    put(in_, w, core_count_);
                    ++term_in_count_;
                    if (get(out_, w))
                        ++term_both_count_;
                }
            }

            BGL_FORALL_OUTEDGES_T(v_this, e, graph_this_, GraphThis)
            {
                vertex_this_type w = target(e, graph_this_);
                if (!get(out_, w))
                {
                    put(out_, w, core_count_);
                    ++term_out_count_;
                    if (get(in_, w))
                        ++term_both_count_;
                }
            }
        }

        // Removes vertex pair from state of graph_this
        void pop(const vertex_this_type& v_this, const vertex_other_type&)
        {

            if (!core_count_)
                return;

            if (get(in_, v_this) == core_count_)
            {
                put(in_, v_this, 0);
                --term_in_count_;
                if (get(out_, v_this))
                    --term_both_count_;
            }

            BGL_FORALL_INEDGES_T(v_this, e, graph_this_, GraphThis)
            {
                vertex_this_type w = source(e, graph_this_);
                if (get(in_, w) == core_count_)
                {
                    put(in_, w, 0);
                    --term_in_count_;
                    if (get(out_, w))
                        --term_both_count_;
                }
            }

            if (get(out_, v_this) == core_count_)
            {
                put(out_, v_this, 0);
                --term_out_count_;
                if (get(in_, v_this))
                    --term_both_count_;
            }

            BGL_FORALL_OUTEDGES_T(v_this, e, graph_this_, GraphThis)
            {
                vertex_this_type w = target(e, graph_this_);
                if (get(out_, w) == core_count_)
                {
                    put(out_, w, 0);
                    --term_out_count_;
                    if (get(in_, w))
                        --term_both_count_;
                }
            }
            put(core_, v_this, graph_traits< GraphOther >::null_vertex());

            --core_count_;
        }

        // Returns true if the in-terminal set is not empty
        bool term_in() const { return core_count_ < term_in_count_; }

        // Returns true if vertex belongs to the in-terminal set
        bool term_in(const vertex_this_type& v) const
        {
            return (get(in_, v) > 0)
                && (get(core_, v) == graph_traits< GraphOther >::null_vertex());
        }

        // Returns true if the out-terminal set is not empty
        bool term_out() const { return core_count_ < term_out_count_; }

        // Returns true if vertex belongs to the out-terminal set
        bool term_out(const vertex_this_type& v) const
        {
            return (get(out_, v) > 0)
                && (get(core_, v) == graph_traits< GraphOther >::null_vertex());
        }

        // Returns true of both (in- and out-terminal) sets are not empty
        bool term_both() const { return core_count_ < term_both_count_; }

        // Returns true if vertex belongs to both (in- and out-terminal) sets
        bool term_both(const vertex_this_type& v) const
        {
            return (get(in_, v) > 0) && (get(out_, v) > 0)
                && (get(core_, v) == graph_traits< GraphOther >::null_vertex());
        }

        // Returns true if vertex belongs to the core map, i.e. it is in the
        // present mapping
        bool in_core(const vertex_this_type& v) const
        {
            return get(core_, v) != graph_traits< GraphOther >::null_vertex();
        }

        // Returns the number of vertices in the mapping
        size_type count() const { return core_count_; }

        // Returns the image (in graph_other) of vertex v (in graph_this)
        vertex_other_type core(const vertex_this_type& v) const
        {
            return get(core_, v);
        }

        // Returns the mapping
        core_map_type get_map() const { return core_; }

        // Returns the "time" (or depth) when vertex was added to the
        // in-terminal set
        size_type in_depth(const vertex_this_type& v) const
        {
            return get(in_, v);
        }

        // Returns the "time" (or depth) when vertex was added to the
        // out-terminal set
        size_type out_depth(const vertex_this_type& v) const
        {
            return get(out_, v);
        }

        // Returns the terminal set counts
        boost::tuple< size_type, size_type, size_type > term_set() const
        {
            return boost::make_tuple(
                term_in_count_, term_out_count_, term_both_count_);
        }
    };

    // Function object that checks whether a valid edge
    // exists. For multi-graphs matched edges are excluded
    template < typename Graph, typename Enable = void >
    struct equivalent_edge_exists
    {
        typedef
            typename boost::graph_traits< Graph >::edge_descriptor edge_type;

        BOOST_CONCEPT_ASSERT((LessThanComparable< edge_type >));

        template < typename EdgePredicate >
        bool operator()(typename graph_traits< Graph >::vertex_descriptor s,
            typename graph_traits< Graph >::vertex_descriptor t,
            EdgePredicate is_valid_edge, const Graph& g)
        {

            BGL_FORALL_OUTEDGES_T(s, e, g, Graph)
            {
                if ((target(e, g) == t) && is_valid_edge(e)
                    && (matched_edges_.find(e) == matched_edges_.end()))
                {
                    matched_edges_.insert(e);
                    return true;
                }
            }

            return false;
        }

    private:
        std::set< edge_type > matched_edges_;
    };

    template < typename Graph >
    struct equivalent_edge_exists< Graph,
        typename boost::disable_if< is_multigraph< Graph > >::type >
    {
        template < typename EdgePredicate >
        bool operator()(typename graph_traits< Graph >::vertex_descriptor s,
            typename graph_traits< Graph >::vertex_descriptor t,
            EdgePredicate is_valid_edge, const Graph& g)
        {

            typename graph_traits< Graph >::edge_descriptor e;
            bool found;
            boost::tie(e, found) = edge(s, t, g);
            if (!found)
                return false;
            else if (is_valid_edge(e))
                return true;

            return false;
        }
    };

    // Generates a predicate for edge e1 given  a binary predicate and a
    // fixed edge e2
    template < typename Graph1, typename Graph2,
        typename EdgeEquivalencePredicate >
    struct edge1_predicate
    {

        edge1_predicate(EdgeEquivalencePredicate edge_comp,
            typename graph_traits< Graph2 >::edge_descriptor e2)
        : edge_comp_(edge_comp), e2_(e2)
        {
        }

        bool operator()(typename graph_traits< Graph1 >::edge_descriptor e1)
        {
            return edge_comp_(e1, e2_);
        }

        EdgeEquivalencePredicate edge_comp_;
        typename graph_traits< Graph2 >::edge_descriptor e2_;
    };

    // Generates a predicate for edge e2 given given a binary predicate and a
    // fixed edge e1
    template < typename Graph1, typename Graph2,
        typename EdgeEquivalencePredicate >
    struct edge2_predicate
    {

        edge2_predicate(EdgeEquivalencePredicate edge_comp,
            typename graph_traits< Graph1 >::edge_descriptor e1)
        : edge_comp_(edge_comp), e1_(e1)
        {
        }

        bool operator()(typename graph_traits< Graph2 >::edge_descriptor e2)
        {
            return edge_comp_(e1_, e2);
        }

        EdgeEquivalencePredicate edge_comp_;
        typename graph_traits< Graph1 >::edge_descriptor e1_;
    };

    enum problem_selector
    {
        subgraph_mono,
        subgraph_iso,
        isomorphism
    };

    // The actual state associated with both graphs
    template < typename Graph1, typename Graph2, typename IndexMap1,
        typename IndexMap2, typename EdgeEquivalencePredicate,
        typename VertexEquivalencePredicate, typename SubGraphIsoMapCallback,
        problem_selector problem_selection >
    class state
    {

        typedef typename graph_traits< Graph1 >::vertex_descriptor vertex1_type;
        typedef typename graph_traits< Graph2 >::vertex_descriptor vertex2_type;

        typedef typename graph_traits< Graph1 >::edge_descriptor edge1_type;
        typedef typename graph_traits< Graph2 >::edge_descriptor edge2_type;

        typedef typename graph_traits< Graph1 >::vertices_size_type
            graph1_size_type;
        typedef typename graph_traits< Graph2 >::vertices_size_type
            graph2_size_type;

        const Graph1& graph1_;
        const Graph2& graph2_;

        IndexMap1 index_map1_;

        EdgeEquivalencePredicate edge_comp_;
        VertexEquivalencePredicate vertex_comp_;

        base_state< Graph1, Graph2, IndexMap1, IndexMap2 > state1_;
        base_state< Graph2, Graph1, IndexMap2, IndexMap1 > state2_;

        // Three helper functions used in Feasibility and Valid functions to
        // test terminal set counts when testing for:
        // - graph sub-graph monomorphism, or
        inline bool comp_term_sets(graph1_size_type a, graph2_size_type b,
            boost::mpl::int_< subgraph_mono >) const
        {
            return a <= b;
        }

        // - graph sub-graph isomorphism, or
        inline bool comp_term_sets(graph1_size_type a, graph2_size_type b,
            boost::mpl::int_< subgraph_iso >) const
        {
            return a <= b;
        }

        // - graph isomorphism
        inline bool comp_term_sets(graph1_size_type a, graph2_size_type b,
            boost::mpl::int_< isomorphism >) const
        {
            return a == b;
        }

        // Forbidden
        state(const state&);
        state& operator=(const state&);

    public:
        state(const Graph1& graph1, const Graph2& graph2, IndexMap1 index_map1,
            IndexMap2 index_map2, EdgeEquivalencePredicate edge_comp,
            VertexEquivalencePredicate vertex_comp)
        : graph1_(graph1)
        , graph2_(graph2)
        , index_map1_(index_map1)
        , edge_comp_(edge_comp)
        , vertex_comp_(vertex_comp)
        , state1_(graph1, graph2, index_map1, index_map2)
        , state2_(graph2, graph1, index_map2, index_map1)
        {
        }

        // Add vertex pair to the state
        void push(const vertex1_type& v, const vertex2_type& w)
        {
            state1_.push(v, w);
            state2_.push(w, v);
        }

        // Remove vertex pair from state
        void pop(const vertex1_type& v, const vertex2_type&)
        {
            vertex2_type w = state1_.core(v);
            state1_.pop(v, w);
            state2_.pop(w, v);
        }

        // Checks the feasibility of a new vertex pair
        bool feasible(const vertex1_type& v_new, const vertex2_type& w_new)
        {

            if (!vertex_comp_(v_new, w_new))
                return false;

            // graph1
            graph1_size_type term_in1_count = 0, term_out1_count = 0,
                             rest1_count = 0;

            {
                equivalent_edge_exists< Graph2 > edge2_exists;

                BGL_FORALL_INEDGES_T(v_new, e1, graph1_, Graph1)
                {
                    vertex1_type v = source(e1, graph1_);

                    if (state1_.in_core(v) || (v == v_new))
                    {
                        vertex2_type w = w_new;
                        if (v != v_new)
                            w = state1_.core(v);
                        if (!edge2_exists(w, w_new,
                                edge2_predicate< Graph1, Graph2,
                                    EdgeEquivalencePredicate >(edge_comp_, e1),
                                graph2_))
                            return false;
                    }
                    else
                    {
                        if (0 < state1_.in_depth(v))
                            ++term_in1_count;
                        if (0 < state1_.out_depth(v))
                            ++term_out1_count;
                        if ((state1_.in_depth(v) == 0)
                            && (state1_.out_depth(v) == 0))
                            ++rest1_count;
                    }
                }
            }

            {
                equivalent_edge_exists< Graph2 > edge2_exists;

                BGL_FORALL_OUTEDGES_T(v_new, e1, graph1_, Graph1)
                {
                    vertex1_type v = target(e1, graph1_);
                    if (state1_.in_core(v) || (v == v_new))
                    {
                        vertex2_type w = w_new;
                        if (v != v_new)
                            w = state1_.core(v);

                        if (!edge2_exists(w_new, w,
                                edge2_predicate< Graph1, Graph2,
                                    EdgeEquivalencePredicate >(edge_comp_, e1),
                                graph2_))
                            return false;
                    }
                    else
                    {
                        if (0 < state1_.in_depth(v))
                            ++term_in1_count;
                        if (0 < state1_.out_depth(v))
                            ++term_out1_count;
                        if ((state1_.in_depth(v) == 0)
                            && (state1_.out_depth(v) == 0))
                            ++rest1_count;
                    }
                }
            }

            // graph2
            graph2_size_type term_out2_count = 0, term_in2_count = 0,
                             rest2_count = 0;

            {
                equivalent_edge_exists< Graph1 > edge1_exists;

                BGL_FORALL_INEDGES_T(w_new, e2, graph2_, Graph2)
                {
                    vertex2_type w = source(e2, graph2_);
                    if (state2_.in_core(w) || (w == w_new))
                    {
                        if (problem_selection != subgraph_mono)
                        {
                            vertex1_type v = v_new;
                            if (w != w_new)
                                v = state2_.core(w);

                            if (!edge1_exists(v, v_new,
                                    edge1_predicate< Graph1, Graph2,
                                        EdgeEquivalencePredicate >(
                                        edge_comp_, e2),
                                    graph1_))
                                return false;
                        }
                    }
                    else
                    {
                        if (0 < state2_.in_depth(w))
                            ++term_in2_count;
                        if (0 < state2_.out_depth(w))
                            ++term_out2_count;
                        if ((state2_.in_depth(w) == 0)
                            && (state2_.out_depth(w) == 0))
                            ++rest2_count;
                    }
                }
            }

            {
                equivalent_edge_exists< Graph1 > edge1_exists;

                BGL_FORALL_OUTEDGES_T(w_new, e2, graph2_, Graph2)
                {
                    vertex2_type w = target(e2, graph2_);
                    if (state2_.in_core(w) || (w == w_new))
                    {
                        if (problem_selection != subgraph_mono)
                        {
                            vertex1_type v = v_new;
                            if (w != w_new)
                                v = state2_.core(w);

                            if (!edge1_exists(v_new, v,
                                    edge1_predicate< Graph1, Graph2,
                                        EdgeEquivalencePredicate >(
                                        edge_comp_, e2),
                                    graph1_))
                                return false;
                        }
                    }
                    else
                    {
                        if (0 < state2_.in_depth(w))
                            ++term_in2_count;
                        if (0 < state2_.out_depth(w))
                            ++term_out2_count;
                        if ((state2_.in_depth(w) == 0)
                            && (state2_.out_depth(w) == 0))
                            ++rest2_count;
                    }
                }
            }

            if (problem_selection != subgraph_mono)
            { // subgraph_iso and isomorphism
                return comp_term_sets(term_in1_count, term_in2_count,
                           boost::mpl::int_< problem_selection >())
                    && comp_term_sets(term_out1_count, term_out2_count,
                        boost::mpl::int_< problem_selection >())
                    && comp_term_sets(rest1_count, rest2_count,
                        boost::mpl::int_< problem_selection >());
            }
            else
            { // subgraph_mono
                return comp_term_sets(term_in1_count, term_in2_count,
                           boost::mpl::int_< problem_selection >())
                    && comp_term_sets(term_out1_count, term_out2_count,
                        boost::mpl::int_< problem_selection >())
                    && comp_term_sets(
                        term_in1_count + term_out1_count + rest1_count,
                        term_in2_count + term_out2_count + rest2_count,
                        boost::mpl::int_< problem_selection >());
            }
        }

        // Returns true if vertex v in graph1 is a possible candidate to
        // be added to the current state
        bool possible_candidate1(const vertex1_type& v) const
        {
            if (state1_.term_both() && state2_.term_both())
                return state1_.term_both(v);
            else if (state1_.term_out() && state2_.term_out())
                return state1_.term_out(v);
            else if (state1_.term_in() && state2_.term_in())
                return state1_.term_in(v);
            else
                return !state1_.in_core(v);
        }

        // Returns true if vertex w in graph2 is a possible candidate to
        // be added to the current state
        bool possible_candidate2(const vertex2_type& w) const
        {
            if (state1_.term_both() && state2_.term_both())
                return state2_.term_both(w);
            else if (state1_.term_out() && state2_.term_out())
                return state2_.term_out(w);
            else if (state1_.term_in() && state2_.term_in())
                return state2_.term_in(w);
            else
                return !state2_.in_core(w);
        }

        // Returns true if a mapping was found
        bool success() const
        {
            return state1_.count() == num_vertices(graph1_);
        }

        // Returns true if a state is valid
        bool valid() const
        {
            boost::tuple< graph1_size_type, graph1_size_type, graph1_size_type >
                term1;
            boost::tuple< graph2_size_type, graph2_size_type, graph2_size_type >
                term2;

            term1 = state1_.term_set();
            term2 = state2_.term_set();

            return comp_term_sets(boost::get< 0 >(term1),
                       boost::get< 0 >(term2),
                       boost::mpl::int_< problem_selection >())
                && comp_term_sets(boost::get< 1 >(term1),
                    boost::get< 1 >(term2),
                    boost::mpl::int_< problem_selection >())
                && comp_term_sets(boost::get< 2 >(term1),
                    boost::get< 2 >(term2),
                    boost::mpl::int_< problem_selection >());
        }

        // Calls the user_callback with a graph (sub)graph mapping
        bool call_back(SubGraphIsoMapCallback user_callback) const
        {
            return user_callback(state1_.get_map(), state2_.get_map());
        }
    };

    // Data structure to keep info used for back tracking during
    // matching process
    template < typename Graph1, typename Graph2, typename VertexOrder1 >
    struct vf2_match_continuation
    {
        typename VertexOrder1::const_iterator graph1_verts_iter;
        typename graph_traits< Graph2 >::vertex_iterator graph2_verts_iter;
    };

    // Non-recursive method that explores state space using a depth-first
    // search strategy.  At each depth possible pairs candidate are compute
    // and tested for feasibility to extend the mapping. If a complete
    // mapping is found, the mapping is output to user_callback in the form
    // of a correspondence map (graph1 to graph2). Returning false from the
    // user_callback will terminate the search. Function match will return
    // true if the entire search space was explored.
    template < typename Graph1, typename Graph2, typename IndexMap1,
        typename IndexMap2, typename VertexOrder1,
        typename EdgeEquivalencePredicate, typename VertexEquivalencePredicate,
        typename SubGraphIsoMapCallback, problem_selector problem_selection >
    bool match(const Graph1& graph1, const Graph2& graph2,
        SubGraphIsoMapCallback user_callback, const VertexOrder1& vertex_order1,
        state< Graph1, Graph2, IndexMap1, IndexMap2, EdgeEquivalencePredicate,
            VertexEquivalencePredicate, SubGraphIsoMapCallback,
            problem_selection >& s)
    {

        typename VertexOrder1::const_iterator graph1_verts_iter;

        typedef typename graph_traits< Graph2 >::vertex_iterator
            vertex2_iterator_type;
        vertex2_iterator_type graph2_verts_iter, graph2_verts_iter_end;

        typedef vf2_match_continuation< Graph1, Graph2, VertexOrder1 >
            match_continuation_type;
        std::vector< match_continuation_type > k;
        bool found_match = false;

    recur:
        if (s.success())
        {
            if (!s.call_back(user_callback))
                return true;
            found_match = true;

            goto back_track;
        }

        if (!s.valid())
            goto back_track;

        graph1_verts_iter = vertex_order1.begin();
        while (graph1_verts_iter != vertex_order1.end()
            && !s.possible_candidate1(*graph1_verts_iter))
        {
            ++graph1_verts_iter;
        }

        boost::tie(graph2_verts_iter, graph2_verts_iter_end) = vertices(graph2);
        while (graph2_verts_iter != graph2_verts_iter_end)
        {
            if (s.possible_candidate2(*graph2_verts_iter))
            {
                if (s.feasible(*graph1_verts_iter, *graph2_verts_iter))
                {
                    match_continuation_type kk;
                    kk.graph1_verts_iter = graph1_verts_iter;
                    kk.graph2_verts_iter = graph2_verts_iter;
                    k.push_back(kk);

                    s.push(*graph1_verts_iter, *graph2_verts_iter);
                    goto recur;
                }
            }
        graph2_loop:
            ++graph2_verts_iter;
        }

    back_track:
        if (k.empty())
            return found_match;

        const match_continuation_type kk = k.back();
        graph1_verts_iter = kk.graph1_verts_iter;
        graph2_verts_iter = kk.graph2_verts_iter;
        k.pop_back();

        s.pop(*graph1_verts_iter, *graph2_verts_iter);

        goto graph2_loop;
    }

    // Used to sort nodes by in/out degrees
    template < typename Graph > struct vertex_in_out_degree_cmp
    {
        typedef typename graph_traits< Graph >::vertex_descriptor vertex_type;

        vertex_in_out_degree_cmp(const Graph& graph) : graph_(graph) {}

        bool operator()(const vertex_type& v, const vertex_type& w) const
        {
            // lexicographical comparison
            return std::make_pair(in_degree(v, graph_), out_degree(v, graph_))
                < std::make_pair(in_degree(w, graph_), out_degree(w, graph_));
        }

        const Graph& graph_;
    };

    // Used to sort nodes by multiplicity of in/out degrees
    template < typename Graph, typename FrequencyMap >
    struct vertex_frequency_degree_cmp
    {
        typedef typename graph_traits< Graph >::vertex_descriptor vertex_type;

        vertex_frequency_degree_cmp(const Graph& graph, FrequencyMap freq)
        : graph_(graph), freq_(freq)
        {
        }

        bool operator()(const vertex_type& v, const vertex_type& w) const
        {
            // lexicographical comparison
            return std::make_pair(
                       freq_[v], in_degree(v, graph_) + out_degree(v, graph_))
                < std::make_pair(
                    freq_[w], in_degree(w, graph_) + out_degree(w, graph_));
        }

        const Graph& graph_;
        FrequencyMap freq_;
    };

    // Sorts vertices of a graph by multiplicity of in/out degrees
    template < typename Graph, typename IndexMap, typename VertexOrder >
    void sort_vertices(
        const Graph& graph, IndexMap index_map, VertexOrder& order)
    {
        typedef typename graph_traits< Graph >::vertices_size_type size_type;

        boost::range::sort(order, vertex_in_out_degree_cmp< Graph >(graph));

        std::vector< size_type > freq_vec(num_vertices(graph), 0);
        typedef iterator_property_map<
            typename std::vector< size_type >::iterator, IndexMap, size_type,
            size_type& >
            frequency_map_type;

        frequency_map_type freq
            = make_iterator_property_map(freq_vec.begin(), index_map);

        typedef typename VertexOrder::iterator order_iterator;

        for (order_iterator order_iter = order.begin();
             order_iter != order.end();)
        {
            size_type count = 0;
            for (order_iterator count_iter = order_iter;
                 (count_iter != order.end())
                 && (in_degree(*order_iter, graph)
                     == in_degree(*count_iter, graph))
                 && (out_degree(*order_iter, graph)
                     == out_degree(*count_iter, graph));
                 ++count_iter)
                ++count;

            for (size_type i = 0; i < count; ++i)
            {
                freq[*order_iter] = count;
                ++order_iter;
            }
        }

        boost::range::sort(order,
            vertex_frequency_degree_cmp< Graph, frequency_map_type >(
                graph, freq));
    }

    // Enumerates all graph sub-graph mono-/iso-morphism mappings between graphs
    // graph_small and graph_large. Continues until user_callback returns true
    // or the search space has been fully explored.
    template < problem_selector problem_selection, typename GraphSmall,
        typename GraphLarge, typename IndexMapSmall, typename IndexMapLarge,
        typename VertexOrderSmall, typename EdgeEquivalencePredicate,
        typename VertexEquivalencePredicate, typename SubGraphIsoMapCallback >
    bool vf2_subgraph_morphism(const GraphSmall& graph_small,
        const GraphLarge& graph_large, SubGraphIsoMapCallback user_callback,
        IndexMapSmall index_map_small, IndexMapLarge index_map_large,
        const VertexOrderSmall& vertex_order_small,
        EdgeEquivalencePredicate edge_comp,
        VertexEquivalencePredicate vertex_comp)
    {

        // Graph requirements
        BOOST_CONCEPT_ASSERT((BidirectionalGraphConcept< GraphSmall >));
        BOOST_CONCEPT_ASSERT((VertexListGraphConcept< GraphSmall >));
        BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< GraphSmall >));
        BOOST_CONCEPT_ASSERT((AdjacencyMatrixConcept< GraphSmall >));

        BOOST_CONCEPT_ASSERT((BidirectionalGraphConcept< GraphLarge >));
        BOOST_CONCEPT_ASSERT((VertexListGraphConcept< GraphLarge >));
        BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< GraphLarge >));
        BOOST_CONCEPT_ASSERT((AdjacencyMatrixConcept< GraphLarge >));

        typedef typename graph_traits< GraphSmall >::vertex_descriptor
            vertex_small_type;
        typedef typename graph_traits< GraphLarge >::vertex_descriptor
            vertex_large_type;

        typedef typename graph_traits< GraphSmall >::vertices_size_type
            size_type_small;
        typedef typename graph_traits< GraphLarge >::vertices_size_type
            size_type_large;

        // Property map requirements
        BOOST_CONCEPT_ASSERT(
            (ReadablePropertyMapConcept< IndexMapSmall, vertex_small_type >));
        typedef typename property_traits< IndexMapSmall >::value_type
            IndexMapSmallValue;
        BOOST_STATIC_ASSERT(
            (is_convertible< IndexMapSmallValue, size_type_small >::value));

        BOOST_CONCEPT_ASSERT(
            (ReadablePropertyMapConcept< IndexMapLarge, vertex_large_type >));
        typedef typename property_traits< IndexMapLarge >::value_type
            IndexMapLargeValue;
        BOOST_STATIC_ASSERT(
            (is_convertible< IndexMapLargeValue, size_type_large >::value));

        // Edge & vertex requirements
        typedef typename graph_traits< GraphSmall >::edge_descriptor
            edge_small_type;
        typedef typename graph_traits< GraphLarge >::edge_descriptor
            edge_large_type;

        BOOST_CONCEPT_ASSERT((BinaryPredicateConcept< EdgeEquivalencePredicate,
            edge_small_type, edge_large_type >));

        BOOST_CONCEPT_ASSERT(
            (BinaryPredicateConcept< VertexEquivalencePredicate,
                vertex_small_type, vertex_large_type >));

        // Vertex order requirements
        BOOST_CONCEPT_ASSERT((ContainerConcept< VertexOrderSmall >));
        typedef typename VertexOrderSmall::value_type order_value_type;
        BOOST_STATIC_ASSERT(
            (is_same< vertex_small_type, order_value_type >::value));
        BOOST_ASSERT(num_vertices(graph_small) == vertex_order_small.size());

        if (num_vertices(graph_small) > num_vertices(graph_large))
            return false;

        typename graph_traits< GraphSmall >::edges_size_type num_edges_small
            = num_edges(graph_small);
        typename graph_traits< GraphLarge >::edges_size_type num_edges_large
            = num_edges(graph_large);

        // Double the number of edges for undirected graphs: each edge counts as
        // in-edge and out-edge
        if (is_undirected(graph_small))
            num_edges_small *= 2;
        if (is_undirected(graph_large))
            num_edges_large *= 2;
        if (num_edges_small > num_edges_large)
            return false;

        detail::state< GraphSmall, GraphLarge, IndexMapSmall, IndexMapLarge,
            EdgeEquivalencePredicate, VertexEquivalencePredicate,
            SubGraphIsoMapCallback, problem_selection >
            s(graph_small, graph_large, index_map_small, index_map_large,
                edge_comp, vertex_comp);

        return detail::match(
            graph_small, graph_large, user_callback, vertex_order_small, s);
    }

} // namespace detail

// Returns vertex order (vertices sorted by multiplicity of in/out degrees)
template < typename Graph >
std::vector< typename graph_traits< Graph >::vertex_descriptor >
vertex_order_by_mult(const Graph& graph)
{

    std::vector< typename graph_traits< Graph >::vertex_descriptor >
        vertex_order;
    std::copy(vertices(graph).first, vertices(graph).second,
        std::back_inserter(vertex_order));

    detail::sort_vertices(graph, get(vertex_index, graph), vertex_order);
    return vertex_order;
}

// Enumerates all graph sub-graph monomorphism mappings between graphs
// graph_small and graph_large. Continues until user_callback returns true or
// the search space has been fully explored.
template < typename GraphSmall, typename GraphLarge, typename IndexMapSmall,
    typename IndexMapLarge, typename VertexOrderSmall,
    typename EdgeEquivalencePredicate, typename VertexEquivalencePredicate,
    typename SubGraphIsoMapCallback >
bool vf2_subgraph_mono(const GraphSmall& graph_small,
    const GraphLarge& graph_large, SubGraphIsoMapCallback user_callback,
    IndexMapSmall index_map_small, IndexMapLarge index_map_large,
    const VertexOrderSmall& vertex_order_small,
    EdgeEquivalencePredicate edge_comp, VertexEquivalencePredicate vertex_comp)
{
    return detail::vf2_subgraph_morphism< detail::subgraph_mono >(graph_small,
        graph_large, user_callback, index_map_small, index_map_large,
        vertex_order_small, edge_comp, vertex_comp);
}

// All default interface for vf2_subgraph_iso
template < typename GraphSmall, typename GraphLarge,
    typename SubGraphIsoMapCallback >
bool vf2_subgraph_mono(const GraphSmall& graph_small,
    const GraphLarge& graph_large, SubGraphIsoMapCallback user_callback)
{
    return vf2_subgraph_mono(graph_small, graph_large, user_callback,
        get(vertex_index, graph_small), get(vertex_index, graph_large),
        vertex_order_by_mult(graph_small), always_equivalent(),
        always_equivalent());
}

// Named parameter interface of vf2_subgraph_iso
template < typename GraphSmall, typename GraphLarge, typename VertexOrderSmall,
    typename SubGraphIsoMapCallback, typename Param, typename Tag,
    typename Rest >
bool vf2_subgraph_mono(const GraphSmall& graph_small,
    const GraphLarge& graph_large, SubGraphIsoMapCallback user_callback,
    const VertexOrderSmall& vertex_order_small,
    const bgl_named_params< Param, Tag, Rest >& params)
{
    return vf2_subgraph_mono(graph_small, graph_large, user_callback,
        choose_const_pmap(
            get_param(params, vertex_index1), graph_small, vertex_index),
        choose_const_pmap(
            get_param(params, vertex_index2), graph_large, vertex_index),
        vertex_order_small,
        choose_param(
            get_param(params, edges_equivalent_t()), always_equivalent()),
        choose_param(
            get_param(params, vertices_equivalent_t()), always_equivalent()));
}

// Enumerates all graph sub-graph isomorphism mappings between graphs
// graph_small and graph_large. Continues until user_callback returns true or
// the search space has been fully explored.
template < typename GraphSmall, typename GraphLarge, typename IndexMapSmall,
    typename IndexMapLarge, typename VertexOrderSmall,
    typename EdgeEquivalencePredicate, typename VertexEquivalencePredicate,
    typename SubGraphIsoMapCallback >
bool vf2_subgraph_iso(const GraphSmall& graph_small,
    const GraphLarge& graph_large, SubGraphIsoMapCallback user_callback,
    IndexMapSmall index_map_small, IndexMapLarge index_map_large,
    const VertexOrderSmall& vertex_order_small,
    EdgeEquivalencePredicate edge_comp, VertexEquivalencePredicate vertex_comp)
{
    return detail::vf2_subgraph_morphism< detail::subgraph_iso >(graph_small,
        graph_large, user_callback, index_map_small, index_map_large,
        vertex_order_small, edge_comp, vertex_comp);
}

// All default interface for vf2_subgraph_iso
template < typename GraphSmall, typename GraphLarge,
    typename SubGraphIsoMapCallback >
bool vf2_subgraph_iso(const GraphSmall& graph_small,
    const GraphLarge& graph_large, SubGraphIsoMapCallback user_callback)
{

    return vf2_subgraph_iso(graph_small, graph_large, user_callback,
        get(vertex_index, graph_small), get(vertex_index, graph_large),
        vertex_order_by_mult(graph_small), always_equivalent(),
        always_equivalent());
}

// Named parameter interface of vf2_subgraph_iso
template < typename GraphSmall, typename GraphLarge, typename VertexOrderSmall,
    typename SubGraphIsoMapCallback, typename Param, typename Tag,
    typename Rest >
bool vf2_subgraph_iso(const GraphSmall& graph_small,
    const GraphLarge& graph_large, SubGraphIsoMapCallback user_callback,
    const VertexOrderSmall& vertex_order_small,
    const bgl_named_params< Param, Tag, Rest >& params)
{

    return vf2_subgraph_iso(graph_small, graph_large, user_callback,
        choose_const_pmap(
            get_param(params, vertex_index1), graph_small, vertex_index),
        choose_const_pmap(
            get_param(params, vertex_index2), graph_large, vertex_index),
        vertex_order_small,
        choose_param(
            get_param(params, edges_equivalent_t()), always_equivalent()),
        choose_param(
            get_param(params, vertices_equivalent_t()), always_equivalent()));
}

// Enumerates all isomorphism mappings between graphs graph1_ and graph2_.
// Continues until user_callback returns true or the search space has been
// fully explored.
template < typename Graph1, typename Graph2, typename IndexMap1,
    typename IndexMap2, typename VertexOrder1,
    typename EdgeEquivalencePredicate, typename VertexEquivalencePredicate,
    typename GraphIsoMapCallback >
bool vf2_graph_iso(const Graph1& graph1, const Graph2& graph2,
    GraphIsoMapCallback user_callback, IndexMap1 index_map1,
    IndexMap2 index_map2, const VertexOrder1& vertex_order1,
    EdgeEquivalencePredicate edge_comp, VertexEquivalencePredicate vertex_comp)
{

    // Graph requirements
    BOOST_CONCEPT_ASSERT((BidirectionalGraphConcept< Graph1 >));
    BOOST_CONCEPT_ASSERT((VertexListGraphConcept< Graph1 >));
    BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< Graph1 >));
    BOOST_CONCEPT_ASSERT((AdjacencyMatrixConcept< Graph1 >));

    BOOST_CONCEPT_ASSERT((BidirectionalGraphConcept< Graph2 >));
    BOOST_CONCEPT_ASSERT((VertexListGraphConcept< Graph2 >));
    BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< Graph2 >));
    BOOST_CONCEPT_ASSERT((AdjacencyMatrixConcept< Graph2 >));

    typedef typename graph_traits< Graph1 >::vertex_descriptor vertex1_type;
    typedef typename graph_traits< Graph2 >::vertex_descriptor vertex2_type;

    typedef typename graph_traits< Graph1 >::vertices_size_type size_type1;
    typedef typename graph_traits< Graph2 >::vertices_size_type size_type2;

    // Property map requirements
    BOOST_CONCEPT_ASSERT(
        (ReadablePropertyMapConcept< IndexMap1, vertex1_type >));
    typedef typename property_traits< IndexMap1 >::value_type IndexMap1Value;
    BOOST_STATIC_ASSERT((is_convertible< IndexMap1Value, size_type1 >::value));

    BOOST_CONCEPT_ASSERT(
        (ReadablePropertyMapConcept< IndexMap2, vertex2_type >));
    typedef typename property_traits< IndexMap2 >::value_type IndexMap2Value;
    BOOST_STATIC_ASSERT((is_convertible< IndexMap2Value, size_type2 >::value));

    // Edge & vertex requirements
    typedef typename graph_traits< Graph1 >::edge_descriptor edge1_type;
    typedef typename graph_traits< Graph2 >::edge_descriptor edge2_type;

    BOOST_CONCEPT_ASSERT((BinaryPredicateConcept< EdgeEquivalencePredicate,
        edge1_type, edge2_type >));

    BOOST_CONCEPT_ASSERT((BinaryPredicateConcept< VertexEquivalencePredicate,
        vertex1_type, vertex2_type >));

    // Vertex order requirements
    BOOST_CONCEPT_ASSERT((ContainerConcept< VertexOrder1 >));
    typedef typename VertexOrder1::value_type order_value_type;
    BOOST_STATIC_ASSERT((is_same< vertex1_type, order_value_type >::value));
    BOOST_ASSERT(num_vertices(graph1) == vertex_order1.size());

    if (num_vertices(graph1) != num_vertices(graph2))
        return false;

    typename graph_traits< Graph1 >::edges_size_type num_edges1
        = num_edges(graph1);
    typename graph_traits< Graph2 >::edges_size_type num_edges2
        = num_edges(graph2);

    // Double the number of edges for undirected graphs: each edge counts as
    // in-edge and out-edge
    if (is_undirected(graph1))
        num_edges1 *= 2;
    if (is_undirected(graph2))
        num_edges2 *= 2;
    if (num_edges1 != num_edges2)
        return false;

    detail::state< Graph1, Graph2, IndexMap1, IndexMap2,
        EdgeEquivalencePredicate, VertexEquivalencePredicate,
        GraphIsoMapCallback, detail::isomorphism >
        s(graph1, graph2, index_map1, index_map2, edge_comp, vertex_comp);

    return detail::match(graph1, graph2, user_callback, vertex_order1, s);
}

// All default interface for vf2_graph_iso
template < typename Graph1, typename Graph2, typename GraphIsoMapCallback >
bool vf2_graph_iso(const Graph1& graph1, const Graph2& graph2,
    GraphIsoMapCallback user_callback)
{

    return vf2_graph_iso(graph1, graph2, user_callback,
        get(vertex_index, graph1), get(vertex_index, graph2),
        vertex_order_by_mult(graph1), always_equivalent(), always_equivalent());
}

// Named parameter interface of vf2_graph_iso
template < typename Graph1, typename Graph2, typename VertexOrder1,
    typename GraphIsoMapCallback, typename Param, typename Tag, typename Rest >
bool vf2_graph_iso(const Graph1& graph1, const Graph2& graph2,
    GraphIsoMapCallback user_callback, const VertexOrder1& vertex_order1,
    const bgl_named_params< Param, Tag, Rest >& params)
{

    return vf2_graph_iso(graph1, graph2, user_callback,
        choose_const_pmap(
            get_param(params, vertex_index1), graph1, vertex_index),
        choose_const_pmap(
            get_param(params, vertex_index2), graph2, vertex_index),
        vertex_order1,
        choose_param(
            get_param(params, edges_equivalent_t()), always_equivalent()),
        choose_param(
            get_param(params, vertices_equivalent_t()), always_equivalent()));
}

// Verifies a graph (sub)graph isomorphism map
template < typename Graph1, typename Graph2, typename CorresponenceMap1To2,
    typename EdgeEquivalencePredicate, typename VertexEquivalencePredicate >
inline bool verify_vf2_subgraph_iso(const Graph1& graph1, const Graph2& graph2,
    const CorresponenceMap1To2 f, EdgeEquivalencePredicate edge_comp,
    VertexEquivalencePredicate vertex_comp)
{

    BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< Graph1 >));
    BOOST_CONCEPT_ASSERT((AdjacencyMatrixConcept< Graph2 >));

    detail::equivalent_edge_exists< Graph2 > edge2_exists;

    BGL_FORALL_EDGES_T(e1, graph1, Graph1)
    {
        typename graph_traits< Graph1 >::vertex_descriptor s1, t1;
        typename graph_traits< Graph2 >::vertex_descriptor s2, t2;

        s1 = source(e1, graph1);
        t1 = target(e1, graph1);
        s2 = get(f, s1);
        t2 = get(f, t1);

        if (!vertex_comp(s1, s2) || !vertex_comp(t1, t2))
            return false;

        typename graph_traits< Graph2 >::edge_descriptor e2;

        if (!edge2_exists(s2, t2,
                detail::edge2_predicate< Graph1, Graph2,
                    EdgeEquivalencePredicate >(edge_comp, e1),
                graph2))
            return false;
    }

    return true;
}

// Variant of verify_subgraph_iso with all default parameters
template < typename Graph1, typename Graph2, typename CorresponenceMap1To2 >
inline bool verify_vf2_subgraph_iso(
    const Graph1& graph1, const Graph2& graph2, const CorresponenceMap1To2 f)
{
    return verify_vf2_subgraph_iso(
        graph1, graph2, f, always_equivalent(), always_equivalent());
}

} // namespace boost

#ifdef BOOST_ISO_INCLUDED_ITER_MACROS
#undef BOOST_ISO_INCLUDED_ITER_MACROS
#include <boost/graph/iteration_macros_undef.hpp>
#endif

#endif // BOOST_VF2_SUB_GRAPH_ISO_HPP


/*

Copyright (c) 2005-2020, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <boost/scoped_array.hpp>
#include "VtkMeshWriter.hpp"
#include "DistributedTetrahedralMesh.hpp"
#include "MixedDimensionMesh.hpp"
#include "NodesOnlyMesh.hpp"

#ifdef CHASTE_VTK
#include "vtkQuadraticTetra.h"
#include "vtkQuadraticTriangle.h"

///////////////////////////////////////////////////////////////////////////////////
// Implementation
///////////////////////////////////////////////////////////////////////////////////
template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
VtkMeshWriter<ELEMENT_DIM, SPACE_DIM>::VtkMeshWriter(const std::string& rDirectory,
                     const std::string& rBaseName,
                     const bool& rCleanDirectory)
    : AbstractTetrahedralMeshWriter<ELEMENT_DIM, SPACE_DIM>(rDirectory, rBaseName, rCleanDirectory),
      mWriteParallelFiles(false)
{
    this->mIndexFromZero = true;

    // Dubious, since we shouldn't yet know what any details of the mesh are.
    mpVtkUnstructedMesh = vtkUnstructuredGrid::New();
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::~VtkMeshWriter()
{
    mpVtkUnstructedMesh->Delete(); // Reference counted
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::MakeVtkMesh()
{
    //Construct nodes aka as Points
    vtkPoints* p_pts = vtkPoints::New(VTK_DOUBLE);
    p_pts->GetData()->SetName("Vertex positions");
    for (unsigned item_num=0; item_num<this->GetNumNodes(); item_num++)
    {
        std::vector<double> current_item = this->GetNextNode(); //this->mNodeData[item_num];
        // Add zeroes if the dimension is below 3
        for (unsigned dim=SPACE_DIM; dim<3; dim++)
        {
            current_item.push_back(0.0);//For y and z-coordinates if necessary
        }
        assert(current_item.size() == 3);
        p_pts->InsertPoint(item_num, current_item[0], current_item[1], current_item[2]);
    }
    mpVtkUnstructedMesh->SetPoints(p_pts);
    p_pts->Delete(); //Reference counted

    //Construct elements aka Cells
    for (unsigned item_num=0; item_num<this->GetNumElements(); item_num++)
    {
        std::vector<unsigned> current_element = this->GetNextElement().NodeIndices; // this->mElementData[item_num];

        assert((current_element.size() == ELEMENT_DIM + 1) || (current_element.size() == (ELEMENT_DIM+1)*(ELEMENT_DIM+2)/2));

        vtkCell* p_cell=nullptr;
        if (ELEMENT_DIM == 3 && current_element.size() == 4)
        {
            p_cell = vtkTetra::New();
        }
        else if (ELEMENT_DIM == 3 && current_element.size() == 10)
        {
            p_cell = vtkQuadraticTetra::New();
        }
        else if (ELEMENT_DIM == 2 && current_element.size() == 3)
        {
            p_cell = vtkTriangle::New();
        }
        else if (ELEMENT_DIM == 2 && current_element.size() == 6)
        {
            p_cell = vtkQuadraticTriangle::New();
        }
        else if (ELEMENT_DIM == 1)
        {
            p_cell = vtkLine::New();
        }

        //Set the linear nodes
        vtkIdList* p_cell_id_list = p_cell->GetPointIds();
        for (unsigned j = 0; j < current_element.size(); ++j)
        {
            p_cell_id_list->SetId(j, current_element[j]);
        }

        //VTK defines the node ordering in quadratic triangles differently to Chaste, so they must be treated as a special case
        if (SPACE_DIM == 2 && current_element.size() == 6)
        {
            p_cell_id_list->SetId(3, current_element[5]);
            p_cell_id_list->SetId(4, current_element[3]);
            p_cell_id_list->SetId(5, current_element[4]);
        }

        mpVtkUnstructedMesh->InsertNextCell(p_cell->GetCellType(), p_cell_id_list);
        p_cell->Delete(); //Reference counted
    }

    if (SPACE_DIM > 1)
    {
        /// \todo #2351 Temporary workaround for parallel writer
        for (unsigned item_num=0; item_num<this->GetNumBoundaryFaces(); item_num++)
        {
            this->GetNextBoundaryElement();
        }
    }

    //If necessary, construct cables
    if (this->GetNumCableElements() > 0)
    {
        AugmentCellData();
        //Make a blank cell radius data for the regular elements
        std::vector<double> radii(this->GetNumElements(), 0.0);
        for (unsigned item_num=0; item_num<this->GetNumCableElements(); item_num++)
        {
            ElementData cable_element_data = this->GetNextCableElement();
            std::vector<unsigned> current_element = cable_element_data.NodeIndices;
            radii.push_back(cable_element_data.AttributeValue);
            assert(current_element.size() == 2);
            vtkCell* p_cell=vtkLine::New();
            vtkIdList* p_cell_id_list = p_cell->GetPointIds();
            for (unsigned j = 0; j < 2; ++j)
            {
                p_cell_id_list->SetId(j, current_element[j]);
            }
            mpVtkUnstructedMesh->InsertNextCell(p_cell->GetCellType(), p_cell_id_list);
            p_cell->Delete(); //Reference counted
        }
        AddCellData("Cable radius", radii);

    }
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::AddProvenance(std::string fileName)
{
    std::string comment = "<!-- " + ChasteBuildInfo::GetProvenanceString() + "-->";

    out_stream p_vtu_file = this->mpOutputFileHandler->OpenOutputFile(fileName, std::ios::out | std::ios::app);

    *p_vtu_file << "\n" << comment << "\n";
    p_vtu_file->close();
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::WriteFiles()
{
    // Using separate scope here to make sure file is properly closed before re-opening it to add provenance info.
    {
        MakeVtkMesh();
        assert(mpVtkUnstructedMesh->CheckAttributes() == 0);
        vtkXMLUnstructuredGridWriter* p_writer = vtkXMLUnstructuredGridWriter::New();
#if VTK_MAJOR_VERSION >= 6
        p_writer->SetInputData(mpVtkUnstructedMesh);
#else
        p_writer->SetInput(mpVtkUnstructedMesh);
#endif
        std::string vtk_file_name = this->mpOutputFileHandler->GetOutputDirectoryFullPath() + this->mBaseName+".vtu";
        p_writer->SetFileName(vtk_file_name.c_str());
        //p_writer->PrintSelf(std::cout, vtkIndent());
        p_writer->Write();
        p_writer->Delete(); //Reference counted
    }

    AddProvenance(this->mBaseName + ".vtu");
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::AddCellData(std::string dataName, std::vector<double> dataPayload)
{
    vtkDoubleArray* p_scalars = vtkDoubleArray::New();
    p_scalars->SetName(dataName.c_str());
    for (unsigned i=0; i<dataPayload.size(); i++)
    {
        p_scalars->InsertNextValue(dataPayload[i]);
    }

    vtkCellData* p_cell_data = mpVtkUnstructedMesh->GetCellData();
    p_cell_data->AddArray(p_scalars);
    p_scalars->Delete(); //Reference counted
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::AugmentCellData()
{
    unsigned num_cell_arrays = mpVtkUnstructedMesh->GetCellData()->GetNumberOfArrays();
    for (unsigned i = 0; i < num_cell_arrays; i++)
    {
        vtkDataArray* array = mpVtkUnstructedMesh->GetCellData()->GetArray(i);

        //Check data was the correct size before the cables were added
        unsigned num_cable_pads = this->GetNumCableElements();
        if (mWriteParallelFiles)
        {
            assert((unsigned)array->GetNumberOfTuples() == this->mpDistributedMesh->GetNumLocalElements());
            num_cable_pads =  this->mpMixedMesh->GetNumLocalCableElements();
        }
        else
        {
            assert((unsigned)array->GetNumberOfTuples() == this->GetNumElements());
        }

        //Check that tuples of size 3 will be big enough for padding the rest of the data
        assert(array->GetNumberOfComponents() <= 3);
        double null_data[3] = {0.0, 0.0, 0.0};

        //Pad data
        for (unsigned new_index = 0; new_index <  num_cable_pads; new_index++)
        {
            array->InsertNextTuple(null_data);
        }
    }
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::AddCellData(std::string dataName, std::vector<c_vector<double, SPACE_DIM> > dataPayload)
{
    vtkDoubleArray* p_vectors = vtkDoubleArray::New();
    p_vectors->SetName(dataName.c_str());
    p_vectors->SetNumberOfComponents(3);
    for (unsigned i=0; i<dataPayload.size(); i++)
    {
        for (unsigned j=0; j<SPACE_DIM; j++)
        {
            p_vectors->InsertNextValue(dataPayload[i][j]);
        }
        //When SPACE_DIM<3, then pad
        for (unsigned j=SPACE_DIM; j<3; j++)
        {
            p_vectors->InsertNextValue(0.0);
        }
    }

    vtkCellData* p_cell_data = mpVtkUnstructedMesh->GetCellData();
    p_cell_data->AddArray(p_vectors);
    p_vectors->Delete(); //Reference counted
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::AddTensorCellData(std::string dataName, std::vector<c_vector<double,SPACE_DIM*(SPACE_DIM+1)/2> > dataPayload)
{
    assert(SPACE_DIM != 1);    // LCOV_EXCL_LINE

    vtkDoubleArray* p_vectors = vtkDoubleArray::New();
    p_vectors->SetName(dataName.c_str());
    p_vectors->SetNumberOfComponents(SPACE_DIM*SPACE_DIM);
    for (unsigned i=0; i<dataPayload.size(); i++)
    {
        if (SPACE_DIM == 2)
        {
            p_vectors->InsertNextValue(dataPayload[i](0)); //a11
            p_vectors->InsertNextValue(dataPayload[i](1)); //a12
            p_vectors->InsertNextValue(dataPayload[i](1)); //a21
            p_vectors->InsertNextValue(dataPayload[i](2)); //a22
        }
        else if (SPACE_DIM == 3)
        {
            p_vectors->InsertNextValue(dataPayload[i](0)); //a11
            p_vectors->InsertNextValue(dataPayload[i](1)); //a12
            p_vectors->InsertNextValue(dataPayload[i](2)); //a13
            p_vectors->InsertNextValue(dataPayload[i](1)); //a21
            p_vectors->InsertNextValue(dataPayload[i](3)); //a22
            p_vectors->InsertNextValue(dataPayload[i](4)); //a23
            p_vectors->InsertNextValue(dataPayload[i](2)); //a31
            p_vectors->InsertNextValue(dataPayload[i](4)); //a32
            p_vectors->InsertNextValue(dataPayload[i](5)); //a33
        }
    }

    vtkCellData* p_cell_data = mpVtkUnstructedMesh->GetCellData();
    p_cell_data->AddArray(p_vectors);
    p_vectors->Delete(); //Reference counted
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::AddTensorCellData(std::string dataName, std::vector<c_matrix<double,SPACE_DIM,SPACE_DIM> > dataPayload)
{
    assert(SPACE_DIM != 1);    // LCOV_EXCL_LINE

    vtkDoubleArray* p_vectors = vtkDoubleArray::New();
    p_vectors->SetName(dataName.c_str());
    p_vectors->SetNumberOfComponents(SPACE_DIM*SPACE_DIM);
    for (unsigned i=0; i<dataPayload.size(); i++)
    {
        if (SPACE_DIM == 2)
        {
            p_vectors->InsertNextValue(dataPayload[i](0,0)); //a11
            p_vectors->InsertNextValue(dataPayload[i](0,1)); //a12
            p_vectors->InsertNextValue(dataPayload[i](1,0)); //a21
            p_vectors->InsertNextValue(dataPayload[i](1,1)); //a22
        }
        else if (SPACE_DIM == 3)
        {
            p_vectors->InsertNextValue(dataPayload[i](0,0)); //a11
            p_vectors->InsertNextValue(dataPayload[i](0,1)); //a12
            p_vectors->InsertNextValue(dataPayload[i](0,2)); //a13
            p_vectors->InsertNextValue(dataPayload[i](1,0)); //a21
            p_vectors->InsertNextValue(dataPayload[i](1,1)); //a22
            p_vectors->InsertNextValue(dataPayload[i](1,2)); //a23
            p_vectors->InsertNextValue(dataPayload[i](2,0)); //a31
            p_vectors->InsertNextValue(dataPayload[i](2,1)); //a32
            p_vectors->InsertNextValue(dataPayload[i](2,2)); //a33
        }
    }

    vtkCellData* p_cell_data = mpVtkUnstructedMesh->GetCellData();
    p_cell_data->AddArray(p_vectors);
    p_vectors->Delete(); //Reference counted
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::AddPointData(std::string dataName, std::vector<double> dataPayload)
{
    vtkDoubleArray* p_scalars = vtkDoubleArray::New();
    p_scalars->SetName(dataName.c_str());

    if (mWriteParallelFiles && this->mpDistributedMesh != nullptr)
    {
        // In parallel, the vector we pass will only contain the values from the privately owned nodes.
        // To get the values from the halo nodes (which will be inserted at the end of the vector we need to
        // communicate with the equivalent vectors on other processes.

        // resize the payload data to include halos
        assert( dataPayload.size() == this->mpDistributedMesh->GetNumLocalNodes() );
        dataPayload.resize( this->mpDistributedMesh->GetNumLocalNodes() + this->mpDistributedMesh->GetNumHaloNodes() );

        // then do the communication
        for ( unsigned rank_offset = 1; rank_offset < PetscTools::GetNumProcs(); rank_offset++ )
        {
            unsigned send_to      = (PetscTools::GetMyRank() + rank_offset) % (PetscTools::GetNumProcs());
            unsigned receive_from = (PetscTools::GetMyRank() + PetscTools::GetNumProcs()- rank_offset ) % (PetscTools::GetNumProcs());

            unsigned number_of_nodes_to_send    = mNodesToSendPerProcess[send_to].size();
            unsigned number_of_nodes_to_receive = mNodesToReceivePerProcess[receive_from].size();

            boost::scoped_array<double> send_data(new double[number_of_nodes_to_send]);
            boost::scoped_array<double> receive_data(new double[number_of_nodes_to_receive]);
            // Pack
            for (unsigned node = 0; node < number_of_nodes_to_send; node++)
            {
                unsigned global_node_index = mNodesToSendPerProcess[send_to][node];
                unsigned local_node_index = global_node_index
                            - this->mpDistributedMesh->GetDistributedVectorFactory()->GetLow();
                send_data[node] = dataPayload[local_node_index];
            }
            {
                // Send
                int ret;
                MPI_Status status;
                ret = MPI_Sendrecv(send_data.get(), number_of_nodes_to_send,
                                   MPI_DOUBLE,
                                   send_to, 0,
                                   receive_data.get(),  number_of_nodes_to_receive,
                                   MPI_DOUBLE,
                                   receive_from, 0,
                                   PETSC_COMM_WORLD, &status);
                UNUSED_OPT(ret);
                assert ( ret == MPI_SUCCESS );
            }

            // Unpack
            for ( unsigned node = 0; node < number_of_nodes_to_receive; node++ )
            {
                unsigned global_node_index = mNodesToReceivePerProcess[receive_from][node];
                unsigned halo_index = mGlobalToNodeIndexMap[global_node_index];
                assert( halo_index >= this->mpDistributedMesh->GetNumLocalNodes() );
                dataPayload[halo_index] = receive_data[node];
            }

        }
    }

    for (unsigned i=0; i<dataPayload.size(); i++)
    {
        p_scalars->InsertNextValue(dataPayload[i]);
    }

    vtkPointData* p_point_data = mpVtkUnstructedMesh->GetPointData();
    p_point_data->AddArray(p_scalars);
    p_scalars->Delete(); //Reference counted
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::AddPointData(std::string dataName, std::vector<c_vector<double, SPACE_DIM> > dataPayload)
{
    vtkDoubleArray* p_vectors = vtkDoubleArray::New();
    p_vectors->SetName(dataName.c_str());

    if (mWriteParallelFiles)
    {
        // In parallel, the vector we pass will only contain the values from the privately owned nodes.
        // To get the values from the halo nodes (which will be inserted at the end of the vector we need to
        // communicate with the equivalent vectors on other processes.

        // resize the payload data to include halos
        assert( dataPayload.size() == this->mpDistributedMesh->GetNumLocalNodes() );
        dataPayload.resize( this->mpDistributedMesh->GetNumLocalNodes() + this->mpDistributedMesh->GetNumHaloNodes() );

        // then do the communication
        for ( unsigned rank_offset = 1; rank_offset < PetscTools::GetNumProcs(); rank_offset++ )
        {
            unsigned send_to      = (PetscTools::GetMyRank() + rank_offset) % (PetscTools::GetNumProcs());
            unsigned receive_from = (PetscTools::GetMyRank() + PetscTools::GetNumProcs()- rank_offset ) % (PetscTools::GetNumProcs());

            unsigned number_of_nodes_to_send    = mNodesToSendPerProcess[send_to].size();
            unsigned number_of_nodes_to_receive = mNodesToReceivePerProcess[receive_from].size();

            boost::scoped_array<double> send_data(new double[number_of_nodes_to_send * SPACE_DIM]);
            boost::scoped_array<double> receive_data(new double[number_of_nodes_to_receive * SPACE_DIM]);

            for (unsigned node = 0; node < number_of_nodes_to_send; node++)
            {
                unsigned global_node_index = mNodesToSendPerProcess[send_to][node];
                unsigned local_node_index = global_node_index
                            - this->mpDistributedMesh->GetDistributedVectorFactory()->GetLow();
                for (unsigned j=0; j<SPACE_DIM; j++)
                {
                    send_data[ node*SPACE_DIM + j ] = dataPayload[local_node_index][j];
                }
            }

                int ret;
                MPI_Status status;
                ret = MPI_Sendrecv(send_data.get(), number_of_nodes_to_send * SPACE_DIM,
                                   MPI_DOUBLE,
                                   send_to, 0,
                                   receive_data.get(),  number_of_nodes_to_receive * SPACE_DIM,
                                   MPI_DOUBLE,
                                   receive_from, 0,
                                   PETSC_COMM_WORLD, &status);
                UNUSED_OPT(ret);
                assert ( ret == MPI_SUCCESS );

            // Unpack
            for ( unsigned node = 0; node < number_of_nodes_to_receive; node++ )
            {
                unsigned global_node_index = mNodesToReceivePerProcess[receive_from][node];
                unsigned halo_index = mGlobalToNodeIndexMap[global_node_index];
                assert( halo_index >= this->mpDistributedMesh->GetNumLocalNodes() );
                for (unsigned j=0; j<SPACE_DIM; j++)
                {
                    dataPayload[halo_index][j] = receive_data[ node*SPACE_DIM + j ];
                }
            }
        }
    }

    p_vectors->SetNumberOfComponents(3);
    for (unsigned i=0; i<dataPayload.size(); i++)
    {
        for (unsigned j=0; j<SPACE_DIM; j++)
        {
            p_vectors->InsertNextValue(dataPayload[i][j]);
        }
        //When SPACE_DIM<3, then pad
        for (unsigned j=SPACE_DIM; j<3; j++)
        {
            p_vectors->InsertNextValue(0.0);
        }
    }

    vtkPointData* p_point_data = mpVtkUnstructedMesh->GetPointData();
    p_point_data->AddArray(p_vectors);
    p_vectors->Delete(); //Reference counted
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::AddTensorPointData(std::string dataName, std::vector<c_matrix<double,SPACE_DIM,SPACE_DIM> > dataPayload)
{
    assert(SPACE_DIM != 1);    // LCOV_EXCL_LINE

    vtkDoubleArray* p_vectors = vtkDoubleArray::New();
    p_vectors->SetName(dataName.c_str());
    p_vectors->SetNumberOfComponents(SPACE_DIM*SPACE_DIM);
    for (unsigned i=0; i<dataPayload.size(); i++)
    {
        if (SPACE_DIM == 2)
        {
            p_vectors->InsertNextValue(dataPayload[i](0,0)); //a11
            p_vectors->InsertNextValue(dataPayload[i](0,1)); //a12
            p_vectors->InsertNextValue(dataPayload[i](1,0)); //a21
            p_vectors->InsertNextValue(dataPayload[i](1,1)); //a22
        }
        else if (SPACE_DIM == 3)
        {
            p_vectors->InsertNextValue(dataPayload[i](0,0)); //a11
            p_vectors->InsertNextValue(dataPayload[i](0,1)); //a12
            p_vectors->InsertNextValue(dataPayload[i](0,2)); //a13
            p_vectors->InsertNextValue(dataPayload[i](1,0)); //a21
            p_vectors->InsertNextValue(dataPayload[i](1,1)); //a22
            p_vectors->InsertNextValue(dataPayload[i](1,2)); //a23
            p_vectors->InsertNextValue(dataPayload[i](2,0)); //a31
            p_vectors->InsertNextValue(dataPayload[i](2,1)); //a32
            p_vectors->InsertNextValue(dataPayload[i](2,2)); //a33
        }
    }

    vtkPointData* p_point_data = mpVtkUnstructedMesh->GetPointData();
    p_point_data->AddArray(p_vectors);
    p_vectors->Delete(); //Reference counted
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>::SetParallelFiles( AbstractTetrahedralMesh<ELEMENT_DIM,SPACE_DIM>& rMesh )
{
    //Have we got a distributed mesh?
    this->mpDistributedMesh = dynamic_cast<DistributedTetrahedralMesh<ELEMENT_DIM,SPACE_DIM>* >(&rMesh);
    mpNodesOnlyMesh = dynamic_cast<NodesOnlyMesh<SPACE_DIM>* >(&rMesh);

    if (this->mpDistributedMesh == nullptr && mpNodesOnlyMesh == nullptr)
    {
        EXCEPTION("Cannot write parallel files using a sequential mesh");
    }

    if (PetscTools::IsSequential())
    {
        return;     // mWriteParallelFiles is not set sequentially (so we don't set up data exchange machinery)
    }

    mWriteParallelFiles = true;

    // Populate the global to node index map (as this will be required to add point data)

    //Node index that we are writing to VTK (index into mNodes and mHaloNodes as if they were concatenated)
    unsigned index = 0;

    // Owned nodes
    for (typename AbstractMesh<ELEMENT_DIM,SPACE_DIM>::NodeIterator node_iter = rMesh.GetNodeIteratorBegin();
         node_iter != rMesh.GetNodeIteratorEnd();
         ++node_iter)
    {
        mGlobalToNodeIndexMap[node_iter->GetIndex()] = index;
        index++;
    }

    // Halo nodes
    if (this->mpDistributedMesh)
    {
        for (typename DistributedTetrahedralMesh<ELEMENT_DIM, SPACE_DIM>::HaloNodeIterator halo_iter=this->mpDistributedMesh->GetHaloNodeIteratorBegin();
                halo_iter != this->mpDistributedMesh->GetHaloNodeIteratorEnd();
                ++halo_iter)
        {
            mGlobalToNodeIndexMap[(*halo_iter)->GetIndex()] = index;
            index++;
        }

        //Calculate the halo exchange so that node-wise payloads can be communicated
        this->mpDistributedMesh->CalculateNodeExchange( mNodesToSendPerProcess, mNodesToReceivePerProcess );
    }
}

///\todo #1322 Mesh should be const
template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void VtkMeshWriter<ELEMENT_DIM, SPACE_DIM>::WriteFilesUsingMesh(
      AbstractTetrahedralMesh<ELEMENT_DIM,SPACE_DIM>& rMesh,
      bool keepOriginalElementIndexing)
{
    // Have we got a parallel mesh?
    this->mpDistributedMesh = dynamic_cast<DistributedTetrahedralMesh<ELEMENT_DIM,SPACE_DIM>* >(&rMesh);
    this->mpMixedMesh = dynamic_cast<MixedDimensionMesh<ELEMENT_DIM,SPACE_DIM>* >(&rMesh);

    if (PetscTools::IsSequential() || !mWriteParallelFiles || (this->mpDistributedMesh == nullptr && mpNodesOnlyMesh == nullptr))
    {
        AbstractTetrahedralMeshWriter<ELEMENT_DIM,SPACE_DIM>::WriteFilesUsingMesh( rMesh,keepOriginalElementIndexing );
    }
    else
    {
        //Make the local mesh into a VtkMesh
        vtkPoints* p_pts = vtkPoints::New(VTK_DOUBLE);
        p_pts->GetData()->SetName("Vertex positions");

        // Owned nodes
        for (typename AbstractMesh<ELEMENT_DIM,SPACE_DIM>::NodeIterator node_iter = rMesh.GetNodeIteratorBegin();
             node_iter != rMesh.GetNodeIteratorEnd();
             ++node_iter)
        {
            c_vector<double, SPACE_DIM> current_item = node_iter->rGetLocation();
            if (SPACE_DIM == 3)
            {
                p_pts->InsertNextPoint(current_item[0], current_item[1], current_item[2]);
            }
            else if (SPACE_DIM == 2)
            {
                p_pts->InsertNextPoint(current_item[0], current_item[1], 0.0);
            }
            else // (SPACE_DIM == 1)
            {
                p_pts->InsertNextPoint(current_item[0], 0.0, 0.0);
            }
        }

        // Halo nodes
        if (this->mpDistributedMesh)
        {
            for (typename DistributedTetrahedralMesh<ELEMENT_DIM, SPACE_DIM>::HaloNodeIterator halo_iter=this->mpDistributedMesh->GetHaloNodeIteratorBegin();
                    halo_iter != this->mpDistributedMesh->GetHaloNodeIteratorEnd();
                    ++halo_iter)
            {
                c_vector<double, SPACE_DIM> current_item = (*halo_iter)->rGetLocation();
                if (SPACE_DIM == 3)
                {
                    p_pts->InsertNextPoint(current_item[0], current_item[1], current_item[2]);
                }
                else if (SPACE_DIM == 2)
                {
                    p_pts->InsertNextPoint(current_item[0], current_item[1], 0.0);
                }
                else // (SPACE_DIM == 1)
                {
                    p_pts->InsertNextPoint(current_item[0], 0.0, 0.0);
                }
            }
        }

        mpVtkUnstructedMesh->SetPoints(p_pts);
        p_pts->Delete(); //Reference counted

        for (typename AbstractTetrahedralMesh<ELEMENT_DIM,SPACE_DIM>::ElementIterator elem_iter = rMesh.GetElementIteratorBegin();
             elem_iter != rMesh.GetElementIteratorEnd();
             ++elem_iter)
        {

            vtkCell* p_cell=nullptr;
            ///\todo This ought to look exactly like the other MakeVtkMesh
            if (ELEMENT_DIM == 3)
            {
                p_cell = vtkTetra::New();
            }
            else if (ELEMENT_DIM == 2)
            {
                p_cell = vtkTriangle::New();
            }
            else //(ELEMENT_DIM == 1)
            {
                p_cell = vtkLine::New();
            }
            vtkIdList* p_cell_id_list = p_cell->GetPointIds();
            for (unsigned j = 0; j < ELEMENT_DIM+1; ++j)
            {
                unsigned global_node_index = elem_iter->GetNodeGlobalIndex(j);
                p_cell_id_list->SetId(j, mGlobalToNodeIndexMap[global_node_index]);
            }
            mpVtkUnstructedMesh->InsertNextCell(p_cell->GetCellType(), p_cell_id_list);
            p_cell->Delete(); //Reference counted
        }
        //If necessary, construct cables
        if (this->mpMixedMesh )
        {
            AugmentCellData();
            //Make a blank cell radius data for the regular elements
            std::vector<double> radii(this->mpMixedMesh->GetNumLocalElements(), 0.0);
            for (typename MixedDimensionMesh<ELEMENT_DIM,SPACE_DIM>::CableElementIterator elem_iter = this->mpMixedMesh->GetCableElementIteratorBegin();
                 elem_iter != this->mpMixedMesh->GetCableElementIteratorEnd();
                 ++elem_iter)
            {
                radii.push_back((*elem_iter)->GetAttribute());
                vtkCell* p_cell=vtkLine::New();
                vtkIdList* p_cell_id_list = p_cell->GetPointIds();
                for (unsigned j = 0; j < 2; ++j)
                {
                    unsigned global_node_index = (*elem_iter)->GetNodeGlobalIndex(j);
                    p_cell_id_list->SetId(j, mGlobalToNodeIndexMap[global_node_index]);
                }
                mpVtkUnstructedMesh->InsertNextCell(p_cell->GetCellType(), p_cell_id_list);
                p_cell->Delete(); //Reference counted
            }
            AddCellData("Cable radius", radii);
        }

        //This block is to guard the mesh writers (vtkXMLPUnstructuredGridWriter) so that they
        //go out of scope, flush buffers and close files
        {
            assert(mpVtkUnstructedMesh->CheckAttributes() == 0);
            vtkXMLPUnstructuredGridWriter* p_writer = vtkXMLPUnstructuredGridWriter::New();

            p_writer->SetDataModeToBinary();

            p_writer->SetNumberOfPieces(PetscTools::GetNumProcs());
            //p_writer->SetGhostLevel(-1);
            p_writer->SetStartPiece(PetscTools::GetMyRank());
            p_writer->SetEndPiece(PetscTools::GetMyRank());

#if VTK_MAJOR_VERSION >= 6
            p_writer->SetInputData(mpVtkUnstructedMesh);
#else
            p_writer->SetInput(mpVtkUnstructedMesh);
#endif
            std::string pvtk_file_name = this->mpOutputFileHandler->GetOutputDirectoryFullPath() + this->mBaseName+ ".pvtu";
            p_writer->SetFileName(pvtk_file_name.c_str());
            //p_writer->PrintSelf(std::cout, vtkIndent());
            p_writer->Write();
            p_writer->Delete(); //Reference counted
        }

        // Add provenance to the individual files
        std::stringstream filepath;
        filepath << this->mBaseName << "_" << PetscTools::GetMyRank() << ".vtu";
        AddProvenance(filepath.str());
        /// Add to the main file \todo #1494 Do we need a barrier?
        if (PetscTools::AmMaster())
        {
            AddProvenance(this->mBaseName+ ".pvtu");
        }
    }
}

// Explicit instantiation
template class VtkMeshWriter<1,1>;
template class VtkMeshWriter<1,2>;
template class VtkMeshWriter<1,3>;
template class VtkMeshWriter<2,2>; // Actually used
template class VtkMeshWriter<2,3>;
template class VtkMeshWriter<3,3>; // Actually used

#endif //CHASTE_VTK