#include <iostream>
#include <vector>

#include "MBTypes.h"
#include "MBInterface.hpp"
#include "MBCore.hpp"


//#include "MergeMesh.hpp"
#include "moab/MergeMesh.hpp"
#include "moab/AdaptiveKDTree.hpp"
#include "moab/Skinner.hpp"
#include "moab/GeomUtil.hpp"
#include "moab/CN.hpp"
#include "MBSkinner.hpp"
#include "moab/OrientedBoxTreeTool.hpp"

#include <time.h>

moab::AdaptiveKDTree* kdtree;
MBEntityHandle kdtree_root;
moab::OrientedBoxTreeTool * obb;

MBInterface *MBI();

int main(int argc, char *argv[])
{
  // create a storage location for mesh
  MBEntityHandle loaded_file_set;
  // create a mesh set
  MBErrorCode result= MBI()->create_meshset( MESHSET_SET, loaded_file_set );
  if ( result != MB_SUCCESS )
    {
      std::cout << "Error, failed to create meshset" << std::endl;
      exit(1);
    }

  // load a mesh file
  result = MBI()->load_file("test2_mesh.h5m", &loaded_file_set );
  if ( result != MB_SUCCESS )
    {
      std::cout << "Error, failed to open file" << std::endl;
      exit(1);
    }
      // 

  MBRange tetrahedra; // tets in the problem
  MBRange faces; // faces of the tets
  result = MBI()->get_entities_by_dimension(loaded_file_set, 3, tetrahedra);
  if ( result != MB_SUCCESS )
    {
      std::cout << "Error, failed to get entities" << std::endl;
      exit(1);
    }

  // get adjacancy data from tets
  // get the dimensionality of the input set
  int dimension = MBI()->dimension_from_handle(tetrahedra[0]);
  if ( result != MB_SUCCESS )
    {
      std::cout << "Error, failed get dimension" << std::endl;
      exit(1);
    }
  // get adjacnecy data
  result = MBI()->get_adjacencies(tetrahedra, dimension-1, true, 
				faces, MBInterface::UNION); 
  if ( result != MB_SUCCESS )
    {
      std::cout << "Error, failed get adjacency data" << std::endl;
      exit(1);
    }
  // merge in the faces into the tet data set
  tetrahedra.merge(faces); 

  moab::EntityHandle set1;
  result= MBI()->create_meshset( MESHSET_SET, set1 );
    if ( result != MB_SUCCESS )
      {
        std::cout << "Error, failed to create meshset" << std::endl;
        exit(1);
      }
    MBI()->add_entities(set1, faces);
    MBI()->write_file("test.vtk", 0, 0, &set1, 1);

  //kdtree = new moab::AdaptiveKDTree( MBI() );
  //moab::AdaptiveKDTree::Settings kdtree_settings;
  moab::OrientedBoxTreeTool::Settings settings;
  obb = new moab::OrientedBoxTreeTool(MBI());
  //result = kdtree->build_tree(faces, kdtree_root, &kdtree_settings );
  result = obb->build(faces, kdtree_root, &settings );
  if ( result != MB_SUCCESS )
      {
        std::cout << "Error, failed to build" << std::endl;
        exit(1);
      }
  std::vector<double> intersections; // distance to each intersection
  std::vector<MBEntityHandle> intersection_facets; // triagnles we hit
  double dir[3] = {1.0,0.0,0.0}; // vector in x dir
  double box_center[3] = {1.,3.9,0.}; // start point of ray
  double ray_length = 900.; // length of ray

  // get the intersections along the ray
  /*
   * ray_intersect_triangles( std::vector<double>& distances_out,
                                         std::vector<EntityHandle>& facets_out,
                                         EntityHandle root_set,
                                         double tolerance,
                                         const double ray_point[3],
                                         const double unit_ray_dir[3],
                                         const double* ray_length = 0,
   */
//  result = kdtree->ray_intersect_triangles( kdtree_root, 1.0e-6,
//					    dir, box_center,
//					    intersection_facets
//					    , intersections, 100,
//					    ray_length);
  result = obb->ray_intersect_triangles( intersections,
      intersection_facets,kdtree_root, 1.0e-6 , box_center,
                 dir);
  if ( result != MB_SUCCESS )
    {
      std::cout << "Error, failed get ray intersect" << std::endl;
      exit(1);
    }					

  if ( intersections.size() == 0 )
    {
      std::cout << "ray misses mesh" << std::endl;
      exit(1);
    }

      
  std::cout << "# of intersections : " << intersections.size() << std::endl;
  std::cout << "intersection distances are on";
  for (unsigned int i = 0; i < intersections.size(); i++) 
    {
      std::cout << " " << intersections[i];
    }
  std::cout << " of ray length " << ray_length << std::endl;

  return 1;
}

MBInterface *MBI() 
{
  static MBCore instance;
  return &instance;
}