#include "TriggerObjects/SVDD_StorableBank.hh"
#include "Stntuple/mod/StntupleUtilities.hh"

//_____________________________________________________________________________
Int_t UnpackSvtBeamline(AbsEvent* event, float beam3D[4])
{

  // Now get beam spot information from SVDD bank
  errlog.setSubroutine("StntupleSvtBeamline"); 
  std::string errmsg_no_bank = "No real SVDD_StorableBank found!";
  std::string errmsg_2_bank  = "More than one real SVDD_StorableBank found!";
 
  // get (x,y) beam position (cm) in the 6 barrels 
  // as computed by Beam Fit Online Routine

  int     bank_counter = 0 ;

  ConstHandle<SVDD_StorableBank> handle;

  EventRecord::ConstIterator iterd(event,  "SVDD_StorableBank" );
  if(!iterd.is_valid()){
      ERRLOG( ELerror, errmsg_no_bank) << endmsg;
      return (-1);
  }
  ConstHandle<SVDD_StorableBank> hand(iterd);

  float bx[6];
  float by[6];
  int bfs[6];
  short int x[6];
  short int y[6];
  for(int i=0;i<6;i++) {
    x[i] = -999;
    y[i] = -999;
    bfs[i] = -999;
    bx[i] = -999.;
    by[i] = -999.;
  }
  for(int i=0;i<4;i++) {
    beam3D[i] = -999.;
  }
  int fs3D = 0;
  int beamfs = 0;
  int beaminfo_found   = 0;
  int beaminfo3d_found = 0;
  int card = 0;
  int iblock = 0;
  
  Trigger_StorableBank::ConstBlockIter triter(hand, iblock, card);
  /* 
   * Loop through blocks/cards looking for a BEAMINFO card section 
   */ 
  for (ConstBankIterTDC block(hand); block.is_valid(); ++block) { 
    iblock = block.block_index(); 
    for (ConstBlockIterTDC module(block); module.is_valid(); ++module) { 
      enum { BEAMINFO = 10 };
      enum { BEAMINFO3D = 11 };
      int icard = module.module_index(); 
      int dlen = module.module_data_size(); 
      if (dlen==0) continue; 
      if ((module.data_word(0)>>16 & 0xffff)==BEAMINFO3D){
        beaminfo_found = 1;
        //handle->print();
        int nsvdd_words = module.module_data_size()-2;
        for (int i=1;i<=6;i++) {
          int word = module.data_word(i);
          x[i-1] = ((word >> 16) & 0xFFFF);
          y[i-1] = ((word) & 0xFFFF);
        }	      
        beamfs = module.data_word(7);
        //beamfs: beam fit status word == 5 bits for each barrel (30 bits tot):
        //          bits 1 to 3 = "qfit", bits 4 to 5 = "errcode"  
        // this how Marco Rescigno builds it:
        //   for(int barrel=0;barrel<6;barrel++) 
        //       beamfs |= ((qfit & 0x7) | (errcode & 0x3) << 3) << 5*barrel 
        for (int i=0;i<6;i++){
          bfs[i] = (beamfs >> 5*i) & 0x1F;
          bx[i] = x[i]/10000.;
          by[i] = y[i]/10000.;
        }
	beam3D[0] = bx[0];
	beam3D[1] = by[0];
	beam3D[2] = 0;
	beam3D[3] = 0;
      }
      if ((module.data_word(0)>>16 & 0xffff)==BEAMINFO3D) { 
        beaminfo3d_found = 1;
        short sbx = ((module.data_word(1) >> 16) & 0xFFFF);    // microns
        short sby = (module.data_word(1) & 0xFFFF) ;           // microns
        short sdxdz = ((module.data_word(2) >> 16) & 0xFFFF);  // microrads
        short sdydz = (module.data_word(2) & 0xFFFF) ;         // microrads
        beam3D[0] = sbx/10000.;
        beam3D[1] = sby/10000.;
        beam3D[2] = sdxdz/1000000.;
        beam3D[3] = sdydz/1000000.;
        if(module.data_word(3)!=0) fs3D = 1;
      }
    }
    if (beaminfo_found && beaminfo3d_found) break;
  }
  
  if(beaminfo_found ==0 && beaminfo3d_found==0) {
    ERRLOG(ELwarning2,"SVDD beam position data not found!") << endmsg;
    return -1; 
  }

  return 0;
}