#include <map>
#include <set>
#include <iostream>
#include "IFieldManager.hxx"
#include "ICOMETLog.hxx"
#include "TVector3.h"
#include "TFile.h"
#include "TTree.h"
#include <math.h>

/// \file IFieldManager.cxx

using namespace std;


 /// The static member pointer to the singleton.
COMET::IFieldManager* COMET::IFieldManager::fTFieldManager = 0;

bool COMET::IFieldManager::fFieldManagerIsInitialized = false; 

COMET::IOAMagneticField* COMET::IFieldManager::fMagField;


//*****************************************************************************
COMET::IFieldManager& COMET::Bman(){
//*****************************************************************************
  return IFieldManager::Get();
}


//*****************************************************************************
void COMET::IFieldManager::InitializeFieldManager() {
//*****************************************************************************
  if (fFieldManagerIsInitialized){
    cout<<"WARNING in IFieldManager::InitializeFieldManager; Is already initialized!"<<endl;
    return;
  }
  fMagField = new COMET::IOAMagneticField();
  fFieldManagerIsInitialized = true;   
  
}

//*****************************************************************************
COMET::IFieldManager::IFieldManager() {
//*****************************************************************************

  InitializeFieldManager();
  //create members on heap which will hold the B-Field mapping information
  //until the IFieldManager destructor is called
 
    fPtrBMap2 = new std::map<TVector3, TVector3, classcomp>;
    fPtrNodesX2 = new std::set<float>;
    fPtrNodesY2 = new std::set<float>;
    fPtrNodesZ2 = new std::set<float>;
    fPtrBMap3 = new std::map<TVector3, TVector3, classcomp>;
    fPtrNodesX3 = new std::set<float>;
    fPtrNodesY3 = new std::set<float>;
    fPtrNodesZ3 = new std::set<float>;



}

//*****************************************************************************
COMET::IFieldManager::~IFieldManager() {
//*****************************************************************************  
  delete fPtrBMap2;
  delete fPtrNodesX2;
  delete fPtrNodesY2;
  delete fPtrNodesZ2;
  delete fPtrBMap3;
  delete fPtrNodesX3;
  delete fPtrNodesY3;
  delete fPtrNodesZ3;
}


//*****************************************************************************
COMET::IFieldManager& COMET::IFieldManager::Get(void) {
//*****************************************************************************
  
  if (!fTFieldManager) {
    COMETLog("Create a new IFieldManager object.");
    fTFieldManager = new COMET::IFieldManager;
    // Load B-Field maps only once on intitialization of IFieldManager
    fTFieldManager->LoadMapData();
  }
  
  return *fTFieldManager;
}

//*****************************************************************************
COMET::IOAMagneticField& COMET::IFieldManager::GetMagneticField() {
//*****************************************************************************
  return *fMagField;
}

//*****************************************************************************
void COMET::IFieldManager::SetMagneticFieldType(int fieldType) {
//*****************************************************************************
  Get().GetMagneticField().SetFieldType(fieldType);
}

//*****************************************************************************
int COMET::IFieldManager::GetMagneticFieldType() {
//*****************************************************************************
  return Get().GetMagneticField().GetFieldType();
}

//*****************************************************************************
void COMET::IFieldManager::SetFieldError(double err) {
//*****************************************************************************
  Get().GetMagneticField().SetFieldError(err);
}

//*****************************************************************************
void COMET::IFieldManager::SetFieldStrength(double bMax) {
//*****************************************************************************
  Get().GetMagneticField().SetFieldStrength(bMax);
}

//*****************************************************************************
void COMET::IFieldManager::SetFieldValue(double x, double y, double z) {
//*****************************************************************************
  Get().GetMagneticField().SetFieldValue(x,y,z);
  std::cout << "B Field = (" << x << " , " << y << " , " << z << " ) " << std::endl;
}

//*****************************************************************************
TVector3 COMET::IFieldManager::GetFieldValue(const TLorentzVector point) {
//*****************************************************************************
  return GetFieldValue(point.Vect());
}

//*****************************************************************************
TVector3 COMET::IFieldManager::GetFieldValue(const TVector3 point) {
//*****************************************************************************
  return Get().GetMagneticField().GetFieldValue(point);
}

//*****************************************************************************
TVector3 COMET::IFieldManager::GetFieldValue(const TLorentzVector point, int type) {
//*****************************************************************************
  return GetFieldValue(point.Vect(), type);
}

//*****************************************************************************
TVector3 COMET::IFieldManager::GetFieldValue(const TVector3 point, int type) {
//*****************************************************************************
  return Get().GetMagneticField().GetFieldValue(point, type);
}

   

//*****************************************************************************
void COMET::IFieldManager::LoadMapData() {
//*****************************************************************************

  std::map<TVector3, TVector3, classcomp> map_B2, map_B3;
  std::set<float> nodes_x2, nodes_x3;
  std::set<float> nodes_y2, nodes_y3;
  std::set<float> nodes_z2, nodes_z3;
  std::set<float>::iterator it;

  //load data from root files
  //TFile f1 ("$CALIBMAGNETROOT/mapdata/P2P_arm1.root","read");
  TFile f2 ("$CALIBMAGNETROOT/mapdata/P2P_arm2.root","read");
  TFile f3 ("$CALIBMAGNETROOT/mapdata/P2P_arm3.root","read");
  std::cout << "Read mag field map files" << std::endl;
  float x,y,z,Bx,By,Bz;
  
  int xidx,yidx,zidx;
 

  //read data taken with arm 2
  f2.cd();
  TTree *tree2 = (TTree *)gDirectory->Get("tree");
  tree2->SetBranchAddress("x",&x);
  tree2->SetBranchAddress("y",&y);
  tree2->SetBranchAddress("z",&z);
  tree2->SetBranchAddress("Bx",&Bx);
  tree2->SetBranchAddress("By",&By);
  tree2->SetBranchAddress("Bz",&Bz);
	
  int nentries2 = (int) tree2->GetEntries();
  for (int i=0; i<nentries2; i++) {
    tree2->GetEntry(i);
    TVector3 point(x,y,z);
    TVector3 B_vec(Bx, By, Bz);
	  
    //fill the container structure
    nodes_x2.insert(x);
    nodes_y2.insert(y);	
    nodes_z2.insert(z);		
    map_B2[point] = B_vec;

    float dx = 57.0;
    float dy = 49.5;
    float dz = 50.0;
    float xfirst = -1083.0;
    float yfirst = -804.70001220703125;
    float zfirst = -887.9000244140625;
    if(x < xfirst) x = xfirst;
    xidx = (int) round( (x-xfirst)/dx);
    if(y < yfirst) y = yfirst;
    yidx = (int) round( (y-yfirst)/dy ); 
    if(z < zfirst) z = zfirst;
    zidx = (int) round( (z-zfirst)/dz );
    //std::cout << " xidx " << xidx << " yidx " << yidx << " " << zidx << std::endl;
   
    //if( zidx == 70 && xidx == 29){
      //std::cout << zidx << " " << xidx << std::endl;
      //std::cout << "zidx " <<zidx <<  " z " << z << " " <<(z-zfirst)/dz <<  std::endl;
     
      //std::cout << " yidx " << yidx << " y " << y << " B " <<  B_vec.X() << " " <<  B_vec.Y() << " " <<  B_vec.Z() <<std::endl;
    //} 
    magfieldarm2[zidx*1599+yidx*39+xidx]= B_vec;
    if(yidx == 5)  {
      //std::cout << " Set Field " << std::endl;
      magfieldarm2[zidx*1599+(yidx+1)*39+xidx]= B_vec;
    }
}

  
  *fPtrBMap2 = map_B2;
  *fPtrNodesX2 = nodes_x2;
  *fPtrNodesY2 = nodes_y2;
  *fPtrNodesZ2 = nodes_z2;
	
  //do the same for the data taken with arm 3
  f3.cd();
  TTree *tree3 = (TTree *)gDirectory->Get("tree");
  tree3->SetBranchAddress("x",&x);
  tree3->SetBranchAddress("y",&y);
  tree3->SetBranchAddress("z",&z);
  tree3->SetBranchAddress("Bx",&Bx);
  tree3->SetBranchAddress("By",&By);
  tree3->SetBranchAddress("Bz",&Bz);
	
  int nentries3 = (int) tree3->GetEntries();
  for (int i=0; i<nentries3; i++) {
    tree3->GetEntry(i);
    TVector3 point(x,y,z);
    TVector3 B_vec(Bx, By, Bz);
    
    //clear and fill the container structure
    nodes_x3.insert(x);
    nodes_y3.insert(y);	
    nodes_z3.insert(z);		
    map_B3[point] = B_vec;
    
    float dx = 171.0;
    float dy = 49.5;
    float dz = 50.0;
    float xfirst = -855.0;
    float yfirst = -1059.699951171875;
    //float yfirst = -1059.701;
    float zfirst = -686.5;
    if(x < xfirst) x = xfirst;
    xidx = (int) round( (x-xfirst)/dx);
    if(y < yfirst) y = yfirst;
    yidx = (int) round( (y-yfirst)/dy ); 
    if(z < zfirst) z = zfirst;
    zidx = (int) round( (z-zfirst)/dz );
   
    magfieldarm3[zidx*1599+yidx*39+xidx]= B_vec; // change indexing maybe ?

  
  }
	
  *fPtrBMap3 = map_B3;
  *fPtrNodesX3 = nodes_x3;
  *fPtrNodesY3 = nodes_y3;
  *fPtrNodesZ3 = nodes_z3;
  //*fPtrMagArm3 =magfieldarm3;

}

  
//*****************************************************************************
std::map <TVector3, TVector3, classcomp> *COMET::IFieldManager::GetBMap(int arm_nr) {
//*****************************************************************************
  if (arm_nr == 2) {
    if (fPtrBMap2->size()==0) {
      std::cerr << "ERROR: B-Field map has not been loaded properly." << std::endl;
    }
    return fPtrBMap2;  
  }
  
  if (arm_nr == 3) {
    if (fPtrBMap3->size()==0) {
      std::cerr << "ERROR: B-Field map has not been loaded properly." << std::endl;
    }
    return fPtrBMap3;
  }

  return 0;
}

/*

void COMET::IFieldManager::GetBMap2Arr(int arm_nr, TVector3 magarm[][41][78]) {
  //void COMET::IFieldManager::GetBMap2Arr(int arm_nr, TVector3 magarm) {



  //magarm = magfieldarm2;
    if (arm_nr == 2) {
    // if (fPtrMagArm2->size()==0) {
    //std::cerr << "ERROR: B-Field map has not been loaded properly." << std::endl;
    //}
      //magarm = magfieldarm2;
     magarm = fPtrMagArm2;
   //return fPtrMagArm2;
  }
  
    
  if (arm_nr == 3) {
    //if (fPtrMagArm3->size()==0) {
    // std::cerr << "ERROR: B-Field map has not been loaded properly." << std::endl;
    //}
    magarm = magfieldarm3;
    //magarm = fPtrMagArm3;
    //return fPtrMagArm3;
  }
    
  //return 0;

}
*/
//*****************************************************************************
TVector3 *COMET::IFieldManager::GetBMapArr(int arm_nr ) {
//void COMET::IFieldManager::GetBMap2Arr(int arm_nr, TVector3 magarm) {
  //*****************************************************************************
  if (arm_nr == 2) {
    // if (fPtrMagArm2->si
  return magfieldarm2;
  }
  if (arm_nr == 3) {
    // if (fPtrMagArm2->si
  return magfieldarm3;
  }
  return 0;
}

//*****************************************************************************
std::set<float> *COMET::IFieldManager::GetNodesX(int arm_nr) {
//*****************************************************************************
  if (arm_nr == 2) {
    if (fPtrNodesX2->size()==0) {
      std::cerr << "ERROR: x nodes have not been loaded properly." << std::endl;
    }
    return fPtrNodesX2;
  }
  
  if (arm_nr == 3) {
    if (fPtrNodesX3->size()==0) {
      std::cerr << "ERROR: x nodes have not been loaded properly." << std::endl;
    }
  return fPtrNodesX3;
  }
  
  return 0;
}

//*****************************************************************************
std::set<float> *COMET::IFieldManager::GetNodesY(int arm_nr) {
//*****************************************************************************
  if (arm_nr == 2) {
    if (fPtrNodesY2->size()==0) {
      std::cerr << "ERROR: y nodes have not been loaded properly." << std::endl;
    }
    return fPtrNodesY2;
  }
  
  if (arm_nr == 3) {
    if (fPtrNodesY3->size()==0) {
      std::cerr << "ERROR: y nodes have not been loaded properly." << std::endl;
    }
    return fPtrNodesY3;
  }
  
  return 0;
}

//*****************************************************************************
std::set<float> *COMET::IFieldManager::GetNodesZ(int arm_nr) {
//*****************************************************************************
  if (arm_nr == 2) {
    if (fPtrNodesZ2->size()==0) {
      std::cerr << "ERROR: z nodes have not been loaded propperly." << std::endl;
    }
    return fPtrNodesZ2;
  }
  
  if (arm_nr == 3) {
    if (fPtrNodesZ3->size()==0) {
      std::cerr << "ERROR: z nodes have not been loaded propperly." << std::endl;
    }
    return fPtrNodesZ3;
  }
  
  return 0;
}