void XRayWithVecGeom(int axis,
                  Vector3D<Precision> origin,
                  Vector3D<Precision> bbox,
                  Vector3D<Precision> dir,
                  double axis1_start, double axis1_end,
                  double axis2_start, double axis2_end,
                  int data_size_x,
                  int data_size_y,
                  double pixel_axis,
                  int * image) {



if(VERBOSE){
    std::cout << "from [" << axis1_start << ";" << axis2_start
              << "] to [" << axis1_end   << ";" << axis2_end << "]\n";
    std::cout << "Xpixels " << data_size_x << " YPixels " << data_size_y << "\n";

    std::cout << pixel_axis << "\n";
}
    double pixel_width_1 = (axis1_end-axis1_start)/data_size_x;
    double pixel_width_2 = (axis2_end-axis2_start)/data_size_y;

    if(VERBOSE){
        std::cout << pixel_width_1 << "\n";
        std::cout << pixel_width_2 << "\n";
    }

    NavigationState * newnavstate = NavigationState::MakeInstance( GeoManager::Instance().getMaxDepth() );
    NavigationState * curnavstate = NavigationState::MakeInstance( GeoManager::Instance().getMaxDepth() );

    for( int pixel_count_2 = 0; pixel_count_2 < data_size_y; ++pixel_count_2 ){
        for( int pixel_count_1 = 0; pixel_count_1 < data_size_x; ++pixel_count_1 )
        {
            double axis1_count = axis1_start + pixel_count_1 * pixel_width_1 + 1E-6;
            double axis2_count = axis2_start + pixel_count_2 * pixel_width_2 + 1E-6;

            if(VERBOSE) {
                std::cout << "\n OutputPoint("<< axis1_count<< ", "<< axis2_count<< ")\n";
            }
         //   std::cout << pixel_count_1 << " " << pixel_count_2 << "\n";

            // set start point of XRay
            Vector3D<Precision> p;
            if( axis== 1 )
              p.Set( origin[0]-bbox[0], axis1_count, axis2_count);
            else if( axis== 2)
              p.Set( axis1_count, origin[1]-bbox[1], axis2_count);
            else if( axis== 3)
              p.Set( axis1_count, axis2_count, origin[2]-bbox[2]);

            SimpleNavigator nav;
            curnavstate->Clear();
            nav.LocatePoint( GeoManager::Instance().GetWorld(), p, *curnavstate, true );

#ifdef VECGEOM_DISTANCE_DEBUG
            gGeoManager->GetCurrentNavigator()->FindNode( p.x(), p.y(), p.z() );
#endif

//          curnavstate->Print();

            double distancetravelled=0.;
            int crossedvolumecount=0;

            if(VERBOSE) {
              std::cout << " StartPoint(" << p[0] << ", " << p[1] << ", " << p[2] << ")";
              std::cout << " Direction <" << dir[0] << ", " << dir[1] << ", " << dir[2] << ">"<< std::endl;
            }

            while( ! curnavstate->IsOutside() ) {
                double step = 0;
                newnavstate->Clear();
                Nav_t navigator;
                navigator.FindNextBoundaryAndStep( p,
                        dir,
                        *curnavstate,
                        *newnavstate,
                        vecgeom::kInfinity, step);

                //std::cout << "step " << step << "\n";
                distancetravelled+=step;
//
//              std::cout << "GOING FROM "
//                       << curnavstate->Top()->GetLabel() << "(";
//                        curnavstate->Top()->PrintType();
//                     std::cout << ") to ";
//
//                if( newnavstate->Top() ){
//                    std::cout << newnavstate->Top()->GetLabel() << "(";
//                    newnavstate->Top()->PrintType();
//                    std::cout << ")";
//                }
//                else
//                    std::cout << "outside ";
//
//                if ( curnavstate->Top() == newnavstate->Top() ) {
//                    std::cout << " CROSSING PROBLEM \n";
//                    curnavstate->Print();
//                    newnavstate->Print();
//                }
//
//                std::cout << "# step " << step << " crossed "<< crossedvolumecount << "\n";

                // here we have to propagate particle ourselves and adjust navigation state
                p = p + dir*(step + 1E-6);

//                std::cout << p << "\n";

                newnavstate->CopyTo(curnavstate);

                // Increase passed_volume
                // TODO: correct counting of travel in "world" bounding box
                if(step>0) crossedvolumecount++;

              //  if(crossedvolumecount > 1000){
                    //std::cerr << "OOPS: Problem for pixel " << pixel_count_1 << " " << pixel_count_2 << " \n";
                    //break;}
             } // end while

             ///////////////////////////////////
             // Store the number of passed volume at 'volume_result'
             *(image+pixel_count_2*data_size_x+pixel_count_1) = crossedvolumecount;

      } // end inner loop
    } // end outer loop

    NavigationState::ReleaseInstance( curnavstate );
    NavigationState::ReleaseInstance( newnavstate );

} // end XRayWithVecGeom
//////////////////////////////////
// main function
int main(int argc, char * argv[])
{
  int axis= 0;

  double axis1_start= 0.;
  double axis1_end= 0.;

  double axis2_start= 0.;
  double axis2_end= 0.;

  double pixel_width= 0;
  double pixel_axis= 1.;

  if( argc < 5 )
  {
    std::cerr<< std::endl;
    std::cerr<< "Need to give rootfile, volumename, axis and number of axis"<< std::endl;
    std::cerr<< "USAGE : ./XRayBenchmarkFromROOTFile [rootfile] [VolumeName] [ViewDirection(Axis)]"
             << "[PixelWidth(OutputImageSize)] [--usolids|--vecgeom(Default:usolids)] [--novoxel(Default:voxel)]"
             << std::endl;
    std::cerr<< "  ex) ./XRayBenchmarkFromROOTFile cms2015.root BSCTrap y 95"<< std::endl;
    std::cerr<< "      ./XRayBenchmarkFromROOTFile cms2015.root PLT z 500 --vecgeom --novoxel"<< std::endl<< std::endl;
    return 1;
  }

  TGeoManager::Import( argv[1] );
  std::string testvolume( argv[2] );

  if( strcmp(argv[3], "x")==0 )
    axis= 1;
  else if( strcmp(argv[3], "y")==0 )
    axis= 2;
  else if( strcmp(argv[3], "z")==0 )
    axis= 3;
  else
  {
    std::cerr<< "Incorrect axis"<< std::endl<< std::endl;
    return 1;
  }

  pixel_width= atof(argv[4]);

  for(auto i= 5; i< argc; i++)
  {
    if( ! strcmp(argv[i], "--usolids") )
      usolids= true;
    if( ! strcmp(argv[i], "--vecgeom") )
      usolids= false;
    if( ! strcmp(argv[i], "--novoxel") )
      voxelize = false;
  }

  int found = 0;
  TGeoVolume * foundvolume = NULL;
  // now try to find shape with logical volume name given on the command line
  TObjArray *vlist = gGeoManager->GetListOfVolumes( );
  for( auto i = 0; i < vlist->GetEntries(); ++i )
  {
    TGeoVolume * vol = reinterpret_cast<TGeoVolume*>(vlist->At( i ));
    std::string fullname(vol->GetName());
    
    std::size_t founds = fullname.compare(testvolume);
    if ( founds==0 ){
      found++;
      foundvolume = vol;

      std::cerr << "("<< i<< ")found matching volume " << foundvolume->GetName()
        << " of type " << foundvolume->GetShape()->ClassName() << "\n";
    }
  }

  std::cerr << "volume found " << found << " times \n\n";

  // if volume not found take world
  if( ! foundvolume ) {
      std::cerr << "specified volume not found; xraying complete detector\n";
      foundvolume = gGeoManager->GetTopVolume();
  }

  if( foundvolume ) {
    foundvolume->GetShape()->InspectShape();
    std::cerr << "volume capacity " 
          << foundvolume->GetShape()->Capacity() << "\n";

    // get bounding box to generate x-ray start positions
    double dx = ((TGeoBBox*)foundvolume->GetShape())->GetDX()*1.05;
    double dy = ((TGeoBBox*)foundvolume->GetShape())->GetDY()*1.05;
    double dz = ((TGeoBBox*)foundvolume->GetShape())->GetDZ()*1.05;
    double origin[3]= {0., };
    origin[0]= ((TGeoBBox*)foundvolume->GetShape())->GetOrigin()[0];
    origin[1]= ((TGeoBBox*)foundvolume->GetShape())->GetOrigin()[1];
    origin[2]= ((TGeoBBox*)foundvolume->GetShape())->GetOrigin()[2];
    
    TGeoMaterial * matVacuum = new TGeoMaterial("Vacuum",0,0,0);
    TGeoMedium * vac = new TGeoMedium("Vacuum",1,matVacuum);

    TGeoVolume* boundingbox= gGeoManager->MakeBox("BoundingBox", vac,
            std::abs(origin[0]) + dx,
            std::abs(origin[1]) + dy,
            std::abs(origin[2]) + dz );

    // TGeoManager * geom = boundingbox->GetGeoManager();
    std::cout << gGeoManager->CountNodes() << "\n";

    if(! voxelize ) DeleteROOTVoxels();

    gGeoManager = 0;

    TGeoManager * mgr2 = new TGeoManager();

//    delete gGeoManager;
//    gGeoManager = new TGeoManager();
    boundingbox->AddNode( foundvolume, 1);
    mgr2->SetTopVolume( boundingbox );
    mgr2->CloseGeometry();
    gGeoManager = mgr2;
    gGeoManager->Export("DebugGeom.root");

    mgr2->GetTopNode()->GetMatrix()->Print();

    std::cout << gGeoManager->CountNodes() << "\n";
    //delete world->GetVoxels();
    //world->SetVoxelFinder(0);
    
    std::cout<< std::endl;
    std::cout<< "BoundingBoxDX: "<< dx<< std::endl;
    std::cout<< "BoundingBoxDY: "<< dy<< std::endl;
    std::cout<< "BoundingBoxDZ: "<< dz<< std::endl;
    
    std::cout<< std::endl;
    std::cout<< "BoundingBoxOriginX: "<< origin[0]<< std::endl;
    std::cout<< "BoundingBoxOriginY: "<< origin[1]<< std::endl;
    std::cout<< "BoundingBoxOriginZ: "<< origin[2]<< std::endl<< std::endl;
  
    Vector3D<Precision> p;
    Vector3D<Precision> dir;
    
    if(axis== 1)
    {
      dir.Set(1., 0., 0.);
      //Transformation3D trans( 0, 0, 0, 5, 5, 5);
      //trans.Print();
     // dir = trans.TransformDirection( Vector3D<Precision> (1,0,0));

      axis1_start= origin[1]- dy;
      axis1_end= origin[1]+ dy;
      axis2_start= origin[2]- dz;
      axis2_end= origin[2]+ dz;
      pixel_axis= (dy*2)/pixel_width;
    }
    else if(axis== 2)
    {
      dir.Set(0., 1., 0.);
      //vecgeom::Transformation3D trans( 0, 0, 0, 5, 5, 5);
      //dir = trans.TransformDirection(dir);
      axis1_start= origin[0]- dx;
      axis1_end= origin[0]+ dx;
      axis2_start= origin[2]- dz;
      axis2_end= origin[2]+ dz;
      pixel_axis= (dx*2)/pixel_width;
    }
    else if(axis== 3)
    {
      dir.Set(0., 0., 1.);
      //vecgeom::Transformation3D trans( 0, 0, 0, 5, 5, 5);
      //dir = trans.TransformDirection(dir);
      axis1_start= origin[0]- dx;
      axis1_end= origin[0]+ dx;
      axis2_start= origin[1]- dy;
      axis2_end= origin[1]+ dy;
      pixel_axis= (dx*2)/pixel_width;
    }

    // init data for image
    int data_size_x= (axis1_end-axis1_start)/pixel_axis;
    int data_size_y= (axis2_end-axis2_start)/pixel_axis;
    int *volume_result= (int*) new int[data_size_y * data_size_x*3];

#ifdef VECGEOM_GEANT4
    int *volume_result_Geant4= (int*) new int[data_size_y * data_size_x*3];
#endif
    int *volume_result_VecGeom= (int*) new int[data_size_y * data_size_x*3];
    int *volume_result_VecGeomABB= (int*) new int[data_size_y * data_size_x*3];

    Stopwatch timer;
    timer.Start();
#ifdef CALLGRIND
    CALLGRIND_START_INSTRUMENTATION;
#endif
    XRayWithROOT( axis,
            Vector3D<Precision>(origin[0],origin[1],origin[2]),
            Vector3D<Precision>(dx,dy,dz),
            dir,
            axis1_start, axis1_end,
            axis2_start, axis2_end,
            data_size_x, data_size_y,
            pixel_axis,
            volume_result );
#ifdef CALLGRIND
    CALLGRIND_STOP_INSTRUMENTATION;
    CALLGRIND_DUMP_STATS;
#endif
    timer.Stop();

    std::cout << std::endl;
    std::cout << " ROOT Elapsed time : "<< timer.Elapsed() << std::endl;

    // Make bitmap file; generate filename
    std::stringstream imagenamebase;
    imagenamebase << "volumeImage_" << testvolume;
    if(axis==1) imagenamebase << "x";
    if(axis==2) imagenamebase << "y";
    if(axis==3) imagenamebase << "z";
    if(voxelize) imagenamebase << "_VOXELIZED_";
    std::stringstream ROOTimage;
    ROOTimage << imagenamebase.str();
    ROOTimage << "_ROOT.bmp";

    make_bmp(volume_result, ROOTimage.str().c_str(), data_size_x, data_size_y);
    make_bmp(volume_result, "foo.bmp", data_size_x, data_size_y, false);
#ifdef VECGEOM_GEANT4

    G4VPhysicalVolume * world = SetupGeant4Geometry( testvolume, Vector3D<Precision>( std::abs(origin[0]) + dx,
            std::abs(origin[1]) + dy,
            std::abs(origin[2]) + dz ) );
    G4GeoManager::Instance().LoadG4Geometry( world );

    timer.Start();

    XRayWithGeant4( world, axis,
            Vector3D<Precision>(origin[0],origin[1],origin[2]),
            Vector3D<Precision>(dx,dy,dz),
            dir,
            axis1_start, axis1_end,
            axis2_start, axis2_end,
            data_size_x, data_size_y,
            pixel_axis,
            volume_result_Geant4 );
    timer.Stop();

    std::stringstream G4image;
    G4image << imagenamebase.str();
    G4image << "_Geant4.bmp";
    make_bmp(volume_result_Geant4, G4image.str().c_str(), data_size_x, data_size_y);
    std::cout << std::endl;
    std::cout << " Geant4 Elapsed time : "<< timer.Elapsed() << std::endl;
    make_diff_bmp(volume_result, volume_result_Geant4, "diffROOTGeant4.bmp", data_size_x, data_size_y);
#endif

    // convert current gGeoManager to a VecGeom geometry
    RootGeoManager::Instance().LoadRootGeometry();
    std::cout << "Detector loaded " << "\n";
    ABBoxManager::Instance().InitABBoxesForCompleteGeometry();
    std::cout << "voxelized " << "\n";
    timer.Start();
#ifdef CALLGRIND
    CALLGRIND_START_INSTRUMENTATION;
#endif
    XRayWithVecGeom<SimpleNavigator>( axis,
               Vector3D<Precision>(origin[0],origin[1],origin[2]),
               Vector3D<Precision>(dx,dy,dz),
               dir,
               axis1_start, axis1_end,
               axis2_start, axis2_end,
               data_size_x, data_size_y,
               pixel_axis,
               volume_result_VecGeom );
#ifdef CALLGRIND
    CALLGRIND_START_INSTRUMENTATION;
    CALLGRIND_DUMP_STATS;
#endif
    timer.Stop();

    std::stringstream VecGeomimage;
    VecGeomimage << imagenamebase.str();
    VecGeomimage << "_VecGeom.bmp";
    make_bmp(volume_result_VecGeom, VecGeomimage.str().c_str(), data_size_x, data_size_y);

    make_diff_bmp(volume_result, volume_result_VecGeom, "diffROOTVecGeom.bmp", data_size_x, data_size_y);

    std::cout << std::endl;
    std::cout << " VecGeom Elapsed time : "<< timer.Elapsed() << std::endl;

    timer.Start();
#ifdef CALLGRIND
    CALLGRIND_START_INSTRUMENTATION;
#endif
    XRayWithVecGeom<ABBoxNavigator>( axis,
    Vector3D<Precision>(origin[0],origin[1],origin[2]),
    Vector3D<Precision>(dx,dy,dz),
    dir,
    axis1_start, axis1_end,
    axis2_start, axis2_end,
    data_size_x, data_size_y,
    pixel_axis,
    volume_result_VecGeomABB );
#ifdef CALLGRIND
    CALLGRIND_STOP_INSTRUMENTATION;
    CALLGRIND_DUMP_STATS;
#endif
    timer.Stop();

    std::stringstream VecGeomABBimage;
    VecGeomABBimage << imagenamebase.str();
    VecGeomABBimage << "_VecGeomABB.bmp";
    make_bmp(volume_result_VecGeomABB, VecGeomABBimage.str().c_str(), data_size_x, data_size_y);

    make_diff_bmp(volume_result_VecGeom, volume_result_VecGeomABB, "diffVecGeomSimplevsABB.bmp", data_size_x, data_size_y);
    make_diff_bmp(volume_result, volume_result_VecGeomABB, "diffROOTVecGeomABB.bmp", data_size_x, data_size_y);


    std::cout << std::endl;
    std::cout << " VecGeom ABB Elapsed time : "<< timer.Elapsed() << std::endl;

    return 0;

    // use the vector interface
    timer.Start();
    XRayWithVecGeom_VecNav( axis,
                   Vector3D<Precision>(origin[0],origin[1],origin[2]),
                   Vector3D<Precision>(dx,dy,dz),
                   dir,
                   axis1_start, axis1_end,
                   axis2_start, axis2_end,
                   data_size_x, data_size_y,
                   pixel_axis,
                   volume_result );
    timer.Stop();
    std::cout << std::endl;
    std::cout << " VecGeom Vector Interface Elapsed time : "<< timer.Elapsed() << std::endl;

    std::stringstream VecGeomimagevec;
    VecGeomimagevec << imagenamebase.str();
    VecGeomimagevec << "_VecGeomVecNav.bmp";
    make_bmp(volume_result, VecGeomimagevec.str().c_str(), data_size_x, data_size_y);



    delete[] volume_result;
  }
  return 0;
}
void XRayWithROOT(int axis,
                 Vector3D<Precision> origin,
                 Vector3D<Precision> bbox,
                 Vector3D<Precision> dir,
                 double axis1_start, double axis1_end,
                 double axis2_start, double axis2_end,
                 int data_size_x,
                 int data_size_y,
                 double pixel_axis,
                 int * image) {

if(VERBOSE){
    std::cout << "from [" << axis1_start << ";" << axis2_start
              << "] to [" << axis1_end   << ";" << axis2_end << "]\n";
    std::cout << "Xpixels " << data_size_x << " YPixels " << data_size_y << "\n";

    std::cout << pixel_axis << "\n";
}

double pixel_width_1 = (axis1_end-axis1_start)/data_size_x;
double pixel_width_2 = (axis2_end-axis2_start)/data_size_y;

    if(VERBOSE){
    std::cout << pixel_width_1 << "\n";
    std::cout << pixel_width_2 << "\n";
    }

    for( int pixel_count_2 = 0; pixel_count_2 < data_size_y; ++pixel_count_2 ){
        for( int pixel_count_1 = 0; pixel_count_1 < data_size_x; ++pixel_count_1 )
        {
            double axis1_count = axis1_start + pixel_count_1 * pixel_width_1 + 1E-6;
            double axis2_count = axis2_start + pixel_count_2 * pixel_width_2 + 1E-6;

            if(VERBOSE) {
                std::cout << "\n OutputPoint("<< axis1_count<< ", "<< axis2_count<< ")\n";
            }
            // set start point of XRay
            Vector3D<Precision> p;

            if( axis== 1 )
              p.Set( origin[0]-bbox[0], axis1_count, axis2_count);
            else if( axis== 2)
              p.Set( axis1_count, origin[1]-bbox[1], axis2_count);
            else if( axis== 3)
              p.Set( axis1_count, axis2_count, origin[2]-bbox[2]);

            TGeoNavigator * nav = gGeoManager->GetCurrentNavigator();
            nav->SetCurrentPoint( p.x(), p.y(), p.z() );
            nav->SetCurrentDirection( dir.x(), dir.y(), dir.z() );

            double distancetravelled=0.;
            int crossedvolumecount=0;
            double accumulateddensity =0.;

            if(VERBOSE) {
              std::cout << " StartPoint(" << p[0] << ", " << p[1] << ", " << p[2] << ")";
              std::cout << " Direction <" << dir[0] << ", " << dir[1] << ", " << dir[2] << ">"<< std::endl;
            }

            // propagate until we leave detector
            TGeoNode const * node = nav->FindNode();
            TGeoMaterial const * curmat = node->GetVolume()->GetMaterial();

          //  std::cout << pixel_count_1 << " " << pixel_count_2 << " " << dir << "\t" << p << "\t";
          //  std::cout << "IN|OUT" << nav->IsOutside() << "\n";
          //  if( node ) std::cout <<    node->GetVolume()->GetName() << "\t";
            while( node !=NULL ) {
                node = nav->FindNextBoundaryAndStep( vecgeom::kInfinity );
                distancetravelled+=nav->GetStep();
                accumulateddensity+=curmat->GetDensity() * distancetravelled;

                if(VERBOSE) {
                    if( node != NULL ){
                        std::cout << "  VolumeName: "<< node->GetVolume()->GetName();
                    }
                    else
                       std::cout << "  NULL: ";

                    std::cout << " step[" << nav->GetStep()<< "]"<< std::endl;
                    double const * pROOT = nav->GetCurrentPoint();
                    p = Vector3D<Precision>(pROOT[0],pROOT[1],pROOT[2]);
                    std::cout << " point(" << p[0] << ", " << p[1] << ", " << p[2] << ")";
                }
                // Increase passed_volume
                // TODO: correct counting of travel in "world" bounding box
                crossedvolumecount++;
                curmat = (node!=0) ? node->GetVolume()->GetMaterial() : 0;
            } // end while
            // std::cout << crossedvolumecount << "\n";

            ///////////////////////////////////
            // Store the number of passed volume at 'volume_result'
            *(image+pixel_count_2*data_size_x+pixel_count_1) = crossedvolumecount;// accumulateddensity ;// crossedvolumecount;

            if(VERBOSE) {
                std::cout << "  EndOfBoundingBox:";
                std::cout << " PassedVolume:" << "<"<< crossedvolumecount << " ";
                std::cout << " step[" << nav->GetStep()<< "]";
                std::cout << " Distance: " << distancetravelled<< std::endl;
            }
      } // end inner loop
    } // end outer loop
} // end XRayWithROOT