#include <iostream>
#include <cstdlib>
#include <sstream>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include "vec3.h"
#include "airwaysTree.h"

#include <cpPlugins/Interface.h>
#include <cpPlugins/Workspace.h>

using namespace airways;
namespace po = boost::program_options;

namespace
{
  const size_t ERROR_IN_COMMAND_LINE = 1;
  const size_t SUCCESS = 0;
  const size_t ERROR_UNHANDLED_EXCEPTION = 2;
} // namespace

typedef std::vector< AirwaysTree > AirwaysVector;

// Auxiliar struct to save info for execution.
typedef void* TImagePointer;
struct TreeInfo{

  TreeInfo( )
    {
      this->myWorkspace = new cpPlugins::Workspace( );
    }
  ~TreeInfo( )
    {
      /*
        if( this->IsMyWorkspace )
        delete this->myWorkspace;
      */
    }

  void CastImage( )
    {
      try
      {
        this->myWorkspace->Execute( "cast" );
      }
      catch( itk::ExceptionObject& err )
      {
        std::cerr << "Error: " << err << std::endl;
        std::exit( 1 );

      } // yrt
      this->Image =
        this->myWorkspace->GetFilter( "cast" )->GetOutputData( "Output" )->
        GetITK< TInputImage >( );
    }

  TInputImage::Pointer Image;
  cpPlugins::Workspace* myWorkspace;
  bool IsMyWorkspace;
  Vec3 seed;
  std::string folderpath_pigResults;
  std::string pig_name;
  std::string image_name;

};
// Constants
unsigned char red[3]   = { 255, 0, 0 };
unsigned char green[3] = { 0, 255, 0 };
unsigned char blue[3]  = { 0, 0, 255 };
unsigned char yellow[3]= { 255, 255, 0 };
unsigned char purple[3]= { 255, 0, 255 };
unsigned char cyan[3]= { 0, 255, 255 };

cpPlugins::Interface myPlugins;

// -------------------------------------------------------------------------
void Load_cpPlugins( const std::string& plugins );
void CreateResultDirectory(AirwaysTree tree);
void DrawVTKLinesFromTree(AirwaysTree& tree, const std::string filename, bool common);
AirwaysTree& CreateAirwaysTreeFromSegmentation(Vec3 seed, TInputImage* input_image, cpPlugins::Workspace& ws );
vector<TreeInfo> ReadInputFile(const char* filename);
void printCommonTreeBetweenTwoTrees(AirwaysTree tree_A, AirwaysTree tree_B, std::vector< std::pair< std::pair<Node*, Node*>, std::pair<Node*, Node*> > > vector_pair_edges_A_to_B, unsigned int Q, unsigned int F);
void printMatchingResultToFile(AirwaysTree tree_A, AirwaysTree tree_B, std::map< unsigned int, std::vector<Node*> > map_A_to_B, std::map< unsigned int, std::vector<Node*> > map_B_to_A, unsigned int Q, unsigned int F);
void createLinesAndPointsForVTK(const Node* node, vtkSmartPointer<vtkPoints>& pts,vtkSmartPointer<vtkCellArray>& lines,	vtkSmartPointer<vtkUnsignedCharArray>& colors, bool common, bool isRoot);

// -------------------------------------------------------------------------
int main( int argc, char* argv[] )
{
  Load_cpPlugins( "./plugins.cfg" );

  try
  {
    // Define and parse the program options
    po::options_description desc("Options");
    desc.add_options()("use_file", po::value<std::string>(), "Adds the filepath containing the file to be used in creaAirwaysTree -- Mandatory")
      ("build_trees","Creates trees from a given filepath (arg) and stores it in a .vtk file")
      ("subtree_levels", po::value<int>(),"Get a subtree(s) by levels - result: a vtk and reconstructed img .mhd")
      ("subtree_length", po::value<float>(),"Get a subtree(s) by length - result: a vtk and reconstructed img .mhd")
      ("subtree_diameter", po::value<float>(),"Get a subtree(s) by diameter - result: a vtk and reconstructed img .mhd")
      ("compare_trees",	"Compare the trees given in the input file or the subtrees using an option - result: a vtk and reconstructed img .mhd");
    //TODO: Fix image segmentation. ("segment_images",  "Creates a binary image corresponding to the segmentation of of a given original image (arg) and seed (arg), saves it to a .mhd file and uses it for subsequent operations")
    po::variables_map vm;
    try
    {
      std::string fileName;
      vector<TreeInfo> treeInfoVector;
      AirwaysVector aVector;
      TImagePointer segmentationImage;
      Vec3 seed;
      po::store(po::parse_command_line(argc, argv, desc), vm); // can throw
      if (vm.count("use_file"))
      {
        fileName = vm["use_file"].as<std::string>();
        treeInfoVector = ReadInputFile(fileName.c_str());
      }

      if(vm.count("segment_images"))
      {
      }

      // Build trees option
      if (vm.count("build_trees"))
      {
        for(unsigned int i = 0; i < treeInfoVector.size(); ++ i){
          TreeInfo info = treeInfoVector[i];
          try
          {
            info.myWorkspace->PrintExecutionOn( );
            info.myWorkspace->Execute( "eb" );
          }
          catch( itk::ExceptionObject& err )
          {
            std::cerr << "Error: " << err << std::endl;
            std::exit( 1 );

          } // fi
          info.CastImage( );
          AirwaysTree tree = CreateAirwaysTreeFromSegmentation( info.seed, info.Image, *(info.myWorkspace ));
          tree.SetResultPath(info.folderpath_pigResults);
          tree.SetImageName(info.image_name);
          aVector.push_back(tree);
        }
        for (unsigned int i = 0; i < aVector.size(); ++i)
        {
          CreateResultDirectory(aVector[i]);
          std::string fullPath = aVector[i].GetResultPath() + "/" + aVector[i].GetImageName() + ".vtk";
          DrawVTKLinesFromTree(aVector[i], fullPath, false);

        } //for
      } //if

      // Subtree levels option
      if (vm.count("subtree_levels"))
      {
      } //if
      else if (vm.count("subtree_length"))
      {
      } //if
      else if (vm.count("subtree_diameter"))
      {
      } //if

      if (vm.count("compare_trees"))
      {
        std::cout << "Option: Compare trees" << std::endl;
        /**
         * this piece of code only test in the case of two trees, so it can be replaced
         * to a code which does a cascade
         */

        //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        //CODIGO DE COMPARACIÓN DE ARBOLES
        //std::cout << "Comparing trees ..." << std::endl;
        //aVector[0].CompareTrees(aVector[1]);
        //std::cout << "Comparing trees ... OK" << std::endl;

        std::cout << "Tree A ... " << std::endl;
        //aVector[0].printNodeAndChildrenIds();
        std::cout << "Tree A ... OK" << std::endl;

        std::cout << "Tree B ... " << std::endl;
        //aVector[1].printNodeAndChildrenIds();
        std::cout << "Tree B ... OK" << std::endl;

        std::cout << "Comparing trees Orkisz-Morales..." << std::endl;
        // Vectors to save the common and uncommon nodes
        vec_nodes commonA;
        vec_nodes commonB;
        vec_nodes nonCommonA;
        vec_nodes nonCommonB;
        std::map< unsigned int, std::vector<Node*> > map_A_to_B;
        std::map< unsigned int, std::vector<Node*> > map_B_to_A;
        std::vector< std::pair< std::pair<Node*, Node*>, std::pair<Node*, Node*> > > vector_pair_edges_A_to_B;

        // Input parameter for comparison
        unsigned int Q = 1; // Corresponds to the depth to select "fathers" nodes
        unsigned int F = 1; // Correspond to the depth to select "family" nodes
        std::cout << "Q = " << Q << std::endl;
        std::cout << "F = " << F << std::endl;

        clock_t before_compare = clock();
        aVector[0].CompareTreesOrkiszMorales(aVector[1], Q, F, commonA, commonB, nonCommonA, nonCommonB, map_A_to_B, map_B_to_A, vector_pair_edges_A_to_B);
        clock_t after_compare = clock();
        double compare_time = double(after_compare - before_compare) / CLOCKS_PER_SEC;
        std::cout << "Matching time: " << compare_time << std::endl;

        // Print the common tree with common edges
        printCommonTreeBetweenTwoTrees(aVector[0], aVector[1], vector_pair_edges_A_to_B, Q, F);
        printMatchingResultToFile(aVector[0], aVector[1], map_A_to_B, map_B_to_A, Q, F);
        //printSeparatedCommonTrees(aVector[0], aVector[1], vector_pair_edges_A_to_B, Q, F);
        //reconstructAndSaveCommonTrees(aVector[0], aVector[1], vector_pair_edges_A_to_B, Q, F);

        // Print all the maps that have more that two connections
        std::cout << "Printing multiple relation A to B. Total relations: " << map_A_to_B.size() << std::endl;
        for(std::map< unsigned int, std::vector<Node*> >::iterator it_map_A_to_B = map_A_to_B.begin(); it_map_A_to_B != map_A_to_B.end(); ++it_map_A_to_B)
        {
          if((*it_map_A_to_B).second.size() > 1)
          {
            std::cout << "Multiple relation A to B with size:" << (*it_map_A_to_B).second.size() << ", from Id:" << (*it_map_A_to_B).first << std::endl;
            for(std::vector<Node*>::iterator it_node = (*it_map_A_to_B).second.begin(); it_node != (*it_map_A_to_B).second.end(); ++it_node)
            {
              std::cout << "Id: " << (*it_node)->GetId() << std::endl;
            }
          }
        }
        std::cout << "Printing multiple relation A to B ... OK" << std::endl;

        std::cout << "Printing multiple relation B to A. Total relations: " << map_B_to_A.size() << std::endl;
        for(std::map< unsigned int, std::vector<Node*> >::iterator it_map_B_to_A = map_B_to_A.begin(); it_map_B_to_A != map_B_to_A.end(); ++it_map_B_to_A)
        {
          if((*it_map_B_to_A).second.size() > 1)
          {
            std::cout << "Multiple relation B to A with size:" << (*it_map_B_to_A).second.size() << ", from Id:" << (*it_map_B_to_A).first << std::endl;
            for(std::vector<Node*>::iterator it_node = (*it_map_B_to_A).second.begin(); it_node != (*it_map_B_to_A).second.end(); ++it_node)
            {
              std::cout << "Id: " << (*it_node)->GetId() << std::endl;
            }
          }
        }
        std::cout << "Printing multiple relation B to A  ... OK" << std::endl;

        // Mark only the common nodes
        aVector[0].UnMarkAll();
        aVector[1].UnMarkAll();

        // --------------------------------------
        // ------ Get common paths marked -------
        // --------------------------------------

        /*
          std::string path_folder = "/run/media/alfredo/Data/Pulmones/results/airwaysGraphs/comparisonResutls/probes/";
          std::string suffix_vtk = ".vtk";
          std::string name_tree = "TreeA_dP2P";
          int iteration = 1;
          for(vec_nodes::iterator it_A = commonA.begin(); it_A != commonA.end(); ++it_A)
          {
          aVector[0].MarkPathFromNodeToNode(aVector[0].GetRoot(), (*it_A));

          std::stringstream filepath_actualIteration;
          filepath_actualIteration << path_folder << name_tree << "__" << iteration << "_idA_" << (*it_A)->GetId() << suffix_vtk;

          DrawVTKLinesFromTree(aVector[0], filepath_actualIteration.str(), true);
          ++iteration;
          }

          iteration = 1;
          name_tree = "TreeB_dP2P";
          for(vec_nodes::iterator it_B = commonB.begin(); it_B != commonB.end(); ++it_B)
          {
          aVector[1].MarkPathFromNodeToNode(aVector[1].GetRoot(), (*it_B));

          std::stringstream filepath_actualIteration;
          filepath_actualIteration << path_folder << name_tree << "__" << iteration << "_idB_" << (*it_B)->GetId() << suffix_vtk;

          DrawVTKLinesFromTree(aVector[1], filepath_actualIteration.str(), true);
          ++iteration;
          }
        */

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

        /*
        //XXXXXXXXXXXXXXXXXXXXXXXXXXXX
        //1. PRINT EACH NODE OF EACH TREE AND SAVE IT USING AS NAME ITS ID

        // Tree A
        for(int i = 2; i < aVector[0].GetWeight(); ++i)
        {
        //std::cout << "Writing idA: " << i << std::endl;
        AirwaysTree* tree_id = aVector[0].GetSingleDetachedTreeNodeById(i);
        //std::cout << "Got id: " << i << ", numberOfNodes: " << tree_id->GetWeight() <<  std::endl;
        if(tree_id)
        {
        std::stringstream filepath_actualIteration;
        filepath_actualIteration << aVector[0].GetResultPath() << "/" << aVector[0].GetImageName() << "_id_" << i << suffix_vtk;
        DrawVTKLinesFromTree(*tree_id, filepath_actualIteration.str(), false);
        delete(tree_id);
        }
        else
        std::cout << "Tree NULL" << std::endl;
        }

        // Tree B
        for(int i = 2; i < aVector[1].GetWeight(); ++i)
        {
        //std::cout << "Writing idA: " << i << std::endl;
        AirwaysTree* tree_id = aVector[1].GetSingleDetachedTreeNodeById(i);
        //std::cout << "Got id: " << i << ", numberOfNodes: " << tree_id->GetWeight() <<  std::endl;
        if(tree_id)
        {
        std::stringstream filepath_actualIteration;
        filepath_actualIteration << aVector[1].GetResultPath() << "/" << aVector[1].GetImageName() << "_id_" << i << suffix_vtk;
        DrawVTKLinesFromTree(*tree_id, filepath_actualIteration.str(), false);
        delete(tree_id);
        }
        else
        std::cout << "Tree NULL" << std::endl;
        }

        std::cout << "Comparing trees Orkisz-Morales... OK" << std::endl;

        */

        //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        /*// AMP
          aVector[0].SubIsomorphism(aVector[1]);

          std::cout << "Flag after SubIsomorphism" << std::endl;
          std::string fullPathA = aVector[0].GetResultPath() + "/" + aVector[0].GetImageName() + "_CMP.vtk";
          std::string fullPathB = aVector[1].GetResultPath() + "/" + aVector[1].GetImageName() + "_CMP.vtk";
          DrawVTKLinesFromTree(aVector[0], fullPathA, true);
          DrawVTKLinesFromTree(aVector[1], fullPathB, true);

          std::cout << "Flag after write vtk" << std::endl;
          TInputImage::Pointer imgA, imgB;
          aVector[0].ImageReconstruction(imgA);
          std::cout << "Flag after Reconstruction A" << std::endl;

          aVector[1].ImageReconstruction(imgB);
          std::cout << "Flag after Reconstruction B" << std::endl;

          std::string fullPathA_mhd = aVector[0].GetResultPath() + "/" + aVector[0].GetImageName() + "_CMP.vtk";
          std::string fullPathB_mhd = aVector[1].GetResultPath() + "/" + aVector[1].GetImageName() + "_CMP.vtk";
          WriteImage(imgA, fullPathA_mhd);
          WriteImage(imgB, fullPathB_mhd);

          //AMP*/
      }
      po::notify(vm); // throws on error, so do after help in case
    }
    catch (std::exception& e)
    {
      std::cerr << "Unhandled Exception reached the top of main: " << e.what() << ", application will now exit" << std::endl;
      return ERROR_UNHANDLED_EXCEPTION;
    } // yrt
  }
  catch (std::exception& e)
  {
    std::cerr << "Unhandled Exception reached the top of main: " << e.what() << ", application will now exit" << std::endl;
    return ERROR_UNHANDLED_EXCEPTION;
  } // yrt
  return( 0 );
}

// -------------------------------------------------------------------------
void Load_cpPlugins( const std::string& plugins )
{
  myPlugins.LoadConfiguration( plugins );
}

// -------------------------------------------------------------------------
void CreateResultDirectory(AirwaysTree tree)
{
  boost::filesystem::path dir(tree.GetResultPath());
  if (boost::filesystem::create_directories(dir))
  {
    std::cout << "FolderName = " << tree.GetResultPath() << " has been created" << std::endl;
  } //if
  else
    std::cout << "FolderName = " << tree.GetResultPath() << " already exists" << std::endl;
}

// -------------------------------------------------------------------------
void DrawVTKLinesFromTree(AirwaysTree& tree, const std::string filepath, bool common)
{
  // Create the array of points, lines, and colors
  vtkSmartPointer<vtkPoints> pts = vtkSmartPointer<vtkPoints>::New();
  vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
  colors->SetNumberOfComponents(3);
  colors->SetName("Colors");

  //vtk sphere for sources
  //vtkSmartPointer<vtkSphereSource> sphereSource = vtkSmartPointer<vtkSphereSource>::New();
  //Vec3 root = tree.GetRoot()->GetCoords();
  //sphereSource->SetCenter(root[0], root[1], root[2]);
  //sphereSource->SetRadius(1);
  //UndirectedGraph

  srand(time(NULL));
  unsigned int id = 1;

  // Create and fill the points, lines, and color vectors
  //CalculateVTKLinesFromEdges(tree.GetRoot(), 0, id, pts, lines, colors, common);
  pts->SetNumberOfPoints(tree.GetWeight()+1);
  createLinesAndPointsForVTK(tree.GetRoot(), pts, lines, colors, common, true);

  // Create the polydata
  vtkSmartPointer<vtkPolyData> linesPolyData = vtkSmartPointer<vtkPolyData>::New();

  // Set the points, lines, and colors
  linesPolyData->SetPoints(pts);
  linesPolyData->SetLines(lines);
  linesPolyData->GetCellData()->SetScalars(colors);

  // Write the file
  vtkSmartPointer<vtkPolyDataWriter> writer = vtkSmartPointer<vtkPolyDataWriter>::New();
  writer->SetFileName(filepath.c_str());

#if VTK_MAJOR_VERSION <= 5
  writer->SetInput(linesPolyData);
#else
  writer->SetInputData(linesPolyData);
#endif
  writer->Write();

  //The following code is to test the vtk polydata and visualize it
  /*	// Visualize
        vtkSmartPointer<vtkPolyDataMapper> mapper =
        vtkSmartPointer<vtkPolyDataMapper>::New();
        #if VTK_MAJOR_VERSION <= 5
        mapper->SetInput(linesPolyData);
        #else
        mapper->SetInputData(linesPolyData);
        #endif
        vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
        actor->SetMapper(mapper);

        //sphere
        vtkSmartPointer<vtkPolyDataMapper> mapperSphere = vtkSmartPointer<
        vtkPolyDataMapper>::New();
        mapperSphere->SetInputConnection(sphereSource->GetOutputPort());

        vtkSmartPointer<vtkActor> actorSphere = vtkSmartPointer<vtkActor>::New();
        actorSphere->SetMapper(mapperSphere);
        //end sphere

        vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
        vtkSmartPointer<vtkRenderWindow> renderWindow = vtkSmartPointer<
        vtkRenderWindow>::New();
        renderWindow->AddRenderer(renderer);
        vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
        vtkSmartPointer<vtkRenderWindowInteractor>::New();
        renderWindowInteractor->SetRenderWindow(renderWindow);
        renderer->AddActor(actorSphere);
        renderer->AddActor(actor);

        renderWindow->Render();
        renderWindowInteractor->Start();*/
}

// -------------------------------------------------------------------------
#include <fpa/Base/ImageSkeleton.h>
#include <fpa/Image/MinimumSpanningTree.h>
#include <cpExtensions/DataStructures/ImageIndexesContainer.h>
#include <itkImage.h>
#include <queue>

template< class TImage, class TVertex >
Node* FAVertexToNode( TVertex vertex, TImage* image )
{
  //The FrontAlgorithms Vertex is an TImageType::IndexType
  typename TImage::PointType point;
  image->TransformIndexToPhysicalPoint( vertex, point );
  Vec3 alfPoint(point[0],point[1],point[2]);
  return new Node(alfPoint);
}

AirwaysTree& ConvertFilterToAirwaysTree( TInputImage* input_image, cpPlugins::Workspace& ws )
{
  typedef
    fpa::Base::ImageSkeleton< fpa::Image::MinimumSpanningTree< 3 > >
    TFASkeleton;
  typedef TFASkeleton::TVertex    TVertexFA;
  typedef TFASkeleton::TVertexCmp TVertexCompareFA;
  typedef cpExtensions::DataStructures::ImageIndexesContainer< 3 > TVerticesFA;

  typedef Node TVertexAirways;
  typedef Edge TEdgeAirways;
  typedef pair_posVox_rad TSkelePoint;
  typedef vec_pair_posVox_rad TSkelePoints;
  typedef std::map< TVertexFA, TVertexAirways*, TVertexCompareFA > VertexMap;
  VertexMap vertexMap;
  std::time_t start,end;
  std::time(&start);
  std::cout << "Starting conversion " << std::endl;

  auto w_filter = ws.GetFilter( "eb" );
  auto distance_map =
    ws.GetFilter( "dmap" )->GetOutputData( "Output" )->
    GetITK< itk::Image< float, 3 > >( );
  auto sk =
    w_filter->GetOutputData( "Skeleton" )->GetITK< TFASkeleton >( );
  const auto& branches = sk->Get( );
  auto seed0 =
    ws.GetFilter( "seed" )->GetOutputData( "Output" )->
    GetITK< TVerticesFA >( )->Get( )[ 0 ];

  std::queue< TVertexFA > q;
  // TODO: std::set< TVertexFA, TVertexCompareFA > marks;
  q.push( seed0 );
  while( !q.empty( ) )
  {
    auto sVertex = q.front( );
    q.pop( );
    auto sIt = branches.find( sVertex );
    /* TODO
       if( marks.find( sVertex ) != marks.end( ) )
       {
       std::cout << "MARK!!!!! : " << sVertex << std::endl;
       }
       marks.insert( sVertex );
    */

    // End node... do nothing
    if( sIt == branches.end( ) )
      continue;

    // Create source node
    auto srcIt = vertexMap.find( sVertex );
    if( srcIt == vertexMap.end( ) )
      srcIt = vertexMap.insert(
        VertexMap::value_type(
          sVertex, FAVertexToNode( sVertex, input_image )
          )
        ).first;

    // TODO: std::cout << sVertex << " " << " " << sLevel << " " << seed0 << std::endl;
    // Create destination nodes
    for( auto eIt = sIt->second.begin( ); eIt != sIt->second.end( ); ++eIt )
    {
      auto dstIt = vertexMap.find( eIt->first );
      if( dstIt == vertexMap.end( ) )
        dstIt = vertexMap.insert(
          VertexMap::value_type(
            eIt->first, FAVertexToNode( eIt->first, input_image )
            )
          ).first;

      // Connect in the acyclic graph
      dstIt->second->SetFather( srcIt->second );
      srcIt->second->AddChild( dstIt->second );

      // Create detailed edge
      TEdgeAirways* edge = new Edge( );
      edge->SetSource( srcIt->second );
      edge->SetTarget( dstIt->second );
      auto path = eIt->second->GetVertexList( );
      for( unsigned int pIt = 0; pIt < path->Size( ); ++pIt )
      {
        itk::ImageBase< 3 >::PointType pnt;
        auto cidx = path->GetElement( pIt );
        input_image->TransformContinuousIndexToPhysicalPoint( cidx, pnt );
        TVertexFA idx;
        input_image->TransformPhysicalPointToIndex( pnt, idx );
        Vec3 coords = FAVertexToNode( idx, input_image )->GetCoords( );
        pair_posVox_rad skPair( coords, distance_map->GetPixel( idx ) );
        edge->AddSkeletonPairInfo( skPair );

      } // rof

      // Finish association
      dstIt->second->SetEdge( edge );

      // Put it as next candidate
      q.push( eIt->first );

    } // rof

  } // elihw

  AirwaysTree* tree =
    new AirwaysTree( input_image, NULL, vertexMap[ seed0 ], false );
  std::time(&end);
  std::cout << "Finished conversion. New AlfTree has weight: "<<tree->GetWeight()<<". Takes "<<(end-start)<<" s." << std::endl;
  return *tree;
}

// -------------------------------------------------------------------------
AirwaysTree& CreateAirwaysTreeFromSegmentation(Vec3 seed, TInputImage* input_image, cpPlugins::Workspace& ws )
{
  return( ConvertFilterToAirwaysTree( input_image, ws ) );
}

// -------------------------------------------------------------------------
vector<TreeInfo> ReadInputFile(const char* filename)
{
  // Variables
  std::ifstream infile(filename);
  std::string line;
  std::string folderpath_allResults;
  vector<TreeInfo> vectorInfo;
  // Parse the file
  bool firstLine = false;

  while (std::getline(infile, line))
  {
    // First line contains the output folder
    std::istringstream iss(line);
    if (!firstLine)
    {
      if (!(iss >> folderpath_allResults))
      {
        std::cout << "no file" << std::endl;
        return vectorInfo;
      } //if
      firstLine = true;
    } //if
    // Other lines, not the first one, contain the information for the airways to be created
    else
    {
      TreeInfo treeInfo;
      float point_trachea[3];
      std::string filepath_airwayImage, name_pig, name_image;
      if (!(iss >> point_trachea[0] >> point_trachea[1] >> point_trachea[2] >> filepath_airwayImage >> name_pig >> name_image))
      {
        std::cout << "There is no tree information in the file." << std::endl;
        break;
      } //if
      else
      {
        // Print information
        std::cout << "Point trachea:[" << point_trachea[0] << "," << point_trachea[1] << "," << point_trachea[2] << "]" << std::endl;
      }

      //Save info.

      //Save seed info.
      Vec3 seed(point_trachea[0], point_trachea[1], point_trachea[2]); //real coords seed
      treeInfo.seed = seed;
      // Create the outputs
      treeInfo.folderpath_pigResults = folderpath_allResults + "/" + name_pig + "/";
      treeInfo.pig_name = name_pig;
      treeInfo.image_name = name_image;
      // Read the image
      /*
        treeInfo.image = ReadImage<TInputImage>(filepath_airwayImage);
      */
      // Execute first pipeline's part
      std::stringstream seed_stream;
      seed_stream
        << point_trachea[0] << " "
        << point_trachea[1] << " "
        << point_trachea[2];

      treeInfo.myWorkspace->SetInterface( &myPlugins );
      std::string err = treeInfo.myWorkspace->LoadWorkspace( "./workspace_airwaysappli.wxml" );
      if( err != "" )
      {
        std::cerr << "Error: " << err << std::endl;
        std::exit( 1 );

      } // fi
      treeInfo.myWorkspace->SetParameter( "FileNames@reader", filepath_airwayImage );
      treeInfo.myWorkspace->SetParameter( "Text@seed", seed_stream.str( ) );
      vectorInfo.push_back(treeInfo);

    } //else
  } //while
  return vectorInfo;
}

// -------------------------------------------------------------------------
void printCommonTreeBetweenTwoTrees(AirwaysTree tree_A, AirwaysTree tree_B, std::vector< std::pair< std::pair<Node*, Node*>, std::pair<Node*, Node*> > > vector_pair_edges_A_to_B, unsigned int Q, unsigned int F)
{
  std::cout << "printCommonTreeBetweenTwoTrees, edges:" << vector_pair_edges_A_to_B.size() << std::endl;

  // Vtk points, cell array, and colors
  vtkSmartPointer<vtkPoints> points_common = vtkSmartPointer<vtkPoints>::New();
  vtkSmartPointer<vtkCellArray> lines_common = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkUnsignedCharArray> colors_common = vtkSmartPointer<vtkUnsignedCharArray>::New();
  colors_common->SetNumberOfComponents(3);
  colors_common->SetName("Colors");

  // Add all the points
  // 0 - 1000 points for tree A and from 1001 for tree B
  points_common->SetNumberOfPoints(1000 + tree_B.GetWeight() + 100);

  // Add points for tree A
  vec_nodes vector_nodesA = tree_A.GetNodes();
  for(vec_nodes::iterator it_nodesA = vector_nodesA.begin(); it_nodesA != vector_nodesA.end(); ++it_nodesA)
  {
    points_common->SetPoint((*it_nodesA)->GetId(),(*it_nodesA)->GetCoords().GetVec3());
  }

  // Add points for tree B
  vec_nodes vector_nodesB = tree_B.GetNodes();
  for(vec_nodes::iterator it_nodesB = vector_nodesB.begin(); it_nodesB != vector_nodesB.end(); ++it_nodesB)
  {
    points_common->SetPoint((*it_nodesB)->GetId()+1000,(*it_nodesB)->GetCoords().GetVec3());
  }

  // Add the edges for both trees
  std::vector< std::pair< std::pair<Node*, Node*>, std::pair<Node*, Node*> > >::iterator it_edges = vector_pair_edges_A_to_B.begin();
  int number_pairs = 0;
  for(; it_edges != vector_pair_edges_A_to_B.end(); ++it_edges)
  {
    std::cout << "Pair:" << number_pairs << std::endl;
    std::pair<Node*, Node*> edge_A = (*it_edges).first;
    std::pair<Node*, Node*> edge_B = (*it_edges).second;

    vec_nodes path_A;
    edge_A.first->GetPathToNode(edge_A.second, path_A);

    vec_nodes path_B;
    edge_B.first->GetPathToNode(edge_B.second, path_B);

    // Set color to be used
    int numColor = number_pairs % 6;
    unsigned char color[3];
    color[0] = green[0];
    color[1] = green[1];
    color[2] = green[2];
    if(numColor == 1)
    {
      color[0] = red[0];
      color[1] = red[1];
      color[2] = red[2];
    }
    else if(numColor == 2)
    {
      color[0] = blue[0];
      color[1] = blue[1];
      color[2] = blue[2];
    }
    else if(numColor == 3)
    {
      color[0] = yellow[0];
      color[1] = yellow[1];
      color[2] = yellow[2];
    }
    else if(numColor == 4)
    {
      color[0] = purple[0];
      color[1] = purple[1];
      color[2] = purple[2];
    }
    else if(numColor == 5)
    {
      color[0] = cyan[0];
      color[1] = cyan[1];
      color[2] = cyan[2];
    }

    number_pairs++;

    // Trace line A
    //if(path_A.size() > 0 && number_pairs < 50)
    if( path_A.size() )
    {
      vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
      lines_common->InsertNextCell(path_A.size());
      int id_point = 0;
      for(vec_nodes::iterator it_pathA = path_A.begin(); it_pathA != path_A.end(); ++it_pathA)
      {
        lines_common->InsertCellPoint((*it_pathA)->GetId());
        //line->GetPointIds()->SetId( id_point, (*it_pathA)->GetId() );
        //id_point++;
      }
      //lines_common->InsertNextCell(line);
      colors_common->InsertNextTupleValue(color);
    }
    else
    {
      std::cout << "No path A" << std::endl;
    }


    // Trace line B
    //if(path_B.size() > 0 && number_pairs < 50)
    if( path_B.size() )
    {
      vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
      lines_common->InsertNextCell(path_B.size());
      int id_point = 0;
      for(vec_nodes::iterator it_pathB = path_B.begin(); it_pathB != path_B.end(); ++it_pathB)
      {
        lines_common->InsertCellPoint((*it_pathB)->GetId()+1000);
        //line->GetPointIds()->SetId( id_point, (*it_pathB)->GetId()+1000 );
        id_point++;
      }
      //lines_common->InsertNextCell(line);
      colors_common->InsertNextTupleValue(color);
    }
    else
    {
      std::cout << "No path B" << std::endl;
    }
  }

  // Save the polydata
  // Create the polydata
  vtkSmartPointer<vtkPolyData> linesPolyData = vtkSmartPointer<vtkPolyData>::New();

  // Set the points, lines, and colors
  linesPolyData->SetPoints(points_common);
  linesPolyData->SetLines(lines_common);
  linesPolyData->GetCellData()->SetScalars(colors_common);

  // ------------------------------------------------
  // Write the vtk file

  // Create the pathfile to save
  std::stringstream filepath_actualIteration;
  filepath_actualIteration << tree_A.GetResultPath() << "/" << tree_A.GetImageName() << "_" << tree_B.GetImageName() << "_Q_" << Q << "_F_" << F << ".vtk";
  std::cout << "File to save:" << filepath_actualIteration.str() << std::endl;

  // Create the writer
  vtkSmartPointer<vtkPolyDataWriter> writer = vtkSmartPointer<vtkPolyDataWriter>::New();
  writer->SetFileName(filepath_actualIteration.str().c_str());

  // Set input and write
#if VTK_MAJOR_VERSION <= 5
  writer->SetInput(linesPolyData);
#else
  writer->SetInputData(linesPolyData);
#endif
  writer->Write();



  // ***************************************
  // Save the links in a file
  // *******

  // Create the outputfile
  std::stringstream filepath_evaluation;
  filepath_evaluation << tree_A.GetResultPath() << "/" << tree_A.GetImageName() << "_" << tree_B.GetImageName() << "_Q_" << Q << "_F_" << F << ".csv";

  std::ofstream file(filepath_evaluation.str().c_str());
  if(file.is_open())
  {
    // Save the header
    //file << "Node_" << tree_A.GetImageName() << " " << "Node_" << tree_B.GetImageName()<< "\n";
    file << "TreeName "
      "idLocalN1 idLocalN2 "
      "Node1x Node1y Node1z "
      "Node2x Node2y Node2z "
      "idGlobalN1 "
      "idMatch" << std::endl;


    std::vector< std::pair< std::pair<Node*, Node*>, std::pair<Node*, Node*> > >::iterator it_edges = vector_pair_edges_A_to_B.begin();
    for(; it_edges != vector_pair_edges_A_to_B.end(); ++it_edges)
    {

      std::pair<Node*, Node*> edge_A = (*it_edges).first;
      std::pair<Node*, Node*> edge_B = (*it_edges).second;

      //file << edge_A.second->GetId() << " " << edge_B.second->GetId()+1000 << "\n";
      Vec3 coord_EndNodeA = edge_A.second->GetCoords();
      Vec3 coord_EndNodeB = edge_B.second->GetCoords();
      file << tree_A.GetImageName() << " " <<
        edge_A.second->GetId() << " " << edge_B.second->GetId()+1000 << " " <<
        coord_EndNodeA[0] << " " << coord_EndNodeA[1] << " " << coord_EndNodeA[2] << " " <<
        coord_EndNodeB[0] << " " << coord_EndNodeB[1] << " " << coord_EndNodeB[2] << " " <<
        tree_A.GetImageName() << coord_EndNodeA[0] << coord_EndNodeA[1] << coord_EndNodeA[2] << " " <<
        tree_B.GetImageName() << coord_EndNodeB[0] << coord_EndNodeB[1] << coord_EndNodeB[2] <<"\n";

      file << tree_B.GetImageName() << " " <<
        edge_B.second->GetId()+1000 << " " << edge_A.second->GetId() << " " <<
        coord_EndNodeB[0] << " " << coord_EndNodeB[1] << " " << coord_EndNodeB[2] << " " <<
        coord_EndNodeA[0] << " " << coord_EndNodeA[1] << " " << coord_EndNodeA[2] << " " <<
        tree_B.GetImageName() << coord_EndNodeB[0] << coord_EndNodeB[1] << coord_EndNodeB[2] << " " <<
        tree_A.GetImageName() << coord_EndNodeA[0] << coord_EndNodeA[1] << coord_EndNodeA[2] << "\n";
    }
  }

  file.close();
  // *******
  // ***************************************




  std::cout << "printCommonTreeBetweenTwoTrees ... OK" << std::endl;
}

// -------------------------------------------------------------------------
void printMatchingResultToFile(AirwaysTree tree_A, AirwaysTree tree_B, std::map< unsigned int, std::vector<Node*> > map_A_to_B, std::map< unsigned int, std::vector<Node*> > map_B_to_A, unsigned int Q, unsigned int F)
{
  std::cout << "Printing matching result ... " << std::endl;

  // Variables and types
  typedef std::map< unsigned int, vec_nodes > map_id_node;

  // Create the outputfile
  std::stringstream filepath_evaluation;
  filepath_evaluation << tree_A.GetResultPath() << "/" << tree_A.GetImageName() << "_" << tree_B.GetImageName() << "_Q_" << Q << "_F_" << F << "_matching.csv";

  std::ofstream file(filepath_evaluation.str().c_str());
  if(file.is_open())
  {
    // Save the header
    file << "TreeName idLocalN1 idLocalN2 "
      "Node1x Node1y Node1z "
      "Node2x Node2y Node2z "
      "idGlobalN1 "
      "idMatch "
      "typeMatch_match_1_nonmatch_0 "
      "depth "
      "leaf"<< std::endl;

    // ---------------
    // From A to B

    // Save the match or non-match for each node from A to B
    for(int id_a=1; id_a <= tree_A.GetWeight( ); ++id_a)
    {
      // Get the actual node in tree A
      Node* node_a = tree_A.GetNodeById( id_a );
      Vec3 coords_a = node_a->GetCoords();
      unsigned int depth_a = tree_A.GetDepthById(id_a);
      bool leaf_a = node_a->IsLeaf();

      // Check if the node was matched
      map_id_node::iterator it_a2b = map_A_to_B.find( id_a );
      if( it_a2b != map_A_to_B.end( ) )
      {
        // Get the correspoding matching nodes and print them
        vec_nodes nodes_B = (*it_a2b).second;

        vec_nodes::iterator it_nodes_b = nodes_B.begin( );
        for( ; it_nodes_b != nodes_B.end( ); ++it_nodes_b)
        {
          Vec3 coords_b = (*it_nodes_b)->GetCoords();

          file << tree_A.GetImageName() << " " << id_a << " " << (*it_nodes_b)->GetId()+1000 << " "
               << coords_a[0] << " " << coords_a[1] << " " << coords_a[2] << " "
               << coords_b[0] << " " << coords_b[1] << " " << coords_b[2] << " "
               << tree_A.GetImageName() << coords_a[0] << coords_a[1] << coords_a[2] << " "
               << tree_A.GetImageName() << coords_a[0] << coords_a[1] << coords_a[2] << coords_b[0] << coords_b[1] << coords_b[2] << " "
               << "1" << " "
               << depth_a << " "
               << leaf_a << "\n";
        }
      }
      else
      {
        file << tree_A.GetImageName() << " " << id_a << " " << "0" << " "
             << coords_a[0] << " " << coords_a[1] << " " << coords_a[2] << " "
             << "0" << " " << "0" << " " << "0" << " "
             << tree_A.GetImageName() << coords_a[0] << coords_a[1] << coords_a[2] << " "
             << tree_A.GetImageName() << coords_a[0] << coords_a[1] << coords_a[2] << "0" << "0" << "0" << " "
             << "0" << " "
             << depth_a << " "
             << leaf_a  << "\n";
      }
    }

    // ---------------
    // From B to A

    // Save the match or non-match for each node from A to B
    for(int id_b=1; id_b <= tree_B.GetWeight( ); ++id_b)
    {
      // Get the actual node in tree B
      Node* node_b = tree_B.GetNodeById( id_b );
      Vec3 coords_b = node_b->GetCoords();
      unsigned int depth_b = tree_B.GetDepthById(id_b);
      bool leaf_b = node_b->IsLeaf();

      // Check if the node was matched
      map_id_node::iterator it_b2a = map_B_to_A.find( id_b );
      if( it_b2a != map_B_to_A.end( ) )
      {
        // Get the correspoding matching nodes and print them
        vec_nodes nodes_A = (*it_b2a).second;

        vec_nodes::iterator it_nodes_a = nodes_A.begin( );
        for( ; it_nodes_a != nodes_A.end( ); ++it_nodes_a)
        {
          Vec3 coords_a = (*it_nodes_a)->GetCoords();

          file << tree_B.GetImageName() << " " << id_b+1000 << " " << (*it_nodes_a)->GetId() << " "
               << coords_b[0] << " " << coords_b[1] << " " << coords_b[2] << " "
               << coords_a[0] << " " << coords_a[1] << " " << coords_a[2] << " "
               << tree_B.GetImageName() << coords_b[0] << coords_b[1] << coords_b[2] << " "
               << tree_B.GetImageName() << coords_b[0] << coords_b[1] << coords_b[2] << coords_a[0] << coords_a[1] << coords_a[2] << " "
               << "1" << " "
               << depth_b << " "
               << leaf_b  << "\n";
        }
      }
      else
      {
        file << tree_B.GetImageName() << " " << id_b+1000 << " " << "0" << " "
             << coords_b[0] << " " << coords_b[1] << " " << coords_b[2] << " "
             << "0" << " " << "0" << " " << "0" << " "
             << tree_B.GetImageName() << coords_b[0] << coords_b[1] << coords_b[2] << " "
             << tree_B.GetImageName() << coords_b[0] << coords_b[1] << coords_b[2] << "0" << "0" << "0" << " "
             << "0" << " "
             << depth_b << " "
             << leaf_b  << "\n";
      }//esle
    }//rof
  }//fi

  file.close(); // Close the file

  std::cout << "Printing matching result DONE" << std::endl;
}

// -------------------------------------------------------------------------
void createLinesAndPointsForVTK(const Node* node, vtkSmartPointer<vtkPoints>& pts,vtkSmartPointer<vtkCellArray>& lines,	vtkSmartPointer<vtkUnsignedCharArray>& colors, bool common, bool isRoot)
{
  // Insert the actual point/node
  //vtkIdType id_father = idNonRoot;
  //if(isRoot)
  //id_father = pts->InsertNextPoint(node->GetCoords().GetVec3());
  vtkIdType id_father = node->GetId();
  if(isRoot)
    pts->SetPoint(id_father,node->GetCoords().GetVec3());

  // Iterate over the children
  const vec_nodes children = node->GetChildren();
  for (vec_nodes::const_iterator it_child = children.begin(); it_child != children.end(); ++it_child)
  {
    if (!(*it_child)->IsMarked() && common)
      continue;

    //vtkIdType id_son = pts->InsertNextPoint((*it_child)->GetCoords().GetVec3());
    vtkIdType id_son = (*it_child)->GetId();
    pts->SetPoint(id_son,(*it_child)->GetCoords().GetVec3());

    // Set color to be used
    int numColor = (*it_child)->GetLevel() % 4;
    if(numColor == 0)
      colors->InsertNextTupleValue(green);
    else if(numColor == 1)
      colors->InsertNextTupleValue(red);
    else if(numColor == 2)
      colors->InsertNextTupleValue(red);
    else
      colors->InsertNextTupleValue(red);

    // Add the line
    vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
    line->GetPointIds()->SetId(0, id_father);
    line->GetPointIds()->SetId(1, id_son);
    lines->InsertNextCell(line);

    createLinesAndPointsForVTK(*it_child, pts, lines, colors, common, false);
  } //for
}

// eof - $RCSfile$