// This will be an interface for meshing service and blueberry extension point etc.

#include <numeric>
#include <thread>
#include <valarray>

// #define CGAL_EIGEN3_ENABLED
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <boost/function_output_iterator.hpp>

// #include <CGAL/Polyhedron_3.h>
// #include <CGAL/extract_mean_curvature_flow_skeleton.h>
// #include <CGAL/boost/graph/split_graph_into_polylines.h>

#include <fstream>

#include "IMeshingKernel.h"

#include <VesselPathAbstractData.h>

#include <SolidData.h>
//#include <DiscreteSolidData.h>
#include <OCCBRepData.h>
#include <MeshData.h>

#include <TopoDS_Shape.hxx>
#include <TopoDS_Vertex.hxx>
#include <TopoDS_Edge.hxx>
#include <TopExp_Explorer.hxx>
#include <TopoDS.hxx>
#include <gp_Pnt.hxx>
#include <gp_Pln.hxx>
#include <BRep_Tool.hxx>
#include <BRep_Builder.hxx>
#include <BRepTools_WireExplorer.hxx>
#include <BRepAdaptor_Curve.hxx>
#include <BRepAdaptor_Surface.hxx>

#include <Geom_Curve.hxx>
#include <Geom_BSplineCurve.hxx>
#include <Geom_BSplineSurface.hxx>
#include <Geom_Plane.hxx>
#include <GeomConvert.hxx>
#include <Geom_Line.hxx>

#include <TColStd_Array1OfReal.hxx>

#include <BSplCLib.hxx>

#include <BRepBuilderAPI_MakeFace.hxx>
#include <BRepBuilderAPI_NurbsConvert.hxx>
#include <BRepLib_FindSurface.hxx>

#include <boost/regex.hpp>
#include <boost/scope_exit.hpp>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <vtkPointData.h>
#include <vtkDoubleArray.h>
#include <vtkNew.h>
#include <vtkProbeFilter.h>
#include <vtkUnstructuredGridBase.h>
#include <vtkIdTypeArray.h>
#include <vtkXMLPolyDataWriter.h>
#include <vtkXMLUnstructuredGridWriter.h>
#include <vtkUnstructuredGrid.h>
#include <vtkCleanPolyData.h>
#include <vtkvmtkPolyDataSurfaceRemeshing.h>
#include <vtkvmtkBoundaryLayerGenerator.h>
#include <vtkvmtkTetGenWrapper.h>
#include "crimsonTetGenWrapper.h"
#include <vtkvmtkPolyDataToUnstructuredGridFilter.h>
#include <vtkvmtkPolyDataSizingFunction.h>
#include <vtkvmtkSurfaceProjection.h>
#include <vtkCellData.h>
#include <vtkPolyDataNormals.h>
#include <vtkGeometryFilter.h>
#include <vtkvmtkSimpleCapPolyData.h>
#include <vtkTriangleFilter.h>
#include <vtkAppendFilter.h>
#include <vtkvmtkUnstructuredGridTetraFilter.h>
#include <vtkThreshold.h>
#include <vtkIntArray.h>
#include <vtkMath.h>
#include <vtkMeshQuality.h>
#include <vtkTriangle.h>
#include <vtkCellLocator.h>
#include <vtkExtractCells.h>
#include <vtkFeatureEdges.h>

#include <Wm5ContMinSphere3.h>
#include <Wm5ApprPlaneFit3.h>

#include <boost/format.hpp>
#include <qstandardpaths.h>

#include <gsl.h>

#include <unordered_set>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_3 Point;
typedef CGAL::Surface_mesh<Point> Mesh;
typedef Mesh::Vertex_index Vertex_index;
typedef boost::graph_traits<Mesh>::halfedge_descriptor halfedge_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor     edge_descriptor;
typedef boost::graph_traits<Mesh>::face_descriptor     face_descriptor;
typedef boost::graph_traits<Mesh>::vertex_descriptor   vertex_descriptor;
namespace PMP = CGAL::Polygon_mesh_processing;

// typedef CGAL::Mean_curvature_flow_skeletonization<Mesh>       Skeletonization;
// typedef Skeletonization::Skeleton                             Skeleton;
// typedef Skeleton::vertex_descriptor                           Skeleton_vertex;
// typedef Skeleton::edge_descriptor                             Skeleton_edge;

namespace crimson
{
	class MeshingTask : public crimson::async::TaskWithResult<mitk::BaseData::Pointer>
	{
	public:
		MeshingTask(const mitk::BaseData::Pointer& solid, const IMeshingKernel::GlobalMeshingParameters& params,
			const std::map<FaceIdentifier, IMeshingKernel::LocalMeshingParameters>& localParams,
			const std::map<VesselPathAbstractData::VesselPathUIDType, std::string>& vesselUIDtoNameMap)
			: solid(solid)
			, params(params)
			, localParams(localParams)
			, vesselUIDtoNameMap(vesselUIDtoNameMap)
		{
		}
		~MeshingTask() { }

		void cancel() override
		{
			crimson::async::TaskWithResult<mitk::BaseData::Pointer>::cancel();
		}

	protected:
		mitk::BaseData::Pointer solid;
		IMeshingKernel::GlobalMeshingParameters params;
		std::map<FaceIdentifier, IMeshingKernel::LocalMeshingParameters> localParams;
		std::map<VesselPathAbstractData::VesselPathUIDType, std::string> vesselUIDtoNameMap;
	};

	class OCCMeshingTask : public crimson::MeshingTask
	{
	public:
		OCCMeshingTask(const mitk::BaseData::Pointer& solid, const IMeshingKernel::GlobalMeshingParameters& params,
			const std::map<FaceIdentifier, IMeshingKernel::LocalMeshingParameters>& localParams,
			const std::map<VesselPathAbstractData::VesselPathUIDType, std::string>& vesselUIDtoNameMap)
			:MeshingTask(solid, params, localParams, vesselUIDtoNameMap) {}

		~OCCMeshingTask(){}

		std::tuple<State, std::string> runTask() override
		{
            static const bool debug = false;
            auto writeVTU = [](vtkUnstructuredGrid* ug, const char* fn) {
                if (!debug) return;
                MITK_INFO << "writing " << fn;
                vtkNew<vtkXMLUnstructuredGridWriter> ugw;
                ugw->SetFileName((std::string("d:/kcl/") + fn + ".vtu").c_str());
                ugw->SetInputData(ug);
                ugw->Write();
            };
            auto writeVTP = [](vtkPolyData* pd, const char* fn) {
                if (!debug) return;
                MITK_INFO << "writing " << fn;
                vtkNew<vtkXMLPolyDataWriter> pdw;
                pdw->SetFileName((std::string("d:/kcl/") + fn + ".vtp").c_str());
                pdw->SetInputData(pd);
                pdw->Write();
            };
            auto coordsToVector = [](double* c) {
                return Wm5::Vector3d(c[0], c[1], c[2]);
            };

            auto brep = dynamic_cast<SolidData*>(solid.GetPointer());

            if (!brep) {
                return std::make_tuple(State_Failed, std::string("Cannot use solids defined not as OpenCASCADE data"));
            }

            stepsAddedSignal(1 + brep->getFaceIdentifierMap().getNumberOfFaceIdentifiers());

            auto faceEdgeSize = [&](int faceId) {
                const FaceIdentifier& identifier = brep->getFaceIdentifierMap().getFaceIdentifier(faceId);
                auto it = localParams.find(identifier);

                double targetLength = it == localParams.end() || !it->second.size ? *params.defaultLocalParameters.size : *it->second.size;
                if (it == localParams.end() || !it->second.sizeRelative ? *params.defaultLocalParameters.sizeRelative : *it->second.sizeRelative) {
                    targetLength *= sqrt(brep->GetGeometry()->GetBoundingBox()->GetDiagonalLength2());
                }
                return targetLength;
            };

            vtkNew<vtkPolyData> cpy;
            cpy->DeepCopy(brep->getSurfaceRepresentation()->GetVtkPolyData());
            auto pd = cpy.Get();
            //auto pd = brep->getSurfaceRepresentation()->GetVtkPolyData();

            vtkNew<vtkPolyData> remeshedPd;
            vtkNew<vtkPoints> points;

            vtkNew<vtkIntArray> faceIdArray;
            faceIdArray->Allocate(1);
            faceIdArray->SetName("Face IDs");

            remeshedPd->SetPoints(points.GetPointer());
            remeshedPd->GetCellData()->AddArray(faceIdArray.GetPointer());
            remeshedPd->Allocate();

            // Compute normals
            vtkNew<vtkCleanPolyData> c;
            c->SetInputData(pd);
            c->Update();

            vtkNew<vtkFeatureEdges> fe;
            fe->SetInputData(c->GetOutput());
            fe->FeatureEdgesOff();
            fe->Update();

            if (fe->GetOutput()->GetNumberOfCells() != 0) {
                return std::make_tuple(State_Failed, std::string("Could not achieve a watertight surface mesh. Try different fillet sizes."));
            }

            vtkNew<vtkPolyDataNormals> n;
            n->SetInputConnection(c->GetOutputPort());
            n->SetComputeCellNormals(1);
            n->SetComputePointNormals(0);
            n->SetAutoOrientNormals(1);
            n->SetFlipNormals(0);
            n->SetConsistency(1);
            n->SplittingOff();
            n->Update();

            // Remesh the surface using CGAL
            vtkPolyData* remeshInput = n->GetOutput();
            writeVTP(remeshInput, "remeshInput");
            {
                Mesh mesh;

                for (int j = 0; j < remeshInput->GetPoints()->GetNumberOfPoints(); ++j) {
                    double* coords = remeshInput->GetPoints()->GetPoint(j);
                    mesh.add_vertex(Point(coords[0], coords[1], coords[2]));
                }

                auto fimap = mesh.add_property_map<face_descriptor, int>("f:id").first;

                std::unordered_set<int> faceIdSet;

                auto arr = remeshInput->GetCellData()->GetArray("Face IDs");
                for (int k = 0; k < remeshInput->GetNumberOfCells(); ++k) {
                    vtkSmartPointer<vtkCell> cell = remeshInput->GetCell(k);
                    if (cell->GetCellType() != VTK_TRIANGLE) {
                        continue;
                    }

                    // Ensure that all faces are passed into CGAL with consistent ordering
                    Vertex_index ids[] = {Vertex_index(cell->GetPointId(0)), Vertex_index(cell->GetPointId(1)), Vertex_index(cell->GetPointId(2))};

                    Wm5::Vector3d coords[3];
                    boost::transform(ids, coords,
                        [&](Vertex_index id) {
                            return coordsToVector(remeshInput->GetPoint(id));
                        }
                    );

                    Wm5::Vector3d normal = coordsToVector(remeshInput->GetCellData()->GetNormals()->GetTuple(k));
                    if (normal.Dot((coords[1] - coords[0]).Cross(coords[2] - coords[1])) < 0) {
                        std::swap(ids[0], ids[1]);
                    }

                    int faceId = arr->GetTuple1(k);
                    fimap[mesh.add_face(ids[0], ids[1], ids[2])] = faceId;
                    faceIdSet.insert(faceId);
                }

                // Arrange the order of remeshing from largest edge size to smallest
                struct FaceEdgeSize {
                    int id;
                    double edgeSize;
                };

                std::vector<FaceEdgeSize> edgeSizes(faceIdSet.size());
                boost::transform(faceIdSet, edgeSizes.begin(), [&](int faceId) { return FaceEdgeSize{faceId, faceEdgeSize(faceId)}; });

                boost::sort(edgeSizes, [](const FaceEdgeSize& lhs, const FaceEdgeSize& rhs) { return lhs.edgeSize > rhs.edgeSize; });

                // Do the remeshing
//                int xx = 0;
                // Map to constrain the edges between different face IDs

                for (const FaceEdgeSize& edgeSize : edgeSizes) {
                    // Map to constrain the edges between different face IDs
                    auto cstmap = mesh.add_property_map<edge_descriptor, bool>("e:cst", false).first;

                    // Find all faces for this face Id
                    std::unordered_set<face_descriptor> facesForId;
                    boost::copy(
                        faces(mesh) | boost::adaptors::filtered([&](face_descriptor d) { return fimap[d] == edgeSize.id; }),
                        std::inserter(facesForId, facesForId.end())
                    );

                    // Add a layer of faces around the edges of this faceId so that the edge is remeshed well
                    std::unordered_set<face_descriptor> additionalFacesForId;
                    for (auto he : mesh.halfedges()) {
//                         if (mesh.is_border(mesh.edge(he))) {
//                             continue;
//                         }

                        int f1Count = facesForId.count(mesh.face(he));
                        int f2Count = facesForId.count(mesh.face(mesh.opposite(he)));
                        if (f1Count + f2Count == 1) { // One of the faces is in our faceId, another one is not
                            cstmap[mesh.edge(he)] = true; // Constrain the edge

                            boost::copy(
                                mesh.faces_around_target(he) |
                                    boost::adaptors::filtered([&](face_descriptor d) { return fimap[d] != edgeSize.id; }),
                                std::inserter(additionalFacesForId, additionalFacesForId.end())
                            );
                        }
                    }

                    // Build the full list of faces to remesh
                    boost::copy(additionalFacesForId, std::inserter(facesForId, facesForId.end()));

//                     std::ostringstream oss;
//                     oss << "d:/kcl/reminput" << xx++ << ".off";
//                     std::ofstream os(oss.str().c_str());
//                     os << mesh;
//                     os.close();

                    MITK_INFO << "Start remeshing of model face " << edgeSize.id << " (" << facesForId.size() << " faces)...";
                    PMP::isotropic_remeshing(
                        facesForId,
                        edgeSize.edgeSize,
                        mesh,
                        PMP::parameters::number_of_iterations(params.surfaceOptimizationLevel)
                        .edge_is_constrained_map(cstmap)
                        .face_patch_map(fimap)
                        .relax_constraints(true)
                    );
                    mesh.collect_garbage();
                    progressMadeSignal(1);

                    mesh.remove_property_map(cstmap);
                }
                MITK_INFO << "Done remeshing - " << mesh.number_of_vertices() << " vertices, " << mesh.number_of_faces() << " faces.";

//                 MITK_INFO << "Skeletonizing";
//
//                 Skeleton skeleton;
//                 //CGAL::extract_mean_curvature_flow_skeleton(mesh, skeleton);
//                 Skeletonization skelMaker(mesh);
//                 skelMaker.set_quality_speed_tradeoff(0.5);
//                 skelMaker(skeleton);
//
//                 MITK_INFO << "Done";
//
//                 {
//                     vtkNew<vtkPolyData> pd;
//                     vtkNew<vtkPoints> points;
//                     pd->SetPoints(points.GetPointer());
//                     pd->Allocate();
//
//                     BOOST_FOREACH (Skeleton_vertex v, vertices(skeleton))
//                         for (vertex_descriptor vd : skeleton[v].vertices) {
//                             auto p = get(CGAL::vertex_point, mesh, vd);
//                             points->InsertNextPoint(p[0], p[1], p[2]);
//
//                             auto p2 = skeleton[v].point;
//                             points->InsertNextPoint(p2[0], p2[1], p2[2]);
//
//                             vtkIdType ids[2] = {points->GetNumberOfPoints() - 2, points->GetNumberOfPoints() - 1};
//
//                             pd->InsertNextCell(VTK_LINE, 2, ids);
//
//                             auto blSizeForFaceId = [&](int faceId) {
//                                 const FaceIdentifier& identifier = brep->getFaceIdentifierMap().getFaceIdentifier(faceId);
//                                 auto it = localParams.find(identifier);
//
//                                 return it == localParams.end() || !it->second.propagationDistance ? *params.defaultLocalParameters.propagationDistance : *it->second.propagationDistance;
//                             };
//
//                             double blSize = 0;
//                             int count = 0;
//                             for (face_descriptor fd : mesh.faces_around_target(mesh.halfedge(vd))) {
//                                 blSize += blSizeForFaceId(fimap[fd]);
//                                 ++count;
//                             }
//                             blSize /= count;
//
//
//
//                             Wm5::Vector3d surfPoint(p[0], p[1], p[2]);
//                             Wm5::Vector3d skelPoint(p2[0], p2[1], p2[2]);
//
//                             Wm5::Vector3d newSurfPoint = skelPoint - surfPoint;
//                             newSurfPoint.Normalize();
//                             newSurfPoint *= blSize;
//                             newSurfPoint += surfPoint;
//                             decltype(p) newP(newSurfPoint[0], newSurfPoint[1], newSurfPoint[2]);
//
//                             put(CGAL::vertex_point, mesh, vd, newP);
//                         }
//
//                     writeVTP(pd.Get(), "Skeleton directions");
//                 }

                // Build the VTK data structure from CGAL
                for (auto vi : mesh.vertices()) {
                    Point p = mesh.point(vi);
                    points->InsertNextPoint(p[0], p[1], p[2]);
                }

                for (auto fi : mesh.faces()) {
                    vtkIdType ids[3];

                    boost::copy(vertices_around_face(mesh.halfedge(fi), mesh), ids);

                    remeshedPd->InsertNextCell(VTK_TRIANGLE, 3, ids);
                    faceIdArray->InsertNextTuple1(fimap[fi]);
                }
            }

            writeVTP(remeshedPd.GetPointer(), "00 - remeshed");

            vtkSmartPointer<vtkUnstructuredGrid> finalResult;

            if (params.meshSurfaceOnly) {
                vtkNew<vtkvmtkPolyDataToUnstructuredGridFilter> surfaceToMesh;
                surfaceToMesh->SetInputData(remeshedPd.GetPointer());
                surfaceToMesh->Update();

                finalResult = surfaceToMesh->GetOutput();
            } else {
                bool boundaryLayers = params.defaultLocalParameters.useBoundaryLayers;
                stepsAddedSignal(boundaryLayers ? 100 : 25);

                auto runTetGen = [&](vtkPolyData* input) {
                    // *******
                    MITK_INFO << "Computing sizing function.";
                    vtkNew<vtkvmtkPolyDataSizingFunction> sizer;
                    sizer->SetSizingFunctionArrayName("VolumeSizingFunction");
                    sizer->SetInputData(input);
                    sizer->Update();

                    vtkNew<vtkvmtkPolyDataToUnstructuredGridFilter> surf2Mesh;
                    surf2Mesh->SetInputConnection(sizer->GetOutputPort());
                    surf2Mesh->Update();

                    MITK_INFO << "Running 3D mesher.";
                    vtkNew<crimsonTetGenWrapper> mesher;
                    mesher->SetInputConnection(surf2Mesh->GetOutputPort());
                    mesher->SetCellEntityIdsArrayName("Face IDs");
                    mesher->SetSizingFunctionArrayName("VolumeSizingFunction");
                    mesher->SetUseSizingFunction(1);
                    mesher->SetOrder(1);
                    mesher->SetQuality(1);
                    mesher->SetPLC(1);
                    mesher->SetNoBoundarySplit(boundaryLayers ? 1 : 0);
                    mesher->SetRemoveSliver(0);
                    mesher->SetOutputSurfaceElements(boundaryLayers ? 0 : 1);
                    mesher->SetOutputVolumeElements(1);
                    mesher->SetVerbose(1);
                    mesher->SetOptimizationLevel(params.volumeOptimizationLevel);
                    mesher->SetMinDihedral(params.minDihedralAngle);
                    mesher->SetMaxDihedral(params.maxDihedralAngle);
                    mesher->SetMaxRatio(params.maxRadiusEdgeRatio);
                    mesher->Update();

                    writeVTU(mesher->GetOutput(), "06 - mesh result");

                    return vtkSmartPointer<vtkUnstructuredGrid>(mesher->GetOutput());
                };

                ///////////////////
                if (!boundaryLayers) {
                    // Remove duplicate points produced when caps are removed
                    vtkNew<vtkCleanPolyData> cleaner;
                    cleaner->SetInputData(remeshedPd.GetPointer());
                    cleaner->Update();

                    finalResult = runTetGen(cleaner->GetOutput());
                    if (finalResult->GetNumberOfCells() == 0) {
                        return std::make_pair(State_Failed, std::string("TetGen failed, see log for details."));
                    }
                    vtkDataArray* faceIdArray = finalResult->GetCellData()->GetArray("Face IDs");
                    for (int faceId = 0; faceId < finalResult->GetNumberOfCells(); ++faceId) {
                        vtkSmartPointer<vtkCell> cell = finalResult->GetCell(faceId);
                        if (cell->GetCellDimension() == 3) {
                            faceIdArray->SetTuple1(faceId, -1);
                        }
                    }
                    progressMadeSignal(25);
                } else {
                    // We will remove caps to generate boundary layers.

                    // Assign thickness before removing caps
                    vtkNew<vtkDoubleArray> thicknessArray;
                    thicknessArray->SetNumberOfTuples(remeshedPd->GetNumberOfPoints());
                    thicknessArray->SetName("BL thickness");
                    remeshedPd->GetPointData()->AddArray(thicknessArray.Get());

                    auto faceBLThickness = [&](int faceId) {
                        const FaceIdentifier& identifier = brep->getFaceIdentifierMap().getFaceIdentifier(faceId);
                        auto it = localParams.find(identifier);

                        return (it == localParams.end() || !it->second.thickness
                                ? *params.defaultLocalParameters.thickness
                                : *it->second.thickness);
                    };

                    vtkNew<vtkIdList> ptIdList;
                    ptIdList->SetNumberOfIds(1);
                    vtkNew<vtkIdList> cellIdList;
                    for (int i = 0; i < remeshedPd->GetNumberOfPoints(); ++i) {
                        ptIdList->SetId(0, i);
                        remeshedPd->GetCellNeighbors(-1, ptIdList.Get(), cellIdList.Get());

                        double averageThickness = 0;
                        for (int cellId = 0; cellId < cellIdList->GetNumberOfIds(); ++cellId) {
                            averageThickness += faceBLThickness(remeshedPd->GetCellData()->GetArray("Face IDs")->GetTuple1(cellIdList->GetId(cellId)));
                        }

                        thicknessArray->SetTuple1(i, averageThickness / cellIdList->GetNumberOfIds());
                    }

                    writeVTP(remeshedPd.Get(), "bl thickness");

                    // Record the plane information for restoration of ids and normal constraints.
                    struct PlaneInfo {
                        Wm5::Plane3d plane;
                        Wm5::Sphere3d sphere;

                        bool inPlane(double* coords, double tol = 0.01) const {
                            Wm5::Vector3d p(coords[0], coords[1], coords[2]);
                            return plane.DistanceTo(p) < tol && (sphere.Center - p).Length() < sphere.Radius + tol;
                        }

                        Wm5::Vector3d project(double* vec) const {
                            Wm5::Vector3d p(vec[0], vec[1], vec[2]);
                            Wm5::Vector3d projected = p - p.Dot(plane.Normal) * plane.Normal;
                            projected.Normalize();
                            return projected;
                        }
                    };

                    std::map<int, PlaneInfo> flowFacePlanes;

                    auto removeCaps = [&](vtkPolyData* polyData) {
                        polyData->BuildLinks();

                        // Find all cap triangles
                        std::unordered_set<int> capIds;
                        const FaceIdentifierMap& faceIdMap = brep->getFaceIdentifierMap();
                        for (int i = 0; i < faceIdMap.getNumberOfFaceIdentifiers(); ++i) {
                            FaceIdentifier::FaceType faceType = faceIdMap.getFaceIdentifier(i).faceType;
                            if (faceType == FaceIdentifier::ftCapInflow || faceType == FaceIdentifier::ftCapOutflow) {
                                capIds.insert(i);
                            }
                        }

                        // Remove the triangles while recording the points that belong to them
                        std::map<int, std::set<int>> pointIds;
                        vtkDataArray* faceIdArray = polyData->GetCellData()->GetArray("Face IDs");
                        for (int cellId = 0; cellId < polyData->GetNumberOfCells(); ++cellId) {
                            if (polyData->GetCell(cellId)->GetCellType() != VTK_TRIANGLE) {
                                polyData->DeleteCell(cellId);
                                continue;
                            }
                            int faceId = static_cast<int>(faceIdArray->GetTuple1(cellId) + 0.5);
                            if (capIds.count(faceId) != 0) {
                                for (int i = 0; i < polyData->GetCell(cellId)->GetNumberOfPoints(); ++i) {
                                    pointIds[faceId].insert(polyData->GetCell(cellId)->GetPointId(i));
                                }

                                polyData->DeleteCell(cellId);
                            }
                        }

                        // Fit the planes to the points of caps
                        for (const auto& faceIdToPointIds : pointIds) {
                            std::vector<Wm5::Vector3d> points(faceIdToPointIds.second.size());
                            boost::transform(faceIdToPointIds.second, points.begin(), [&](int ptId) { return coordsToVector(polyData->GetPoint(ptId)); });

                            PlaneInfo planeInfo;
                            planeInfo.plane = Wm5::OrthogonalPlaneFit3(points.size(), points.data());
                            Wm5::MinSphere3d(points.size(), points.data(), planeInfo.sphere);

                            flowFacePlanes[faceIdToPointIds.first] = planeInfo;
                        }

                        // Finally, remove the cells completely
                        polyData->RemoveDeletedCells();
                    };

                    MITK_INFO << "Removing caps before boundary layer creation";

                    removeCaps(remeshedPd.GetPointer());
                    writeVTP(remeshedPd.GetPointer(), "01 - noCaps");

                    progressMadeSignal(5);
                    if (isCancelling()) {
                        return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                    }
                    MITK_INFO << "Computing normals";

                    // Remove duplicate points produced when caps are removed
                    vtkNew<vtkCleanPolyData> cleaner;
                    cleaner->SetInputData(remeshedPd.GetPointer());
                    cleaner->Update();

                    // Recompute normals on the remeshed surface
                    vtkNew<vtkPolyDataNormals> normals;
                    normals->SetInputConnection(cleaner->GetOutputPort());
                    normals->SetComputeCellNormals(0);
                    normals->SetAutoOrientNormals(1);
                    normals->SetFlipNormals(0);
                    normals->SetConsistency(1);
                    normals->SplittingOff();
                    normals->Update();
                    normals->GetOutput()->GetPointData()->GetNormals()->SetName("Normals");

                    // Ensure normals are in original planes for flow faces
                    for (int ptId = 0; ptId < normals->GetOutput()->GetNumberOfPoints(); ++ptId) {
                        double* coords = normals->GetOutput()->GetPoint(ptId);
                        for (const auto& faceIdToPlaneInfo : flowFacePlanes) {
                            if (faceIdToPlaneInfo.second.inPlane(coords)) {
                                double* n = normals->GetOutput()->GetPointData()->GetNormals()->GetTuple3(ptId);
                                Wm5::Vector3d projected = faceIdToPlaneInfo.second.project(n);
                                normals->GetOutput()->GetPointData()->GetNormals()->SetTuple3(ptId, projected.X(), projected.Y(), projected.Z());
                            }
                        }
                    }

                    writeVTP(normals->GetOutput(), "02 - normals");

                    progressMadeSignal(5);
                    if (isCancelling()) {
                        return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                    }
                    MITK_INFO << "Generating boundary layers";

                    vtkNew<vtkvmtkPolyDataToUnstructuredGridFilter> surfaceToMesh;
                    surfaceToMesh->SetInputConnection(normals->GetOutputPort());
                    surfaceToMesh->Update();

                    const int InnerSurfId = surfaceToMesh->GetOutput()->GetCellData()->GetArray("Face IDs")->GetRange()[1] + 10;
                    const int SidewallId = InnerSurfId + 1;
                    const int VolumeId = SidewallId + 1;

                    vtkNew<vtkvmtkBoundaryLayerGenerator> boundaryLayer;
                    boundaryLayer->SetInputData(surfaceToMesh->GetOutput());
                    boundaryLayer->SetWarpVectorsArrayName("Normals");
                    boundaryLayer->SetCellEntityIdsArrayName("Face IDs");
                    boundaryLayer->NegateWarpVectorsOn();
                    boundaryLayer->IncludeSurfaceCellsOff();
                    boundaryLayer->IncludeSidewallCellsOn();
                    boundaryLayer->SetInnerSurfaceCellEntityId(InnerSurfId);
                    boundaryLayer->SetSidewallCellEntityId(SidewallId);
                    boundaryLayer->SetVolumeCellEntityId(VolumeId);

                    // Common boundary layer parameters
                    boundaryLayer->SetNumberOfSubLayers(params.defaultLocalParameters.numSubLayers);
                    boundaryLayer->SetSubLayerRatio(params.defaultLocalParameters.subLayerRatio);

                    boundaryLayer->SetNumberOfSubsteps(2000);
                    boundaryLayer->SetRelaxation(0.01);
                    boundaryLayer->SetLocalCorrectionFactor(0.45);
                    boundaryLayer->SetLayerThicknessArrayName("BL thickness");
                    boundaryLayer->SetConstantThickness(0);
                    boundaryLayer->SetLayerThicknessRatio(1); // Just a multiplier

                    boundaryLayer->Update();

                    writeVTU(boundaryLayer->GetOutput(), "03 - bl");

                    progressMadeSignal(25);
                    if (isCancelling()) {
                        return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                    }
                    MITK_INFO << "Capping the internal surface of boundary layer result";

                    // Extract volume cells
                    vtkNew<vtkThreshold> blVolumeCells;
                    blVolumeCells->SetInputData(boundaryLayer->GetOutput());
                    blVolumeCells->SetInputArrayToProcess(0, 0, 0, vtkDataObject::FIELD_ASSOCIATION_CELLS, "Face IDs");
                    blVolumeCells->ThresholdBetween(VolumeId, VolumeId);
                    blVolumeCells->Update();

                    // Extract sidewall cells
                    vtkNew<vtkThreshold> blSidewallCells;
                    blSidewallCells->SetInputData(boundaryLayer->GetOutput());
                    blSidewallCells->SetInputArrayToProcess(0, 0, 0, vtkDataObject::FIELD_ASSOCIATION_CELLS, "Face IDs");
                    blSidewallCells->ThresholdBetween(SidewallId, SidewallId);
                    blSidewallCells->Update();

                    // Cap the inner surface
                    vtkNew<vtkGeometryFilter> meshToSurfaceInner;
                    meshToSurfaceInner->SetInputData(boundaryLayer->GetInnerSurface());
                    meshToSurfaceInner->Update();

                    const int FirstCapId = VolumeId + 10;
                    vtkNew<vtkvmtkSimpleCapPolyData> capper;
                    capper->SetInputData(meshToSurfaceInner->GetOutput());
                    capper->SetCellEntityIdsArrayName("Face IDs");
                    capper->SetCellEntityIdOffset(FirstCapId);
                    capper->Update();

                    vtkNew<vtkTriangleFilter> triangulator;
                    triangulator->SetInputData(capper->GetOutput());
                    triangulator->Update();

                    writeVTP(triangulator->GetOutput(), "04 - capped");

                    progressMadeSignal(5);
                    if (isCancelling()) {
                        return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                    }
                    MITK_INFO << "Remesing caps";

                    // Remesh caps
                    // Assign mesh sizes for caps
                    auto assignFlowFaceMeshSizes = [&](vtkPolyData* pd, const char* arrayName) {
                        vtkNew<vtkDoubleArray> sizeArray;
                        sizeArray->SetName(arrayName);
                        sizeArray->SetNumberOfTuples(pd->GetNumberOfPoints());
                        sizeArray->FillComponent(0, 0);
                        pd->GetPointData()->AddArray(sizeArray.GetPointer());

                        vtkDataArray* faceIdArray = pd->GetCellData()->GetArray("Face IDs");
                        for (int faceId = 0; faceId < pd->GetNumberOfCells(); ++faceId) {
                            vtkSmartPointer<vtkCell> cell = pd->GetCell(faceId);
                            double cellId = faceIdArray->GetTuple1(faceId);

                            if (cellId >= FirstCapId) {
                                for (const auto& faceIdToPlaneInfo : flowFacePlanes) {
                                    if (faceIdToPlaneInfo.second.inPlane(cell->GetPoints()->GetPoint(0))) {
                                        double edgeSize = faceEdgeSize(faceIdToPlaneInfo.first);
                                        double targetArea = sqrt(3) / 4 * edgeSize * edgeSize;

                                        for (int ptId = 0; ptId < cell->GetNumberOfPoints(); ++ptId) {
                                            sizeArray->SetTuple1(cell->GetPointId(ptId), targetArea);
                                        }
                                    }
                                }
                            }
                        }
                    };

                    assignFlowFaceMeshSizes(triangulator->GetOutput(), "Cap edge size");

                    vtkNew<vtkvmtkPolyDataSurfaceRemeshing> remeshCaps;
                    remeshCaps->SetInputData(triangulator->GetOutput());
                    remeshCaps->SetCellEntityIdsArrayName("Face IDs");
                    remeshCaps->SetPreserveBoundaryEdges(1);
                    remeshCaps->SetTargetAreaArrayName("Cap edge size");
                    remeshCaps->SetElementSizeModeToTargetAreaArray();
                    vtkNew<vtkIdList> idList;
                    idList->InsertNextId(InnerSurfId);
                    remeshCaps->SetExcludedEntityIds(idList.GetPointer());
                    remeshCaps->Update();

                    writeVTP(remeshCaps->GetOutput(), "05 - cap-remesh");

                    progressMadeSignal(25);
                    if (isCancelling()) {
                        return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                    }

                    // Remove duplicated points after cap remeshing
                    vtkNew<vtkCleanPolyData> cleaner2;
                    cleaner2->SetInputData(remeshCaps->GetOutput());
                    cleaner2->Update();

                    // Extract remeshed caps
                    vtkNew<vtkThreshold> remeshedCapCells;
                    remeshedCapCells->SetInputData(cleaner2->GetOutput());
                    remeshedCapCells->SetInputArrayToProcess(0, 0, 0, vtkDataObject::FIELD_ASSOCIATION_CELLS, "Face IDs");
                    remeshedCapCells->ThresholdByUpper(FirstCapId);
                    remeshedCapCells->Update();

                    vtkNew<vtkGeometryFilter> meshToSurface;
                    meshToSurface->SetInputData(cleaner2->GetOutput());
                    meshToSurface->Update();

                    vtkSmartPointer<vtkUnstructuredGrid> result = runTetGen(meshToSurface->GetOutput());
                    if (result->GetNumberOfCells() == 0) {
                        return std::make_pair(State_Failed, std::string("TetGen failed, see log for details."));
                    }

                    progressMadeSignal(25);
                    if (isCancelling()) {
                        return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                    }
                    MITK_INFO << "Assembling final result";

                    vtkNew<vtkAppendFilter> appender;
                    appender->SetMergePoints(1);
                    appender->AddInputData(result);
                    appender->AddInputData(blVolumeCells->GetOutput());
                    appender->AddInputData(blSidewallCells->GetOutput());
                    appender->AddInputData(remeshedCapCells->GetOutput());
                    appender->AddInputData(surfaceToMesh->GetOutput()); // For surface IDs
                    appender->Update();

                    if (!appender->GetOutput()->GetCellData()->GetArray("Face IDs")) {
                        return std::make_pair(State_Failed, std::string("Please reloft/reblend before meshing."));
                    }

                    auto restoreMoreFlowFaceIds = [&](vtkUnstructuredGrid* ug) {
                        vtkDataArray* faceIdArray = ug->GetCellData()->GetArray("Face IDs");
                        for (int faceId = 0; faceId < ug->GetNumberOfCells(); ++faceId) {
                            vtkSmartPointer<vtkCell> cell = ug->GetCell(faceId);
                            double cellId = faceIdArray->GetTuple1(faceId);

                            if (cell->GetCellDimension() == 3) {
                                faceIdArray->SetTuple1(faceId, -1);
                                continue;
                            }

                            if (cellId >= FirstCapId || cellId == SidewallId) {
                                for (const auto& faceIdToPlaneInfo : flowFacePlanes) {
                                    if (faceIdToPlaneInfo.second.inPlane(cell->GetPoints()->GetPoint(0))) {
                                        faceIdArray->SetTuple1(faceId, faceIdToPlaneInfo.first);
                                        break;
                                    }
                                }
                            }
                        }
                    };
                    restoreMoreFlowFaceIds(appender->GetOutput());

                    writeVTU(appender->GetOutput(), "07 - full mesh");

                    vtkNew<vtkvmtkUnstructuredGridTetraFilter> tetrahedralizer;
                    tetrahedralizer->SetInputData(appender->GetOutput());
                    tetrahedralizer->Update();

                    writeVTU(tetrahedralizer->GetOutput(), "08 - tetrahedralized");
                    finalResult = tetrahedralizer->GetOutput();

                    progressMadeSignal(10);
                }
            }

            auto ug = mitk::UnstructuredGrid::New();
            ug->SetVtkUnstructuredGrid(finalResult);
            auto result = MeshData::New();
            result->setFaceIdentifierMap(brep->getFaceIdentifierMap());
            result->setUnstructuredGrid(ug, true);
            this->setResult(result.GetPointer());

            progressMadeSignal(1);

            return std::make_tuple(State_Finished, std::string());
		}

	};

	class DiscreteMeshingTask : public crimson::MeshingTask
	{
	public:
		DiscreteMeshingTask(const mitk::BaseData::Pointer& solid, const IMeshingKernel::GlobalMeshingParameters& params,
			const std::map<FaceIdentifier, IMeshingKernel::LocalMeshingParameters>& localParams,
			const std::map<VesselPathAbstractData::VesselPathUIDType, std::string>& vesselUIDtoNameMap)
			:MeshingTask(solid, params, localParams, vesselUIDtoNameMap) {}

		~DiscreteMeshingTask(){}

		std::tuple<State, std::string> runTask() override
		{
			stepsAddedSignal(1);
        }
	};

    std::shared_ptr<async::TaskWithResult<mitk::BaseData::Pointer>>
		IMeshingKernel::createMeshSolidTask(const mitk::BaseData::Pointer& solid, const GlobalMeshingParameters& params,
		const std::map<FaceIdentifier, LocalMeshingParameters>& localParams,
		const std::map<VesselPathAbstractData::VesselPathUIDType, std::string>& vesselUIDtoNameMap)
	{
		return std::static_pointer_cast<crimson::async::TaskWithResult<mitk::BaseData::Pointer>>(
			std::make_shared<OCCMeshingTask>(solid, params, localParams, vesselUIDtoNameMap));
	}

    class AdaptMeshTask : public crimson::async::TaskWithResult<mitk::BaseData::Pointer>
	{
		using GradientType = Eigen::Vector3d;
		using HessianType = Eigen::Matrix<double, 6, 1>;

	public:
		AdaptMeshTask(const mitk::BaseData::Pointer& originalMesh, double factor, double hmin, double hmax,
			const std::string& errorIndicatorArrayName)
			: originalMesh(dynamic_cast<MeshData*>(originalMesh.GetPointer()))
			, factor(factor)
			, hmax(hmax)
			, hmin(hmin)
			, errorIndicatorArrayName(errorIndicatorArrayName)
		{
		}
		~AdaptMeshTask() { }

		void cancel() override
		{
			crimson::async::TaskWithResult<mitk::BaseData::Pointer>::cancel();
		}

		std::tuple<State, std::string> runTask() override
		{
            stepsAddedSignal(6);

			if (originalMesh.IsNull()) {
				return std::make_tuple(State_Failed,
					std::string("Original mesh was not created with the meshing kernel other than ."));
			}

			try {
                MITK_INFO << "Computing size field";
                setSizeFieldUsingHessians();

                progressMadeSignal(1);
                if (isCancelling()) {
                    return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                }

                auto runTetGen = [&](vtkUnstructuredGridBase* input) {
                    // *******
                    vtkNew<crimsonTetGenWrapper> mesher;
                    mesher->SetInputData(input);
                    mesher->SetCellEntityIdsArrayName("Face IDs");
                    mesher->SetSizingFunctionArrayName("Target size");
                    mesher->SetUseSizingFunction(1);
                    mesher->SetOrder(1);
                    mesher->SetQuality(1);
                    mesher->SetRefine(1);
                    mesher->SetCoarsen(0); // TetGen sometimes crashes when coarsening the outer surface.
                    mesher->SetNoBoundarySplit(0);
                    mesher->SetRemoveSliver(0);
                    mesher->SetOutputSurfaceElements(1);
                    mesher->SetOutputVolumeElements(1);
                    mesher->SetVerbose(1);
                    mesher->Update();

                    return vtkSmartPointer<vtkUnstructuredGrid>(mesher->GetOutput());
                };

                progressMadeSignal(1);
                if (isCancelling()) {
                    return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                }
                MITK_INFO << "Adapting mesh";

                auto newUnstructuredGrid = runTetGen(originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid());
                originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid()->GetPointData()->RemoveArray("Target size");

                if (newUnstructuredGrid->GetNumberOfCells() == 0) {
                    return std::make_pair(State_Failed, std::string("TetGen failed, see log for details."));
                }

				int timeStep = 0;
				originalMesh->GetPropertyList()->GetIntProperty("timeStep", timeStep);

                progressMadeSignal(1);
                if (isCancelling()) {
                    return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                }
                MITK_INFO << "Transferring the solution";

				// Transfer solution
				vtkUnstructuredGridBase* originalUnstructuredGrid =
					originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid();

				vtkNew<vtkProbeFilter> solutionTransferFilter;
				solutionTransferFilter->SetSourceData(originalUnstructuredGrid);
				solutionTransferFilter->SetInputData(newUnstructuredGrid);
				solutionTransferFilter->Update();

                progressMadeSignal(1);
                if (isCancelling()) {
                    return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                }
                MITK_INFO << "Transferring the solution for nodes outside original mesh domain";

				// For nodes that were snapped to the model surface that are outside of original mesh - just find closest points
				vtkPointData* transferredPointData = solutionTransferFilter->GetOutput()->GetPointData();
				vtkDataArray* validPointMaskArray =
					transferredPointData->GetArray(solutionTransferFilter->GetValidPointMaskArrayName());
				for (int i = 0; i < newUnstructuredGrid->GetNumberOfPoints(); ++i) {
					if (validPointMaskArray->GetComponent(i, 0) != 0) {
						continue;
					}
					// Failed the solution transfer - find the closest point
					int closestPointId = originalUnstructuredGrid->FindPoint(newUnstructuredGrid->GetPoint(i));

					for (int arrayIndex = 0; arrayIndex < originalUnstructuredGrid->GetPointData()->GetNumberOfArrays();
						++arrayIndex) {
						transferredPointData->GetArray(arrayIndex)
							->SetTuple(i, originalUnstructuredGrid->GetPointData()->GetArray(arrayIndex)->GetTuple(closestPointId));
					}
				}
				transferredPointData->RemoveArray(solutionTransferFilter->GetValidPointMaskArrayName());

                for (int i = 0; i < originalUnstructuredGrid->GetCellData()->GetNumberOfArrays(); ++i) {
                    transferredPointData->RemoveArray(originalUnstructuredGrid->GetCellData()->GetArrayName(i));
                }

                newUnstructuredGrid->GetPointData()->ShallowCopy(transferredPointData);

                progressMadeSignal(1);
                if (isCancelling()) {
                    return std::make_tuple(State_Cancelled, std::string("Operation cancelled."));
                }
                MITK_INFO << "Transferring original face identifiers";

                vtkSmartPointer<vtkPolyData> pd = originalMesh->getSurfaceRepresentation()->GetVtkPolyData();

                vtkDataArray* outArray = newUnstructuredGrid->GetCellData()->GetArray("Face IDs");

                for (int i = 0; i < newUnstructuredGrid->GetNumberOfCells(); ++i) {
                    if (newUnstructuredGrid->GetCellType(i) == VTK_TETRA) {
                        outArray->SetComponent(i, 0, -1);
                    } else if (outArray->GetComponent(i, 0) == -1) {
                        outArray->SetComponent(i, 0, 0);
                    }
                }

                // Finally, create the output MITK object
                auto ug = mitk::UnstructuredGrid::New();
                ug->SetVtkUnstructuredGrid(newUnstructuredGrid);
                auto result = MeshData::New();
                result->setFaceIdentifierMap(originalMesh->getFaceIdentifierMap());
                result->setUnstructuredGrid(ug, true);
                this->setResult(result.GetPointer());

                progressMadeSignal(1);

				MITK_INFO << "Adaptation complete";

				setResult(result.GetPointer());
				return std::make_tuple(State_Finished, std::string("Adaptation complete."));
			}
			catch (...) {
				return std::make_tuple(State_Failed, std::string("Unhandled exception caught"));
			}
		}

		// Build a linear system using the coordinates of the nodes
		Eigen::Matrix4d buildSystem(vtkCell* cell)
		{
            auto ug = originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid();
            Eigen::Matrix4d m;

			for (int i = 0; i < 4; ++i) {
				double X[3];
                ug->GetPoint(cell->GetPointId(i), X);

				m(i, 0) = 1;
				for (int j = 0; j < 3; ++j) {
					m(i, j + 1) = X[j];
				}
			}

			return m;
		}

		// reconstruct the element gradient
		GradientType elementGradient(vtkCell* tet)
		{
			// build the linear system
			Eigen::Matrix4d matrix = buildSystem(tet);

			// get the field vals
			Eigen::Vector4d rhs;
			auto errorIndicatorData =
				static_cast<vtkDoubleArray*>(originalMesh->getPointData()->GetArray(errorIndicatorArrayName.c_str()));
			for (int i = 0; i < 4; i++) {
				rhs(i) = errorIndicatorData->GetComponent(tet->GetPointId(i), 0);
			}

			return matrix.colPivHouseholderQr()
				.solve(rhs)
				.tail<3>(); // Solve the system and take the solution from the last 3 elements
		}

		// compute the element gradient
		HessianType elementHessian(vtkCell* tet)
		{
			// build the linear system
			Eigen::Matrix4d matrix = buildSystem(tet);

			auto solverMethod = matrix.colPivHouseholderQr();

			// get the field vals
			Eigen::Vector4d hessianComponents[3];
            for (int gradientCoord = 0; gradientCoord < 3; ++gradientCoord) {
				Eigen::Vector4d rhs;
				for (int i = 0; i < 4; i++) {
					rhs(i) = nodalGradients[tet->GetPointId(i)](gradientCoord); // speed component
				}
				hessianComponents[gradientCoord] = solverMethod.solve(rhs);
			}

			HessianType hessian;
			hessian(0) = hessianComponents[0](1); // xx
			hessian(1) = hessianComponents[0](2); // xy
			hessian(2) = hessianComponents[0](3); // xz
			hessian(3) = hessianComponents[1](2); // yy
			hessian(4) = hessianComponents[1](3); // yz
			hessian(5) = hessianComponents[2](3); // zz

			return hessian;
		}

		// Compute values for all elements in the mesh
		template <typename DataType, typename Functor>
		void computeElementValues(std::vector<DataType>& outputElementValues, Functor&& computeFunction)
		{
			outputElementValues.resize(originalMesh->getNElements());
            auto ug = originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid();
            for (int i = 0; i < originalMesh->getNElements(); ++i) {
                vtkSmartPointer<vtkCell> tet = ug->GetCell(i);
                Expects(tet->GetCellType() == VTK_TETRA);

                outputElementValues[i] = computeFunction(tet.GetPointer());
            }
		}

		// Compute values at nodes using element values using volume-wieghted averaging
		template <typename DataType>
		void computeNodalValuesFromElementValues(std::vector<DataType>& outputNodalValues,
			const std::vector<DataType>& elementValues)
		{
			outputNodalValues.resize(originalMesh->getNNodes());
            auto ug = originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid();
            Expects(ug->GetNumberOfPoints() == originalMesh->getNNodes());

			// loop over vertices and get patch of elements
            vtkNew<vtkIdList> pointIdList;
            pointIdList->SetNumberOfIds(1);

            vtkNew<vtkIdList> neighborCells;
            for (int vertexId = 0; vertexId < ug->GetNumberOfPoints(); ++vertexId) {
                pointIdList->SetId(0, vertexId);
                ug->GetCellNeighbors(-1, pointIdList.Get(), neighborCells.Get());

                double patchVolume = 0;

                DataType patchValue;
                patchValue.fill(0);

                // loop over elements and recover gradient at vertex
                for (int cellId = 0; cellId < neighborCells->GetNumberOfIds(); cellId++) {
                    vtkSmartPointer<vtkCell> cell = ug->GetCell(neighborCells->GetId(cellId));
                    if (cell->GetCellType() != VTK_TETRA) {
                        continue;
                    }

                    double tetVolume = vtkMeshQuality::TetVolume(cell);
                    patchVolume += tetVolume;

                    // get element gradient or hessian for each element in the patch at vertex
                    patchValue += elementValues[neighborCells->GetId(cellId)] * tetVolume;
                }

                // attach the recovered gradient to the vertex
                outputNodalValues[vertexId] = patchValue / patchVolume;
            }
		}

		// simple average over a patch surrounding the vertex
		void smoothHessians()
		{
			std::vector<HessianType> smoothedHessians(nodalHessians.size());

			// keep the vals in memory before finally setting them
            auto ug = originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid();

            // loop over vertices and get patch of elements
            vtkNew<vtkIdList> pointIdList;
            pointIdList->SetNumberOfIds(1);

            vtkNew<vtkIdList> neighborCells;
            for (int vertexId = 0; vertexId < ug->GetNumberOfPoints(); ++vertexId) {
                pointIdList->SetId(0, vertexId);
                ug->GetCellNeighbors(-1, pointIdList.Get(), neighborCells.Get());

                std::unordered_set<vtkIdType> allIds;
                std::unordered_set<vtkIdType> surfaceIds;

                allIds.insert(vertexId);
                for (int cellId = 0; cellId < neighborCells->GetNumberOfIds(); cellId++) {
                    vtkSmartPointer<vtkCell> cell = ug->GetCell(neighborCells->GetId(cellId));
                    for (int neighVertexId = 0; neighVertexId < cell->GetNumberOfPoints(); ++neighVertexId) {
                        (cell->GetCellType() == VTK_TETRA ? allIds : surfaceIds).insert(cell->GetPointId(neighVertexId));
                    }
                }

                for (vtkIdType id : surfaceIds) {
                    allIds.erase(id);
                }

                if (allIds.empty()) {
// 					MITK_WARN << "\nerror in SmoothHessians: there is a boundary vertex whose\n"
// 						<< "neighbors are exclusively classified on model faces/edges/vertices\n"
// 						<< "and NOT in the interior.\n";
					smoothedHessians[vertexId] = nodalHessians[vertexId];
					continue;
				}

                HessianType averageHessian;
                averageHessian.fill(0);

                for (vtkIdType id : allIds) {
                    averageHessian += nodalHessians[id];
                }

                smoothedHessians[vertexId] = averageHessian / allIds.size();
			}

			nodalHessians = smoothedHessians;
		}

		// relative interpolation error along an edge
		double E_error(vtkIdType v1, vtkIdType v2, const Eigen::Matrix3d& H)
		{
			Eigen::Vector3d vertexCoords[2];

            auto ug = originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid();

            ug->GetPoint(v1, &vertexCoords[0](0));
            ug->GetPoint(v2, &vertexCoords[1](0));

			Eigen::Vector3d edgeVector = vertexCoords[1] - vertexCoords[0];

			double localError = 0;
			for (int i = 0; i < 3; i++) {
				for (int j = 0; j < 3; j++) {
					localError += H(i, j) * edgeVector(i) * edgeVector(j);
				}
			}

			return abs(localError);
		}

		// max relative interpolation error at a vertex
		double maxLocalError(vtkIdType vertexId, const Eigen::Matrix3d& H)
		{
			double maxLocE = 0;

            auto ug = originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid();

            vtkNew<vtkIdList> pointIdList;
            pointIdList->SetNumberOfIds(1);

            vtkNew<vtkIdList> neighborCells;

            pointIdList->SetId(0, vertexId);
            ug->GetCellNeighbors(-1, pointIdList.Get(), neighborCells.Get());

            std::unordered_set<vtkIdType> allIds;
            for (int cellId = 0; cellId < neighborCells->GetNumberOfIds(); cellId++) {
                vtkSmartPointer<vtkCell> cell = ug->GetCell(neighborCells->GetId(cellId));
                for (int otherVertexId = 0; otherVertexId < cell->GetNumberOfPoints(); ++otherVertexId) {
                    allIds.insert(cell->GetPointId(otherVertexId));
                }
            }
            allIds.erase(vertexId);

			for (vtkIdType otherId : allIds) {
				maxLocE = std::max(maxLocE, E_error(vertexId, otherId, H));
			}

			return maxLocE;
		}

		void setSizeFieldUsingHessians()
		{
			MITK_INFO << "Computing per-element Hessians";
			computeHessians();

			double tol = 1.e-12;
			double totalError = 0;                                     // total error for all vertices
			double localErrorMax = 0.;                                 // max local error
			double localErrorMin = std::numeric_limits<double>::max(); // min local error

			int hminCount = 0;
			int hmaxCount = 0;
			int bothHminHmaxCount = 0;
			int bdryNumNodes = 0;

			std::vector<std::tuple<Eigen::Vector3d, Eigen::Matrix3d>> eigenValuesVectors(nodalHessians.size());

            auto ug = originalMesh->getUnstructuredGridRepresentation()->GetVtkUnstructuredGrid();

            vtkNew<vtkDoubleArray> arr;
            arr->SetNumberOfTuples(ug->GetNumberOfPoints());
            arr->SetName("Target size");

            for (int vertexIndex = 0; vertexIndex < ug->GetNumberOfPoints(); ++vertexIndex) {
                // Hessian in the symmetric matrix form
                Eigen::Matrix3d hessian;

                hessian(0, 0) = nodalHessians[vertexIndex](0);
                hessian(0, 1) = hessian(1, 0) = nodalHessians[vertexIndex](1);
                hessian(0, 2) = hessian(2, 0) = nodalHessians[vertexIndex](2);
                hessian(1, 1) = nodalHessians[vertexIndex](3);
                hessian(1, 2) = hessian(2, 1) = nodalHessians[vertexIndex](4);
                hessian(2, 2) = nodalHessians[vertexIndex](5);

                // compute eigen values and eigen vectors
                Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(hessian);

                if (eigensolver.info() != Eigen::Success) {
                    MITK_WARN << "Failed to find eigenvectors for the hessian for node " << vertexIndex;
                    continue;
                }

                auto eigenValuesAbsolute = eigensolver.eigenvalues().cwiseAbs();

                if (eigenValuesAbsolute.maxCoeff() < tol) {
                    eigenValuesVectors[vertexIndex] = std::make_tuple(eigenValuesAbsolute, Eigen::Matrix3d::Zero());

                    MITK_WARN << "Zero maximum eigenvalue for node " << vertexIndex;
                    continue;
                }

                eigenValuesVectors[vertexIndex] = std::make_tuple(eigenValuesAbsolute, eigensolver.eigenvectors());

                // estimate relative interpolation error
                // needed for scaling metric field (mesh size field)
                // to get an idea refer Appendix A in Li's thesis
                double localError = maxLocalError(vertexIndex, hessian);
                totalError += localError;
                localErrorMax = std::max(localErrorMax, localError);
                localErrorMin = std::min(localErrorMin, localError);
            }

            originalMesh->getPointData()->AddArray(arr.GetPointer());

			double averageError = totalError / originalMesh->getNNodes();
			MITK_INFO << "Info on relative interpolation error: ";
			MITK_INFO << "   total: " << totalError;
			MITK_INFO << "   mean:  " << averageError;
			MITK_INFO << "   max local: " << localErrorMax;
			MITK_INFO << "   min local: " << localErrorMin;

			double targetLocalError = averageError * factor;

			MITK_INFO << " towards uniform local error distribution of " << targetLocalError;
			MITK_INFO << "   with max edge length=" << hmax << " min edge length=" << hmin;

            for (int vertexIndex = 0; vertexIndex < ug->GetNumberOfPoints(); ++vertexIndex) {
				double tol2 = 0.01 * hmax;
				double tol3 = 0.01 * hmin;

				Eigen::Vector3d eigenValues;
				Eigen::Matrix3d eigenVectors;
				std::tie(eigenValues, eigenVectors) = eigenValuesVectors[vertexIndex];

				double directionalSize[3];

				for (int j = 0; j < 3; j++) {
					if (eigenValues(j) < tol)
						directionalSize[j] = hmax;
					else {
						directionalSize[j] = std::max(hmin, std::min(hmax, sqrt(targetLocalError / eigenValues(j))));
					}
				}

                arr->SetTuple1(vertexIndex, *std::min_element(directionalSize, directionalSize + 3));

				bool foundHmin = false;
				bool foundHmax = false;
				for (int j = 0; j < 3; j++) {
					if (abs(directionalSize[j] - hmax) <= tol2) {
						foundHmax = true;
					}
					if (abs(directionalSize[j] - hmin) <= tol3) {
						foundHmin = true;
					}
				}
				if (foundHmin) {
					hminCount++;
				}
				if (foundHmax) {
					hmaxCount++;
				}
				if (foundHmin && foundHmax) {
					bothHminHmaxCount++;
				}
			}

			MITK_INFO << "Nodes with hmin into effect : " << hminCount << endl;
			MITK_INFO << "Nodes with hmax into effect : " << hmaxCount << endl;
			MITK_INFO << "Nodes with both hmin/hmax into effect : " << bothHminHmaxCount << endl;
			MITK_INFO << "(No nodes are ignored in boundary layer for CGALVMTK mesher)" << endl << endl;

			nodalHessians.clear();
		}

		void computeHessians()
		{
			std::vector<GradientType> elementGradients;
			computeElementValues<GradientType>(elementGradients, [this](vtkCell* r) { return elementGradient(r); });
			computeNodalValuesFromElementValues(nodalGradients, elementGradients);
			elementGradients.clear(); // Element gradients are no longer needed

            // vtkNew<vtkDoubleArray> arr;
            // arr->SetNumberOfComponents(3);
            // arr->SetNumberOfTuples(nodalGradients.size());
            // arr->SetName("grad");
            //
            // for (int i = 0; i < nodalGradients.size(); ++i) {
            //     for (int j = 0; j < 3; ++j) {
            //         arr->SetComponent(i, j, nodalGradients[i](j));
            //     }
            // }
            // originalMesh->getPointData()->AddArray(arr.GetPointer());

			std::vector<HessianType> elementHessians;
			computeElementValues<HessianType>(elementHessians, [this](vtkCell* r) { return elementHessian(r); });
			computeNodalValuesFromElementValues(nodalHessians, elementHessians);
			elementHessians.clear(); // Element hessians are no longer needed
			nodalGradients.clear();  // Nodal gradients are no longer needed

			smoothHessians();

			// Now the nodal hessians are ready to use in the adaptation procedure
		}
	private:
		MeshData::Pointer originalMesh;

		double factor;
		double hmax;
		double hmin;
		std::string errorIndicatorArrayName;

		std::vector<GradientType> nodalGradients;
		std::vector<HessianType> nodalHessians;
	};

    std::shared_ptr<async::TaskWithResult<mitk::BaseData::Pointer>>
		IMeshingKernel::createAdaptMeshTask(const mitk::BaseData::Pointer& originalMesh, double factor, double hmin, double hmax,
		const std::string& errorIndicatorArrayName)
	{
    	return std::static_pointer_cast<crimson::async::TaskWithResult<mitk::BaseData::Pointer>>(
		    std::make_shared<AdaptMeshTask>(originalMesh, factor, hmin, hmax, errorIndicatorArrayName));
	}

} // namespace crimson


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This example shows how to classify an image into one of the 1000 imagenet
    categories using the deep learning tools from the dlib C++ Library.  We will
    use the pretrained ResNet34 model available on the dlib website.

    The ResNet34 architecture is from the paper Deep Residual Learning for Image
    Recognition by He, Zhang, Ren, and Sun.  The model file that comes with dlib
    was trained using the dnn_imagenet_train_ex.cpp program on a Titan X for
    about 2 weeks.  This pretrained model has a top5 error of 7.572% on the 2012
    imagenet validation dataset.

    For an introduction to dlib's DNN module read the dnn_introduction_ex.cpp and
    dnn_introduction2_ex.cpp example programs.

    Finally, these tools will use CUDA and cuDNN to drastically accelerate
    network training and testing.  CMake should automatically find them if they
    are installed and configure things appropriately.  If not, the program will
    still run but will be much slower to execute.
*/

#include <dlib/dnn.h>
#include <iostream>
#include <dlib/data_io.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_transforms.h>

using namespace std;
using namespace dlib;

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

// This block of statements defines the resnet-34 network

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

template <typename SUBNET> using level1 = ares<512,ares<512,ares_down<512,SUBNET>>>;
template <typename SUBNET> using level2 = ares<256,ares<256,ares<256,ares<256,ares<256,ares_down<256,SUBNET>>>>>>;
template <typename SUBNET> using level3 = ares<128,ares<128,ares<128,ares_down<128,SUBNET>>>>;
template <typename SUBNET> using level4 = ares<64,ares<64,ares<64,SUBNET>>>;

using anet_type = loss_multiclass_log<fc<1000,avg_pool_everything<
                            level1<
                            level2<
                            level3<
                            level4<
                            max_pool<3,3,2,2,relu<affine<con<64,7,7,2,2,
                            input_rgb_image_sized<227>
                            >>>>>>>>>>>;

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

rectangle make_random_cropping_rect_resnet(
    const matrix<rgb_pixel>& img,
    dlib::rand& rnd
)
{
    // figure out what rectangle we want to crop from the image
    double mins = 0.466666666, maxs = 0.875;
    auto scale = mins + rnd.get_random_double()*(maxs-mins);
    auto size = scale*std::min(img.nr(), img.nc());
    rectangle rect(size, size);
    // randomly shift the box around
    point offset(rnd.get_random_32bit_number()%(img.nc()-rect.width()),
                 rnd.get_random_32bit_number()%(img.nr()-rect.height()));
    return move_rect(rect, offset);
}

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

void randomly_crop_images (
    const matrix<rgb_pixel>& img,
    dlib::array<matrix<rgb_pixel>>& crops,
    dlib::rand& rnd,
    long num_crops
)
{
    std::vector<chip_details> dets;
    for (long i = 0; i < num_crops; ++i)
    {
        auto rect = make_random_cropping_rect_resnet(img, rnd);
        dets.push_back(chip_details(rect, chip_dims(227,227)));
    }

    extract_image_chips(img, dets, crops);

    for (auto&& img : crops)
    {
        // Also randomly flip the image
        if (rnd.get_random_double() > 0.5)
            img = fliplr(img);

        // And then randomly adjust the colors.
        apply_random_color_offset(img, rnd);
    }
}

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

int main(int argc, char** argv) try
{
    if (argc == 1)
    {
        cout << "Give this program image files as command line arguments.\n" << endl;
        cout << "You will also need a copy of the file resnet34_1000_imagenet_classifier.dnn " << endl;
        cout << "available at http://dlib.net/files/resnet34_1000_imagenet_classifier.dnn.bz2" << endl;
        cout << endl;
        return 1;
    }

    std::vector<string> labels;
    anet_type net;
    deserialize("resnet34_1000_imagenet_classifier.dnn") >> net >> labels;

    // Make a network with softmax as the final layer.  We don't have to do this
    // if we just want to output the single best prediction, since the anet_type
    // already does this.  But if we instead want to get the probability of each
    // class as output we need to replace the last layer of the network with a
    // softmax layer, which we do as follows:
    softmax<anet_type::subnet_type> snet;
    snet.subnet() = net.subnet();

    dlib::array<matrix<rgb_pixel>> images;
    matrix<rgb_pixel> img, crop;

    dlib::rand rnd;
    image_window win;

    // Read images from the command prompt and print the top 5 best labels for each.
    for (int i = 1; i < argc; ++i)
    {
        load_image(img, argv[i]);
        const int num_crops = 16;
        // Grab 16 random crops from the image.  We will run all of them through the
        // network and average the results.
        randomly_crop_images(img, images, rnd, num_crops);
        // p(i) == the probability the image contains object of class i.
        matrix<float,1,1000> p = sum_rows(mat(snet(images.begin(), images.end())))/num_crops;

        win.set_image(img);
        // Print the 5 most probable labels
        for (int k = 0; k < 5; ++k)
        {
            unsigned long predicted_label = index_of_max(p);
            cout << p(predicted_label) << ": " << labels[predicted_label] << endl;
            p(predicted_label) = 0;
        }

        cout << "Hit enter to process the next image";
        cin.get();
    }

}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MP_CPP_INT_CORE_HPP
#define BOOST_MP_CPP_INT_CORE_HPP

#include <boost/integer.hpp>
#include <boost/integer_traits.hpp>
#include <boost/assert.hpp>

namespace nil {
    namespace crypto3 {
        namespace multiprecision {

            namespace detail {

                //
                // These traits calculate the largest type in the list
                // [unsigned] boost::long_long_type, long, int, which has the specified number
                // of bits.  Note that intN_t and boost::int_t<N> find the first
                // member of the above list, not the last.  We want the last in the
                // list to ensure that mixed arithmetic operations are as efficient
                // as possible.
                //
                template<unsigned N>
                struct largest_signed_type {
                    using type = typename std::conditional<
                        1 + std::numeric_limits<boost::long_long_type>::digits == N, boost::long_long_type,
                        typename std::conditional<
                            1 + std::numeric_limits<long>::digits == N, long,
                            typename std::conditional<1 + std::numeric_limits<int>::digits == N, int,
                                                      typename boost::int_t<N>::exact>::type>::type>::type;
                };

                template<unsigned N>
                struct largest_unsigned_type {
                    using type = typename std::conditional<
                        std::numeric_limits<boost::ulong_long_type>::digits == N, boost::ulong_long_type,
                        typename std::conditional<
                            std::numeric_limits<unsigned long>::digits == N, unsigned long,
                            typename std::conditional<std::numeric_limits<unsigned int>::digits == N, unsigned int,
                                                      typename boost::uint_t<N>::exact>::type>::type>::type;
                };

            }    // namespace detail

#if defined(BOOST_HAS_INT128)

            using limb_type = detail::largest_unsigned_type<64>::type;
            using signed_limb_type = detail::largest_signed_type<64>::type;
            using double_limb_type = boost::uint128_type;
            using signed_double_limb_type = boost::int128_type;
            constexpr const limb_type max_block_10 = 1000000000000000000uLL;
            constexpr const limb_type digits_per_block_10 = 18;

            inline BOOST_MP_CXX14_CONSTEXPR limb_type block_multiplier(unsigned count) {
                constexpr const limb_type values[digits_per_block_10] = {10,
                                                                         100,
                                                                         1000,
                                                                         10000,
                                                                         100000,
                                                                         1000000,
                                                                         10000000,
                                                                         100000000,
                                                                         1000000000,
                                                                         10000000000,
                                                                         100000000000,
                                                                         1000000000000,
                                                                         10000000000000,
                                                                         100000000000000,
                                                                         1000000000000000,
                                                                         10000000000000000,
                                                                         100000000000000000,
                                                                         1000000000000000000};
                BOOST_ASSERT(count < digits_per_block_10);
                return values[count];
            }

// Can't do formatted IO on an __int128
#define BOOST_MP_NO_DOUBLE_LIMB_TYPE_IO

            // Need to specialise integer_traits for __int128 as it's not a normal native type:
        }
    }
}    // namespace nil::crypto3::multiprecision

namespace boost {
    template<>
    class integer_traits<nil::crypto3::multiprecision::double_limb_type>
        : public std::numeric_limits<nil::crypto3::multiprecision::double_limb_type>,
          public detail::integer_traits_base<nil::crypto3::multiprecision::double_limb_type, 0,
                                             ~static_cast<nil::crypto3::multiprecision::double_limb_type>(0)> { };
    template<>
    class integer_traits<nil::crypto3::multiprecision::signed_double_limb_type>
        : public std::numeric_limits<nil::crypto3::multiprecision::signed_double_limb_type>,
          public detail::integer_traits_base<
              nil::crypto3::multiprecision::signed_double_limb_type,
              static_cast<nil::crypto3::multiprecision::signed_double_limb_type>(
                  (static_cast<nil::crypto3::multiprecision::double_limb_type>(1) << 127)),
              static_cast<nil::crypto3::multiprecision::signed_double_limb_type>(
                  ((~static_cast<nil::crypto3::multiprecision::double_limb_type>(0)) >> 1))> { };
}    // namespace boost

namespace nil {
    namespace crypto3 {
        namespace multiprecision {

#else

            using limb_type = detail::largest_unsigned_type<32>::type;
            using signed_limb_type = detail::largest_signed_type<32>::type;
            using double_limb_type = detail::largest_unsigned_type<64>::type;
            using signed_double_limb_type = detail::largest_signed_type<64>::type;
            constexpr const limb_type max_block_10 = 1000000000;
            constexpr const limb_type digits_per_block_10 = 9;

            inline limb_type block_multiplier(unsigned count) {
                constexpr const limb_type values[digits_per_block_10] = {
                    10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000};
                BOOST_ASSERT(count < digits_per_block_10);
                return values[count];
            }

#endif

            constexpr const unsigned bits_per_limb = sizeof(limb_type) * CHAR_BIT;

            template<class T>
            inline BOOST_MP_CXX14_CONSTEXPR void minmax(const T& a, const T& b, T& aa, T& bb) {
                if (a < b) {
                    aa = a;
                    bb = b;
                } else {
                    aa = b;
                    bb = a;
                }
            }

            enum cpp_integer_type {
                signed_magnitude = 1,
                unsigned_magnitude = 0,
                signed_packed = 3,
                unsigned_packed = 2
            };

            enum cpp_int_check_type { checked = 1, unchecked = 0 };
        }    // namespace multiprecision
    }        // namespace crypto3
}    // namespace nil

#endif    // BOOST_MP_CPP_INT_CORE_HPP


// *****************************************************************************
//
// Copyright (c) 2014, Southwest Research Institute® (SwRI®)
// 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 Southwest Research Institute® (SwRI®) 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 <COPYRIGHT HOLDER> 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 <swri_transform_util/local_xy_util.h>

#include <cmath>

#include <boost/make_shared.hpp>

#include <tf/transform_datatypes.h>

#include <geographic_msgs/GeoPose.h>
#include <geometry_msgs/PoseStamped.h>
#include <gps_common/GPSFix.h>

#include <swri_math_util/constants.h>
#include <swri_math_util/trig_util.h>
#include <swri_transform_util/earth_constants.h>
#include <swri_transform_util/transform_util.h>

namespace swri_transform_util
{
  void LocalXyFromWgs84(
      double latitude,
      double longitude,
      double reference_latitude,
      double reference_longitude,
      double& x,
      double& y)
  {
    LocalXyWgs84Util local_xy(reference_latitude, reference_longitude);
    local_xy.ToLocalXy(latitude, longitude, x, y);
  }

  void Wgs84FromLocalXy(
      double x,
      double y,
      double reference_latitude,
      double reference_longitude,
      double& latitude,
      double& longitude)
  {
    LocalXyWgs84Util local_xy(reference_latitude, reference_longitude);
    local_xy.ToWgs84(x, y, latitude, longitude);
  }

  LocalXyWgs84Util::LocalXyWgs84Util(
      double reference_latitude,
      double reference_longitude,
      double reference_angle,
      double reference_altitude) :
    reference_latitude_(reference_latitude * swri_math_util::_deg_2_rad),
    reference_longitude_(reference_longitude * swri_math_util::_deg_2_rad),
    reference_angle_(reference_angle * swri_math_util::_deg_2_rad),
    reference_altitude_(reference_altitude),
    rho_lat_(0),
    rho_lon_(0),
    cos_angle_(0),
    sin_angle_(0),
    frame_("map"),
    initialized_(false)
  {
    Initialize();
  }

  LocalXyWgs84Util::LocalXyWgs84Util() :
    reference_latitude_(0),
    reference_longitude_(0),
    reference_angle_(0),
    reference_altitude_(0),
    rho_lat_(0),
    rho_lon_(0),
    cos_angle_(0),
    sin_angle_(0),
    frame_("map"),
    initialized_(false)
  {
    ros::NodeHandle node;

    ROS_INFO("Subscribing to /local_xy_origin");
    origin_sub_ = node.subscribe("/local_xy_origin", 1, &LocalXyWgs84Util::HandleOrigin, this);
  }

  void LocalXyWgs84Util::ResetInitialization()
  {
    if( initialized_ )
    {
      ros::NodeHandle node;
      origin_sub_ = node.subscribe("/local_xy_origin", 1, &LocalXyWgs84Util::HandleOrigin, this);
      initialized_ = false;
    }
  }

  void LocalXyWgs84Util::Initialize()
  {
    reference_angle_ = swri_math_util::WrapRadians(reference_angle_, 0);

    cos_angle_ = std::cos(reference_angle_);
    sin_angle_ = std::sin(reference_angle_);

    double depth = -reference_altitude_;

    double p = _earth_eccentricity * std::sin(reference_latitude_);
    p = 1.0 - p * p;

    double rho_e = _earth_equator_radius *
        (1.0 - _earth_eccentricity * _earth_eccentricity) / (std::sqrt(p) * p);
    double rho_n = _earth_equator_radius / std::sqrt(p);

    rho_lat_ = rho_e - depth;
    rho_lon_ = (rho_n - depth) * std::cos(reference_latitude_);
    initialized_ = true;
  }

  void LocalXyWgs84Util::HandleOrigin(const topic_tools::ShapeShifter::ConstPtr msg)
  {
    if (!initialized_)
    {
      ros::NodeHandle node;
      bool ignore_reference_angle = false;
      node.param("/local_xy_ignore_reference_angle", ignore_reference_angle, ignore_reference_angle);

      try
      {
        const gps_common::GPSFixConstPtr origin = msg->instantiate<gps_common::GPSFix>();
        reference_latitude_ = origin->latitude * swri_math_util::_deg_2_rad;
        reference_longitude_ = origin->longitude * swri_math_util::_deg_2_rad;
        reference_altitude_ = origin->altitude;

        if (!ignore_reference_angle)
        {
          reference_angle_ = ToYaw(origin->track);
        }

        std::string frame = origin->header.frame_id;

        if (frame.empty())
        {
          // If the origin has an empty frame id, look for a frame in
          // the global parameter /local_xy_frame.  This provides
          // compatibility with older bag files.
          node.param("/local_xy_frame", frame, frame_);
        }

        frame_ = frame;

        Initialize();
        origin_sub_.shutdown();
        return;
      }
      catch (...) {}

      try
      {
        const geometry_msgs::PoseStampedConstPtr origin = msg->instantiate<geometry_msgs::PoseStamped>();
        reference_latitude_ = origin->pose.position.y * swri_math_util::_deg_2_rad;
        reference_longitude_ = origin->pose.position.x * swri_math_util::_deg_2_rad;
        reference_altitude_ = origin->pose.position.z;

        if (!ignore_reference_angle)
        {
          reference_angle_ = tf::getYaw(origin->pose.orientation);
        }

        std::string frame = origin->header.frame_id;

        if (frame.empty())
        {
          // If the origin has an empty frame id, look for a frame in
          // the global parameter /local_xy_frame.  This provides
          // compatibility with older bag files.
          node.param("/local_xy_frame", frame, frame_);
        }

        frame_ = frame;

        Initialize();
        origin_sub_.shutdown();
        return;
      }
      catch (...) {}

      try
      {
        const geographic_msgs::GeoPoseConstPtr origin = msg->instantiate<geographic_msgs::GeoPose>();
        reference_latitude_ = origin->position.latitude * swri_math_util::_deg_2_rad;
        reference_longitude_ = origin->position.longitude * swri_math_util::_deg_2_rad;
        reference_altitude_ = origin->position.altitude;

        if (!ignore_reference_angle)
        {
          reference_angle_ = tf::getYaw(origin->orientation);
        }

        node.param("/local_xy_frame", frame_, frame_);

        Initialize();
        origin_sub_.shutdown();
        return;
      }
      catch (...) {}

      ROS_WARN("Invalid /local_xy topic type.");
    }
    origin_sub_.shutdown();
  }

  double LocalXyWgs84Util::ReferenceLongitude() const
  {
    return reference_longitude_ * swri_math_util::_rad_2_deg;
  }

  double LocalXyWgs84Util::ReferenceLatitude() const
  {
    return reference_latitude_ * swri_math_util::_rad_2_deg;
  }

  double LocalXyWgs84Util::ReferenceAngle() const
  {
    return reference_angle_ * swri_math_util::_rad_2_deg;
  }

  double LocalXyWgs84Util::ReferenceAltitude() const
  {
    return reference_altitude_;
  }

  bool LocalXyWgs84Util::ToLocalXy(
      double latitude,
      double longitude,
      double& x,
      double& y) const
  {
    if (initialized_)
    {
      if (latitude < -90.0 || latitude > 90.0)
      {
        return false;
      }

      if (longitude < -180.0 || longitude > 180.0)
      {
        return false;
      }

      double rlat = latitude * swri_math_util::_deg_2_rad;
      double rlon = longitude * swri_math_util::_deg_2_rad;
      double dLat = (rlat - reference_latitude_) * rho_lat_;
      double dLon = (rlon - reference_longitude_) * rho_lon_;

      x =  cos_angle_ * dLon + sin_angle_ * dLat;
      y = -sin_angle_ * dLon + cos_angle_ * dLat;

      return true;
    }

    return initialized_;
  }

  bool LocalXyWgs84Util::ToWgs84(
      double x,
      double y,
      double& latitude,
      double& longitude) const
  {
    if (initialized_)
    {
      double dLon = cos_angle_ * x - sin_angle_ * y;
      double dLat = sin_angle_ * x + cos_angle_ * y;
      double rlat = (dLat / rho_lat_) + reference_latitude_;
      double rlon = (dLon / rho_lon_) + reference_longitude_;

      latitude = rlat * swri_math_util::_rad_2_deg;
      longitude = rlon * swri_math_util::_rad_2_deg;
    }

    return initialized_;
  }
}


/*
 *  testConstitutiveTensorField.cpp
 *  Trogdor6
 *
 *  Created by Paul Hansen on 1/11/11.
 *  Copyright 2011 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestConstitutiveTensorField

#include <boost/test/unit_test.hpp>

#include "ConstitutiveTensorField.h"

using namespace std;
using namespace RLE;
using namespace YeeUtilities;

BOOST_AUTO_TEST_CASE( ConstructorEtCetera )
{
    Rect3i yeeCells(0,0,0,10,10,0);

    ConstitutiveTensorField c1;
//    BOOST_CHECK(c1.fieldOctant() == Octant::electric());
//    BOOST_CHECK(c1.fieldOctant(2,1) == Octant::electric()(2,1));
    for (int xyz = 0; xyz < 3; xyz++)
    {
//        BOOST_CHECK(c1.halfCellOffset(xyz,xyz) == eFieldOffset(xyz));
//        for (int abc = 0; abc < 3; abc++)
//        if (abc != xyz).
//            BOOST_CHECK(c1.halfCellOffset(xyz,abc) == Vector3i(0,0,0));
    }

    ConstitutiveTensorField c2(Octant::magnetic(), yeeCells);
//    BOOST_CHECK(c2.fieldOctant() == Octant::magnetic());
//    BOOST_CHECK(c2.fieldOctant(1,0) == Octant::magnetic()(1,0));
    for (int xyz = 0; xyz < 3; xyz++)
    {
//        BOOST_CHECK(c2.halfCellOffset(xyz,xyz) == hFieldOffset(xyz));
//        for (int abc = 0; abc < 3; abc++)
//        if (abc != xyz)
//            BOOST_CHECK(c2.halfCellOffset(xyz,abc) == Vector3i(1,1,1));
    }

    for (int xyz = 0; xyz < 3; xyz++)
    for (int abc = 0; abc < 3; abc++)
    {
        BOOST_CHECK_EQUAL(c2(xyz,abc).hasDimension(0), true);
        BOOST_CHECK_EQUAL(c2(xyz,abc).hasDimension(1), true);
        BOOST_CHECK_EQUAL(c2(xyz,abc).hasDimension(2), false);
    }
}

static Precision::RationalFunction ofOrder(int numerOrder, int denomOrder)
{
    Precision::RationalFunction r;
    for (int nn = 0; nn <= numerOrder; nn++)
        r.numerator().coefficient(nn, nn+1);
    for (int dd = 0; dd <= denomOrder; dd++)
        r.denominator().coefficient(dd, dd+1);
    return r;
}

BOOST_AUTO_TEST_CASE( SupportAndFilter )
{
    // Test each octant separately
    for (int xyz = 0; xyz < 3; xyz++)
    for (int abc = 0; abc < 3; abc++)
    {
        ConstitutiveTensorField c;
        c(xyz,abc).mark(0,0,0, ofOrder(1,0));
        c(xyz,abc).mark(0,1,0, ofOrder(1,1));
        c(xyz,abc).mark(0,2,0, ofOrder(0,1));

        DynamicRLE3<Precision::RationalFunction> order10, order11, order01;
        SupportRegion3 support10, support11, support01;

        order10.mark(0,0,0, ofOrder(1,0));
        order11.mark(0,1,0, ofOrder(1,1));
        order01.mark(0,2,0, ofOrder(0,1));
        support10 = SupportRegion3(order10);
        support11 = SupportRegion3(order11);
        support01 = SupportRegion3(order01);

        BOOST_CHECK(c.support(xyz,abc,1,0) == support10);
        BOOST_CHECK(c.support(xyz,abc,0,1) == support01);
        BOOST_CHECK(c.support(xyz,abc,1,1) == support11);

        for (int yy = 0; yy < 3; yy++)
        {
            BOOST_CHECK(c.filter(xyz,abc,1,0).at(0,yy,0) == order10.at(0,yy,0));
            BOOST_CHECK(c.filter(xyz,abc,0,1).at(0,yy,0) == order01.at(0,yy,0));
            BOOST_CHECK(c.filter(xyz,abc,1,1).at(0,yy,0) == order11.at(0,yy,0));
        }

        // Wildcards too.
        // Find all the cells with numerator order 1, and ANY denominator.
        // Find all the cells with denominator order 1, and ANY numerator.
        DynamicRLE3<Precision::RationalFunction> order1_, order_1;
        SupportRegion3 support1_, support_1;

        order1_.mark(0,0,0, ofOrder(1,0));
        order1_.mark(0,1,0, ofOrder(1,1));
        order_1.mark(0,1,0, ofOrder(1,1));
        order_1.mark(0,2,0, ofOrder(0,1));
        support1_ = SupportRegion3(order1_);
        support_1 = SupportRegion3(order_1);

        BOOST_CHECK(c.support(xyz,abc,1,-1) == support1_);
        BOOST_CHECK(c.support(xyz,abc,-1,1) == support_1);

        for (int yy = 0; yy < 3; yy++)
        {
            BOOST_CHECK(c.filter(xyz,abc,1,-1).at(0,yy,0) == order1_.at(0,yy,0));
            BOOST_CHECK(c.filter(xyz,abc,-1,1).at(0,yy,0) == order_1.at(0,yy,0));
        }
    }
}

BOOST_AUTO_TEST_CASE( OffDiagonalSupport_Simple )
{
//    Rect3i physicalYeeCells(-10,-10,0,10,10,0);
//    NodeExtents extents(physicalYeeCells, yeeToHalf(physicalYeeCells),
//        yeeToHalf(physicalYeeCells), yeeToHalf(physicalYeeCells));
//
//    ConstitutiveTensorField eps(YeeUtilities::Octant::electric(),
//        physicalYeeCells);
//    ConstitutiveTensorField mu(YeeUtilities::Octant::magnetic(),
//        physicalYeeCells);
//
//    DynamicRLE3<Precision::RationalFunction> gridMu(true,true,false),
//        gridEps(true,true,false);
//
//    gridMu.mark(-10,-10,0,10,10,0, 1.0);
//    gridEps.mark(-10,-10,0,10,10,0, 1.0);
//
//    for (int ii = 0; ii < 3; ii++)
//    for (int jj = 0; jj < 3; jj++)
//        eps(ii,jj) = gridEps;
//
//    for (int xyz = 0; xyz < 3; xyz++)
//        mu(xyz,xyz) = gridMu;
//
//    SupportRegion3 everywhere(gridMu);
//
//    BOOST_CHECK(eps.offDiagonalSupport() == everywhere);
//    BOOST_CHECK(mu.offDiagonalSupport().numRuns() == 0);
}

BOOST_AUTO_TEST_CASE( Extrude )
{
    BOOST_CHECK(true);
    // This functionality should actually move to OrientedVoxels, I think.
}

BOOST_AUTO_TEST_CASE( Copy )
{
    BOOST_CHECK(true);
    // Low priority now (Jan 11 2011).
}


#ifndef PLANNING_ASTAR_HPP
#define PLANNING_ASTAR_HPP

// Component
#include "Costmap.hpp"
#include "GraphNode.hpp"
#include "LocalPlannerConfig.hpp"
#include "LocalPlannerData.hpp"
#include "Point.hpp"

// Ros
#include <nav_msgs/Path.h>

// Libraries
#include <boost/cstdfloat.hpp>
#include <boost/shared_ptr.hpp>

// Standard
#include <cstdint>
#include <memory>
#include <queue>
#include <unordered_map>
#include <unordered_set>
#include <vector>

namespace local_planner
{

using float64_t = boost::float64_t; ///< Alias for 64 bit float

/// @brief Class to do graph search things
class AStar
{
public:
    /// @brief Default constructor
    /// @param cfg Local planner config
    AStar(const std::shared_ptr<LocalPlannerConfig>& cfg);

    /// @brief Default destructor
    ~AStar();

    /// @brief Main driving function of the solver
    /// @return `true` if udpate was successful and trajectory was found
    bool update();

    /// @brief Accessor for planned path
    /// @return Planned path
    /// @{
    const std::vector<Point>& getPath()    const noexcept {return m_path;}
    nav_msgs::Path::ConstPtr  getRosPath() const noexcept {return boost::make_shared<nav_msgs::Path>(m_ros_path);}
    /// @}

    /// @brief Mutator for the local planner data
    /// @param val The data
    /// @{
    void setLocalPlannerData(const LocalPlannerData& data) noexcept {m_data = data;}
    void setLocalPlannerData(LocalPlannerData&& data)      noexcept {m_data = data;}
    /// @}

private:
    /// @brief Resets the planner
    void resetPlanner() noexcept;

    /// @brief Initializes the planner
    /// @return `true` if successful
    bool initializePlanner() noexcept;

    /// @brief Plans a trajectory
    /// @return `true` if successful
    bool planTrajectory();

    /// @brief Expands the frontier
    /// @param current_node The node to expand from
    void expandFrontier(const GraphNode& current_node);

    /// @brief Gets neighbors of the current node
    /// @param current_node The node to find neighbors of
    std::vector<GraphNode> calcNeighbors(const GraphNode& current_node);

    /// @brief Calculates G-score of a node
    /// @param parent_node The parent node of the new node
    /// @param node The node to calculate score of
    /// @return G-score
    float64_t calcNodeGScore(const GraphNode& parent_node, const GraphNode& node) const noexcept;

    /// @brief Calculates the cost of a node
    /// @param node The node to calculate score of
    /// @return The cost of the node
    float64_t calcNodeCost(const GraphNode& node) const;

    /// @brief Checks for collision
    /// @param point The point to check for collision (uses inflated params)
    /// @return probability of collision
    std::int64_t getProbabilityCollision(const Point& pt);

    /// @brief Reconstructs path from the nodes after the goal is found
    /// @return `true` if successful
    bool reconstructPath();

    /// @brief Graph search node-related members
    /// @{
    std::priority_queue<GraphNode, std::vector<GraphNode>> m_frontier;     ///< P-q of graph nodes to use
    std::unordered_set<GraphNode>                          m_open_nodes;   ///< Open nodes
    std::unordered_map<std::uint64_t, GraphNode>           m_nodes;        ///< All nodes by ID, used for reconstruction
    GraphNode                                              m_goal_node;    ///< Goal node
    Costmap                                                m_costmap;      ///< Costmap for collision checking
    /// @}

    /// @brief Inputs and outputs
    /// @{
    std::vector<Point>                   m_path;       ///< Holonomic path found
    nav_msgs::Path                       m_ros_path;   ///< Ros Path
    std::shared_ptr<LocalPlannerConfig>  m_cfg;        ///< Config for the local planner
    LocalPlannerData                     m_data;       ///< Inbound data over IPC
    /// @}

};

} // namespace local_planner

#endif // PLANNING_ASTAR_HPP