//
// $Id: smt_global_to_local.cpp,v 1.2 2005/04/03 15:36:37 shfu Exp $
//
// File: global_to_local.cpp
// Purpose: Shell giving command line conversion of hits from global_to_local
//          coordinates.  Uses default geometry and reads from standard input
// Created: 02-JUN-1998   John Hobbs
//
// Jan 24, 2005 by Shaohua Fu
// added Layer0 // SFL0_Jan_2005
//
// $Revision: 1.2 $
//
//

// Include files
#include "d0om/d0_Ref.hpp"
#include "silicon_geometry/base/SiGeometer.hpp"
#include "silicon_geometry/base/SiBaseGeometry.hpp"
#include "silicon_geometry/utils/smt_build_default.hpp"
#include "geometry_system/components/CartesianCoordinate.hpp"
#include <spacegeom/SpacePoint.h>

std::ostream& operator<<(std::ostream &os, const dgs::CartesianCoordinate& pt) {
  return os << "(" << pt.x() << "," << pt.y() << "," << pt.z() << ")";
}

using namespace std;
using namespace dgs;

int main()
//
// Purpose: 
//
// Arguements:
//
// Returns:
//
{
  // radius of new beam pipe for RunIIb // added SFL0_Jan_2005
  const double RBEAMPIPE = 2.202; // added SFL0_Jan_2005

  // Instantiate the detector
  d0_Ref<const SiBaseGeometry> theDetector = smt_build_default();
  SiGeometer* theGeometer = SiGeometer::get_instance();

  // Build a vector of barrel z end points and instances...
  vector<double> barrel_zmax;
  vector<const SiBarrel*> barrels;
  const SiCenter* sic = theDetector->get_center(1);
  for( int ic=0 ; ic<sic->get_central_count() ; ic++ ) {
    const SiCentral* sice = sic->get_central(ic+1);
    for( int ib=0 ; ib<sice->get_barrel_count(); ib++ ) {
      const SiBarrel* sib = sice->get_barrel(ib+1);
      const SiLayer* sil = sib->get_layer(1);  // Assume aligned geometry...
      CartesianCoordinate end(0.0,0.0,sil->get_zhalf());
      SpacePoint gend=sib->local_to_global(end);
      barrel_zmax.push_back(gend.z());
      barrels.push_back(sib);
    }
  }

  // added SFL0_Jan_2005
  // Build a vector of layer 0 barrel z end points and instances...
  vector<double> l0barrel_zmax;
  vector<const SiBarrel*> l0barrels;
  const SiL0* sil0 = theDetector->get_l0(1);
  for( int ib=0 ; ib<sil0->get_barrel_count(); ib++ ) {
    const SiBarrel* sib = sil0->get_barrel(ib+1);
    const SiLayer* sil = sib->get_layer(1);  // Assume aligned geometry...
    CartesianCoordinate end(0.0,0.0,sil->get_zhalf());
    SpacePoint gend=sib->local_to_global(end);
    l0barrel_zmax.push_back(gend.z());
    l0barrels.push_back(sib);
  }
  // end of SFL0_Jan_2005



  // Build a vector of layer maximum radii.   Assume layers parallel
  // to the global z-axis.
  vector< vector<double> > layer_rmax;
  for( int ib=0 ; ib<barrels.size() ; ib++ ) {
    const SiBarrel* sib = barrels[ib];
    vector<double> empty;
    layer_rmax.push_back(empty);

    // Look at all layers in this barrel one-at-a-time
    for( int il=0 ; il<sib->get_layer_count() ; il++ ) {
      const SiLayer* layer = sib->get_layer(il+1);

      // Find the largest radius in this layer.  
      double rlmax=0.0;
      for( int ilad=0 ; ilad<layer->get_ladder_count() ; ilad++ ) {
	const SiLadder* ladder = layer->get_ladder(ilad+1);
	double dx=ladder->get_xsize();
	CartesianCoordinate xmin(-dx,0.0,0.0), xmax(dx,0.0,0.0);
	SpacePoint gmin = ladder->local_to_global(xmin);
	SpacePoint gmax = ladder->local_to_global(xmax);
	double rmin = sqrt(gmin.x()*gmin.x()+gmin.y()*gmin.y());
	double rmax = sqrt(gmax.x()*gmax.x()+gmax.y()*gmax.y());
	if( rmin>rmax && rmin>rlmax ) rlmax=rmin;
	if( rmin<=rmax && rmax>rlmax ) rlmax=rmax;
      }
      
      // And store the maximum radius 
      layer_rmax[ib].push_back(rlmax);
    }
  }

  // added SFL0_Jan_2005
  // Build a vector of layer0 layer maximum radii.   Assume layers parallel
  // to the global z-axis.
  // Well, not really useful, since layer0's two layers are too close!
  vector< vector<double> > l0layer_rmax;
  for( int ib=0 ; ib<l0barrels.size() ; ib++ ) {
    const SiBarrel* sib = l0barrels[ib];
    vector<double> empty;
    l0layer_rmax.push_back(empty);

    // Look at all layers in this layer0 barrel one-at-a-time
    for( int il=0 ; il<sib->get_layer_count() ; il++ ) {
      const SiLayer* layer = sib->get_layer(il+1);

      // Find the largest radius in this layer.  
      double rlmax=0.0;
      for( int ilad=0 ; ilad<layer->get_ladder_count() ; ilad++ ) {
	const SiLadder* ladder = layer->get_ladder(ilad+1);
	double dx=ladder->get_xsize();
	CartesianCoordinate xmin(-dx,0.0,0.0), xmax(dx,0.0,0.0);
	SpacePoint gmin = ladder->local_to_global(xmin);
	SpacePoint gmax = ladder->local_to_global(xmax);
	double rmin = sqrt(gmin.x()*gmin.x()+gmin.y()*gmin.y());
	double rmax = sqrt(gmax.x()*gmax.x()+gmax.y()*gmax.y());
	if( rmin>rmax && rmin>rlmax ) rlmax=rmin;
	if( rmin<=rmax && rmax>rlmax ) rlmax=rmax;
      }
      
      // And store the maximum radius 
      l0layer_rmax[ib].push_back(rlmax);
    }
  }
  // end of SFL0_Jan_2005



  // and process user inputs
  double xglobal, yglobal, zglobal;
  cout << "Usage: input  xglobal yglobal zglobal" << endl; // added SFL0_Jan_2005
  cout << "              0 0 0 to stop" << endl; // added SFL0_Jan_2005
  cin >> xglobal >> yglobal >> zglobal;
  //while( !cin.eof() ) { // cmt.out SFL0_Jan_2005
  while( !cin.eof() && !(xglobal==0 && yglobal==0) ) { // added SFL0_Jan_2005

    // Convert to some necessary coordinates.
    double rglobal = sqrt(xglobal*xglobal + yglobal*yglobal);

    int dettype = 1; // dettype=1 for barrel, dettype=0 for layer0 // added SFL0_Jan_2005
    if (rglobal < RBEAMPIPE) dettype = 0; // added SFL0_Jan_2005


    // Find the barrel and layer for this hit (Because of overlap, finding
    // the ladder will be trickier.  Do this below...
   if (dettype==1) { // added SFL0_Jan_2005
    int ibar=0,ilay=0;
    while( ibar<barrel_zmax.size() && zglobal>barrel_zmax[ibar] ) ++ibar;
    while( ibar<barrel_zmax.size()          // Do this only if valid barrels
	   && ilay<layer_rmax[ibar].size() // and the valid layer range
	   && rglobal>layer_rmax[ibar][ilay] ) ++ilay;

    // Loop over all ladders, keeping the result closest to ladder local org as correct
    if( ibar<barrel_zmax.size() ) {
      CartesianCoordinate g(xglobal,yglobal,zglobal),lc_close;
      int ilclose=-1;
      double dclose=9999.9;
      const SiLayer* layer = barrels[ibar]->get_layer(ilay+1);
      for( int ilad=0 ; ilad<layer->get_ladder_count() ; ilad++ ) {
	//const SiLadder* theLadder = theGeometer->get_ladder(ibar+1,ilay+1,ilad+1); // cmt.out SFL0_Jan_2005
	const SiLadder* theLadder = theGeometer->get_ladder(0,ibar+1,ilay+1,ilad+1); // SFL0_Jan_2005
	if( theLadder ) {
	  CartesianCoordinate local = theLadder->global_to_local(g);
	  double d=sqrt(local.x()*local.x()+local.y()*local.y());
	  if( d<dclose ) {
	    ilclose = ilad;
	    dclose=d;
	    lc_close = local;
	  }
	}
	else std::cerr << "Error getting (barrel,layer,ladder) = ("
		  << ibar+1 << "," << ilay+1 << "," << ilad+1 << ")" << endl;
      }
      if( ilclose != (-1) ) cout << g << " -> " 
				<< ibar+1 << ", " << ilay+1 << ", " << ilclose+1 
				<< ": " << lc_close << endl;
    }
    else std::cerr << "Error.  Barrel ID = " << ibar << " outside of range " << endl;
   } // added SFL0_Jan_2005
   // added SFL0_Jan_2005
   else if (dettype==0) { // layer0
    int ibar=0;
    while( ibar<l0barrel_zmax.size() && zglobal>l0barrel_zmax[ibar] ) ++ibar;
    // Loop over all ladders, keeping the result closest to ladder local org as correct
    if( ibar<l0barrel_zmax.size() ) {
      CartesianCoordinate g(xglobal,yglobal,zglobal),lc_close;
      int iladclose=-1, ilayclose=-1;
      double dclose=9999.9;
      for( int ilay=0; ilay<l0layer_rmax[ibar].size(); ++ilay ) {
	const SiLayer* layer = l0barrels[ibar]->get_layer(ilay+1);
	for( int ilad=0 ; ilad<layer->get_ladder_count() ; ilad++ ) {
	  const SiLadder* theLadder = theGeometer->get_ladder_L0(3,ibar+1,ilay+1,ilad+1);
	  if( theLadder ) {
	    CartesianCoordinate local = theLadder->global_to_local(g);
	    double d=sqrt(local.x()*local.x()+local.y()*local.y());
	    if( d<dclose ) {
	      iladclose = ilad;
	      ilayclose = ilay;
	      dclose=d;
	      lc_close = local;
	    }
	  }
	  else {
	    int iilay=ilay+1, iilad=ilad+1;
	    if (iilay==1) {
	      if (iilad==1) iilad=1;
	      else if (iilad==2) iilad=3;
	      else if (iilad==3) iilad=5;
	    }
	    else if (iilay==2) {
	      if (iilad==1) iilad=2;
	      else if (iilad==2) iilad=4;
	      else if (iilad==3) iilad=6;
	      iilay=1;
	    }
	    std::cerr << "Error getting layer0 (barrel,layer,ladder) = ("
		      << ibar+1 << "," << iilay << "," << iilad << ")" << endl;
	  }
	}
      }
      if( iladclose != (-1) ) {
	int iilay=ilayclose+1, iilad=iladclose+1;
	if (iilay==1) {
	  if (iilad==1) iilad=1;
	  else if (iilad==2) iilad=3;
	  else if (iilad==3) iilad=5;
	}
	else if (iilay==2) {
	  if (iilad==1) iilad=2;
	  else if (iilad==2) iilad=4;
	  else if (iilad==3) iilad=6;
	  iilay=1;
	}
	cout << g << " -> layer0  " 
	     << ibar+1 << ", " << iilay << ", " << iilad 
	     << ": " << lc_close << endl;
      }
    }
    else std::cerr << "Error.  Layer0 Barrel ID = " << ibar << " outside of range " << endl;
   }
   // end of SFL0_Jan_2005

    // Get the next point
    cin >> xglobal >> yglobal >> zglobal;
  }

  return 0;
}