// Author: David Lawrence  June 25, 2004
//
//
// MyProcessor.cc
//

#include <iostream>
#include <sstream>

#include "MyProcessor.h"
#include "HDDM/hddm_s.h"

#include "DCoordinateSystem.h"

#include "HDGEOMETRY/DGeometry.h"
#include "PID/DStartTime.h"
#include "START_COUNTER/DSCHit.h"
#include "TRACKING/DTrackWireBased.h"
#include "PID/DKinematicData.h"
#include "TRACKING/DMCThrown.h"

std::string MyProcessor::NtupleHeader = "runno:i32\teventno:i32\tsector:i32\tde:f32\tt:f32\ttheta:f32\tphi:f32";

MyProcessor::MyProcessor(std::vector<std::string> &args, int max_events, 
			 std::string &output_file, bool batch_mode): 
  jana::JEventProcessor(), 
  _args(args),
  _max_events(max_events),
  _output_file(output_file),
  _batch_mode(batch_mode),

  _ofs(NULL), 
  _runno(0) 

{}

//------------------------------------------------------------------
// init   -Open output file here (e.g. a ROOT file)
//------------------------------------------------------------------
jerror_t MyProcessor::init(void)
{
	// open ROOT file
	// ROOTfile = new TFile("hd_ana.root","RECREATE","Produced by hd_ana");
	// cout<<"Opened ROOT file \"hd_example.root\""<<endl;

	// // Create histogram
	// cdc_ring_vs_straw	= new TH2F("cdc_ring_vs_straw","CDC RING vs. STRAW",200, 0.0, 1000.0, 200, 0.0, 100.0);
	// fcal_y_vs_x		= new TH2F("fcal_y_vs_x","FCAL Y vs. X",200, -100.0, 100.0, 200, -100.0, 100.0);
	// fcalhitE		= new TH1F("fcalhitE","fcal single detector energy(GeV)",100, 0.0, 6.0);
  
  _ofs = new std::ofstream(_output_file.c_str()); 
  (*_ofs) << '#' << NtupleHeader << std::endl;

  return NOERROR;
}

bool MyProcessor::GetSCPlanesBarrel(std::vector<DVector3>& norms, 
				    std::vector<DVector3>& pts,
				    std::vector<double>& ang_edges_low) {
  bool good = true;
  double TO_RADS = 2*M_PI/360.0;

  std::vector<double> phi0;
  std::vector<double> npad;

  std::vector<double> barrel_downstream_end;
  std::vector<double> barrel_upstream_end;

  double barrel_upstream_ri   = 7.322573;
  double barrel_upstream_ro   = 7.623501;
  double barrel_upstream_z    = 0.0;
  double barrel_downstream_ri = barrel_upstream_ri;
  double barrel_downstream_ro = barrel_upstream_ro;
  double barrel_downstream_z  = 50.000000;
  double barrel_center_z      = (barrel_downstream_z - barrel_upstream_z) / 2 + barrel_upstream_z;
  double barrel_center_r      = (barrel_upstream_ro - barrel_upstream_ri) / 2 + barrel_upstream_ri;
     
  good |= _dgeo->Get("//StartCntr_s/section/composition/mposPhi/@Phi0", phi0);
  good |= _dgeo->Get("//StartCntr_s/section/composition/mposPhi/@ncopy", npad);

  if (!good)
    return good;

  double deg_per_pad = 360.0 / npad[0];

  std::cout <<"PHI0! " << phi0[0] << std::endl;

  // Assume paddle 1 covers the range 0 to deg_per_paddle
  for (int i=0; i < npad[0]; i++) {
    double pad_norm_phi = phi0[0] + deg_per_pad*i;
    double pad_norm_th  = 90.0;

    //    std::cout << "PHI! " << pad_norm_phi << std::endl;

    DVector3 norm; 
    norm.SetMagThetaPhi(1, pad_norm_th*TO_RADS, pad_norm_phi*TO_RADS);

    //    std::cout << "PHI! " << pad_norm_phi << " " << atan2(norm.Y(), norm.X())*(1/TO_RADS) << std::endl;
    norms.push_back(norm);

    DVector3 point;
    point.SetMagThetaPhi(sqrt(pow(barrel_center_r, 2) + pow(barrel_center_z, 2)),
			 atan2(barrel_center_r, barrel_center_z),
			 pad_norm_phi*TO_RADS);

    pts.push_back(point);

    // SC angular limits
    // Phi limits for each paddle
    // Theta range for barrel to nose
    // Z-position when track arrives at SC
    // Gaussian w/ track uncertainty in phi, integrate gaussian over phi range with rectangles in
    // the SC angular width, order by largest to smallest in area.

    ang_edges_low.push_back((pad_norm_phi - phi0[0])*TO_RADS);
  }
    // std::cout << "ANGS!!! ";
    // for (unsigned int i=0; i < ang_edges_low.size(); i++) 
    //   std::cout << ang_edges_low[i] << " " << flush;
    // std::cout << std::endl;

  return good;
}  

jerror_t MyProcessor::MatchToSC(const DKinematicData* trk, 
				const std::vector<const DSCHit*>& sc_hits, 
				const DSCHit*& assoc_hit) 
{
  // Want to know how to use covariance matrix to get uncertainty in phi
  // If paddle X gets hit left of center, wanna shade to the paddles on the left
  // if we can't find a hit in paddle X
  // rectangular integral of gaussian of phi, sort by area in box.

  assoc_hit = NULL;

  if (trk == NULL)
    return RESOURCE_UNAVAILABLE;
  if (sc_hits.size() == 0)
    return RESOURCE_UNAVAILABLE;

  double TO_RAD = 2*M_PI/ 360.0;

  std::map<int, const DSCHit*> sector_hit_map;

  for (unsigned int i=0; i < sc_hits.size(); i++) 
    sector_hit_map[sc_hits[i]->sector] = sc_hits[i];
    
  double trk_phi = (trk->momentum().Phi() < 0) ? (trk->momentum().Phi() + 2*M_PI) : trk->momentum().Phi();
  
  // Assume a constant uncertainty of 18 degrees.
  double trk_phi_unc = 18.0 * TO_RAD;
  
  double trk_phi_low  = trk_phi - trk_phi_unc;
  double trk_phi_high = trk_phi + trk_phi_unc;

  int sector_low  = std::lower_bound(_sc_phi_low_edges.begin(), 
				     _sc_phi_low_edges.end(), 
				     trk_phi_low) - _sc_phi_low_edges.begin(); 
  int sector_high  = std::lower_bound(_sc_phi_low_edges.begin(), 
				      _sc_phi_low_edges.end(), 
				      trk_phi_high) - _sc_phi_low_edges.begin(); 
  int sector_guess = std::lower_bound(_sc_phi_low_edges.begin(), 
				      _sc_phi_low_edges.end(), 
				      trk_phi) - _sc_phi_low_edges.begin(); 
    
  int sector_probe = sector_guess;
 
  for (int i=0; i < 40; i++) {
    sector_probe += pow(-1.0, i)*i;
    map<int, const DSCHit*>::iterator sector_hit = sector_hit_map.find(sector_probe);

    if (sector_hit != sector_hit_map.end()) {
      assoc_hit = (*sector_hit).second;
      break;
    }
    
    if ((sector_probe > sector_high) || (sector_probe < sector_low))
      break;
  }


  if (assoc_hit == NULL) {
      std::cout << "MATCHTOSC: " << sc_hits[0]->sector << " " << sector_low << " " << sector_high << " " << sector_guess 
	    << " " << _sc_phi_low_edges[sector_guess-1]*(1/TO_RAD) << " "
		<< trk_phi_low*(1/TO_RAD) << " < " << trk_phi*(1/TO_RAD) << " < " << trk_phi_high*(1/TO_RAD) << std::endl;

    return RESOURCE_UNAVAILABLE; // This is a sucky way to tell you I can't find a hit to match this track.
  }
  
  std::cout << "assoc_hit: " << assoc_hit << std::endl;  
  return NOERROR;
}
  

jerror_t MyProcessor::brun(jana::JEventLoop *evLoop, int runNumber) {
  DApplication* dapp = dynamic_cast<DApplication*>(evLoop->GetJApplication());
  
  if (!dapp) 
    return RESOURCE_UNAVAILABLE;

  _runno = runNumber;

  _dgeo = dapp->GetDGeometry(runNumber);
  
  if (!_dgeo)
    return RESOURCE_UNAVAILABLE;
  
  double tgt_z; 
  double tgt_zlen;
  
  if (!_dgeo->GetTargetZ(tgt_z))
    return RESOURCE_UNAVAILABLE;
  if (!_dgeo->GetTargetLength(tgt_zlen))
    return RESOURCE_UNAVAILABLE;

  _geomap["tgt_z"] = tgt_z;
  _geomap["tgt_zlen"] = tgt_zlen;

  //  std::vector<std::vector<double> > sc_planes;

  //  if (!_dgeo->GetSCPlanesNose(sc_planes))
  //  return RESOURCE_UNAVAILABLE;


  // if (!_dgeo->GetSCPlanesBarrel(sc_barrel_norms, sc_barrel_pts)) {
  //   std::cout << "BLAP!! Failed to get stuff from XML!" << std::endl;
  //   return RESOURCE_UNAVAILABLE;
  // }
  
  GetSCPlanesBarrel(_sc_barrel_norms, _sc_barrel_pts, _sc_phi_low_edges);

  std::ofstream ofsvecs((_output_file + ".mthphi.vecs").c_str());

  double TODEG = 360.0 / (2*M_PI);
  for (unsigned int i=0; i < _sc_barrel_norms.size(); i++) {
    ofsvecs << _sc_barrel_norms[i].Mag() << "\t"
	    << _sc_barrel_norms[i].Theta()*TODEG << "\t"
	    << _sc_barrel_norms[i].Phi()*TODEG << "\t"
	    << _sc_barrel_pts[i].Mag()   << "\t"
	    << _sc_barrel_pts[i].Theta()*TODEG   << "\t"
	    << _sc_barrel_pts[i].Phi()*TODEG   << std::endl;
  }

  ofsvecs.close();

  std::ofstream ofsvecs2((_output_file + ".xyz.vecs").c_str());

  for (unsigned int i=0; i < _sc_barrel_norms.size(); i++) {
    ofsvecs2 << _sc_barrel_norms[i].X() << "\t"
	     << _sc_barrel_norms[i].Y() << "\t"
	     << _sc_barrel_norms[i].Z() << "\t"
	     << _sc_barrel_pts[i].X()   << "\t"
	     << _sc_barrel_pts[i].Y()   << "\t"
	     << _sc_barrel_pts[i].Z()   << std::endl;
  }

  ofsvecs2.close();


  return NOERROR;
}

//------------------------------------------------------------------
// evnt   -Fill histograms here
//------------------------------------------------------------------
jerror_t MyProcessor::evnt(jana::JEventLoop *eventLoop, int eventnumber)
{
	// vector<const DStartTime*> start_times;
	// eventLoop->Get(start_times);

	std::vector<const DSCHit*> sc_hits;
	std::vector<const DTrackWireBased*> wb_trks;
	std::vector<const DMCThrown*> mc_trks;
	
	eventLoop->Get(sc_hits);
	eventLoop->Get(wb_trks);
	eventLoop->Get(mc_trks);
	
	if ((sc_hits.size() != 1) || 
	    (mc_trks.size() < 1)  || 
	    (wb_trks.size() < 1)) 
	  {
	    cout << "BLAP! sc_hits.size() != 1 --> " << sc_hits.size() << endl;
	    return NOERROR;
	  }

	// for (unsigned int i=0; i < sc_hits.size(); i++) 
	//   *(_ofs) << _runno << "\t" << eventnumber << "\t"  << sc_hits[i]->sector << "\t" 
	// 	  << sc_hits[i]->dE << "\t" << sc_hits[i]->t << "\t" 
	// 	  << std::endl;

	// std::cout << "BLAP! TRY DIS! " << eventnumber << std::endl;
	// DVector3 ptrk = wb_trks[0]->momentum();
	// std::cout << "BLAP! WORK GUD!" << std::endl;
	// ptrk.Theta();
	// std::cout << "BLAP! THETAR GUD!" << std::endl;
	// ptrk.Phi();
	// std::cout << "BLAP! PHI GUD!" << std::endl;
	
	const DSCHit* assoc_hit = NULL; 
	MatchToSC(wb_trks[0], sc_hits, assoc_hit);
	
	if (assoc_hit == NULL)
	  std::cout << "MyProcessor::evnt(): MatchToSC failed on event " << eventnumber << ": " << assoc_hit << std::endl;

	*(_ofs) << _runno << "\t" << eventnumber << "\t"  << sc_hits[0]->sector << "\t" 
		<< sc_hits[0]->dE << "\t" << sc_hits[0]->t << "\t" 
		<< mc_trks[0]->momentum().Theta()/(2*M_PI) * 360  << "\t" << mc_trks[0]->momentum().Phi()/(2*M_PI)*360 << std::endl;
	
	//	eventLoop->Print("DSCHit");

	  //	std::cout << start_times.size() <<  std::endl;

	

	// for(unsigned int i=0;i<cdchits.size();i++){
	// 	const DCDCHit *cdchit = cdchits[i];
	// 	float ring = (float) cdchit->ring;
	// 	float straw = (float) cdchit->straw;
	// 	cdc_ring_vs_straw->Fill(straw,ring);
	// }

	// for(unsigned int i=0;i<fcalhits.size();i++){
	// 	const DFCALHit *fcalhit = fcalhits[i];
	// 	fcal_y_vs_x->Fill(fcalhit->y,fcalhit->x);
	// 	fcalhitE->Fill(fcalhit->E);
	// }
	
	return NOERROR;
}


jerror_t MyProcessor::erun() {  
  _sc_phi_low_edges.clear();
  _sc_barrel_norms.clear();
  _sc_barrel_pts.clear();
  _sc_nose_norms.clear();
  _sc_nose_pts.clear();

  return NOERROR;
}


//------------------------------------------------------------------
// fini   -Close output file here
//------------------------------------------------------------------
jerror_t MyProcessor::fini(void)
{
  if (_ofs) {
    _ofs->close();
    delete _ofs;
    _ofs = NULL;
  }

  return NOERROR;
}