/*
 * PitchAngleExpositionAction.cpp
 *
 *  Created on: 04-april-2010
 *      Author: Vitaly Malakhov
 */

/*! @file PitchAngleExpositionAction.cpp The PitchAngleExpositionAction class implementation file. */

#include "PitchAngleExpositionAction.h"
#include "TMatrixD.h"
#include "OrientationInfo.h"

#define XBINS_F 3600
#define YBINS_F 400

PitchAngleExpositionAction::PitchAngleExpositionAction(const char *actionName, TString outFileBase, TString mode,
    bool outRoot, bool outText, TString title, bool CalcPitchExpo, UInt_t nPitchBins, Float_t lowerPitch, Float_t upperPitch) :
    Histo2DAction<Float_t> (actionName, title, outFileBase, mode, outRoot, outText), _calcPitchExpo(CalcPitchExpo), _nPitchBins(nPitchBins), _lowerPitch(lowerPitch), _upperPitch(upperPitch) {

}

void PitchAngleExpositionAction::OnGood(PamLevel2 *event) {
    Float_t Bx = -event->GetOrbitalInfo()->Bdown;
    Float_t By = event->GetOrbitalInfo()->Beast;
    Float_t Bz = event->GetOrbitalInfo()->Bnorth;
    OrientationInfo* PO = new OrientationInfo();
    TMatrixD Fij = PO->ECItoGreenwich(PO->QuatoECI(event->GetOrbitalInfo()->q0,event->GetOrbitalInfo()->q1,event->GetOrbitalInfo()->q2,event->GetOrbitalInfo()->q3),event->GetOrbitalInfo()->absTime);
    TMatrixD Dij = PO->GreenwichtoGEO(event->GetOrbitalInfo()->lat,event->GetOrbitalInfo()->lon,Fij);
    TMatrixD Iij = PO->ColPermutation(Dij);
    TMatrixD Sij = Iij;
    Sij.Invert();
    TMatrixD Nij = PO->PamelatoGEO(Sij,Bx,By,Bz);
    Float_t BZen = acos(Nij(2,0)) * TMath::RadToDeg();
    Float_t angle = 0;
    if(Nij(0,0)!=0)angle = atan(Nij(1,0)/Nij(0,0)) * TMath::RadToDeg();else angle = 90.;
    Float_t BAz = 360. + angle;
    if(Nij(0,0) >=0 && Nij(1,0) >=0) BAz = angle;
    if(Nij(0,0)<0) BAz = 180. + angle;
    Float_t weight = event->GetTrigLevel2()->dltime[0] * 0.16 /1000.;
    Fill(BAz, BZen, weight);
    PO->Delete();
}

void PitchAngleExpositionAction::Finalize() {

    Histo2DAction<Float_t>::Finalize();

    if(_calcPitchExpo){
      TH2F tmphisto;
      tmphisto.SetBins(XBINS_F,0,360,YBINS_F,140,180);
      TString tmpname = GetRootHisto()->GetName();
      Float_t delta = (_upperPitch - _lowerPitch)/_nPitchBins;
      for(UInt_t i=0; i<_nPitchBins; i++){
	 _pitchAngleHisto.push_back(tmphisto);
	 TString tmp = tmpname;
	 TString tmp2 = "";
	 tmp+=i*delta;
	 tmp+="_";
	 tmp+=(i+1)*delta;
	 tmp+="deg";
	 for(Int_t j = 0; j<tmp.Sizeof()-1; j++){
	   if(tmp[j] == '.') tmp[j] = 'p';
	   if(tmp[j] == ' '){
	     tmp.Replace(j,1,NULL);
	     j--;
	   }
	 }
        cout<<"size is "<<_pitchAngleHisto.size()<<"\t"<<tmp<<endl;
        _pitchAngleHisto[i].SetName(tmp);
        _pitchAngleHisto[i].SetTitle(tmp);
        cout<<"PitchHisto["<<i<<"] name is "<<_pitchAngleHisto[i].GetName()<<endl;
      }

      Float_t BinWidthX = GetRootHisto()->GetXaxis()->GetXmax()/GetRootHisto()->GetNbinsX();
      Float_t BinWidthY = GetRootHisto()->GetYaxis()->GetXmax()/GetRootHisto()->GetNbinsY();
      Float_t tetaB = 0.; //Zenith angle of Magnetic field vector B in PAMELA reference frame
      Float_t fiB = 0.;  //Azimuth angle of Magnetic field vector B in PAMELA reference frame
      Float_t Psi = 0.;   //Pitch-Angle;
      Float_t Ksi = 0.;   //Azimuth-angle according pitch-angle Psi

      Bool_t pth[XBINS_F][YBINS_F];
      for(Int_t i=1;i<=GetRootHisto()->GetNbinsX();i++){
	  cout<<i<<" of "<<GetRootHisto()->GetNbinsX()<<endl;
        for(Int_t j=1;j<=GetRootHisto()->GetNbinsY();j++){
          if(GetRootHisto()->GetBinContent(i,j)!=0){
	     cout<<"j = "<<j<<endl;
	     fiB = TMath::DegToRad()*(i*BinWidthX+BinWidthX/2);
	     tetaB = TMath::DegToRad()*(j*BinWidthY+BinWidthY/2);
	     for(UInt_t iPsi=0;iPsi<_nPitchBins;iPsi++){ //We suggest here that Pitch-angel bin size is 5 degrees;
	       for(UInt_t ip=0;ip<XBINS_F;ip++){
	         for(UInt_t jp=0;jp<YBINS_F;jp++) pth[ip][jp]=false;
		}
		cout<<"iPsi = "<<iPsi<<endl;
	       for(UInt_t jPsi=0;jPsi<delta*10;jPsi++){ //cycle inside one pitch-angle bin;
	         for(UInt_t iKsi=0;iKsi<XBINS_F;iKsi++){ //cycle over azimuth angle according this Pitch-Angle;
	           Psi=TMath::DegToRad()*(iPsi*5+jPsi*0.1);
	           Ksi=TMath::DegToRad()*(iKsi*0.1);
	           Float_t z_ = 0;
	           Float_t x_ = sin(Psi)*cos(Ksi);
	           Float_t y_ = sin(Psi)*sin(Ksi);
	           Float_t x1 = x_*cos(tetaB)+z_*sin(tetaB);
	           Float_t z1 = -x_*sin(tetaB)+z_*cos(tetaB);
	           Float_t y1 = y_;
	      	    Float_t x2 = x1*cos(fiB)+y1*sin(fiB);
	           Float_t y2 = -x1*sin(fiB)+y1*cos(fiB);
	           Float_t z2 = z1;
	           Float_t a2 = pow(sin(Psi),2);
	           Float_t b2 = 2-2*cos(Psi);
	           Float_t l = 1 - sqrt(b2-a2);
	           Float_t xm = l*sin(tetaB)*cos(fiB);
	           Float_t ym = l*sin(tetaB)*sin(fiB);
	           Float_t zm = l*cos(tetaB);
	           Float_t x3 = x2 - xm;
	           Float_t y3 = y2 - ym;
	           Float_t z3 = z2 - zm;
	           Float_t tetaDir = 0.;  //tetaDir and fiDir is a direction for Pitch and Azimuth angle given in Instrumental reference frame
	           if(z3!=0)tetaDir = TMath::RadToDeg()*atan(sqrt(pow(x3,2)+pow(y3,2))/z3);
	           if(z3<0)tetaDir=180+tetaDir;
	           if(z3==0)tetaDir=90.;
	           tetaDir=180-tetaDir;
	           Float_t fiDir = 0;
	           if(x3!=0)fiDir = TMath::RadToDeg()*atan(TMath::Abs(y3)/TMath::Abs(x3));
	           if(x3<0 && y3>0) fiDir = 180. - fiDir;
	           if(x3<0 && y3<0) fiDir+=180.;
	           if(x3>0 && y3<0) fiDir = 360. - fiDir;
	           if(x3==0 && y3>0) fiDir = 90.;
	           if(x3==0 && y3<0) fiDir = 270.;
	           if(x3<0 && y3==0) fiDir = 180.;
		    
		    if(!pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]){
		      _pitchAngleHisto[iPsi].Fill(fiDir,tetaDir,GetRootHisto()->GetBinContent(i,j));
		      pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]=true;
		    }/*
	           if(fiDir<=35.){
		      if(tetaDir>=158.8-0.0035*pow(fiDir,2)){
		   	 if(!pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]){
		          _pitchAngleHisto[iPsi].Fill(fiDir,tetaDir,GetRootHisto()->GetBinContent(i,j));
		          pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]=true;
		        }
		      }
		    }
	      	    if(fiDir>35. && fiDir<=140.){
		      if(tetaDir>=163.2-0.0035*pow(fiDir-90.,2)){
		        if(!pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]){
		     	   _pitchAngleHisto[iPsi].Fill(fiDir,tetaDir,GetRootHisto()->GetBinContent(i,j));
		     	   pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]=true;
		   	 }
		      }
		    }
	      	    if(fiDir>140. && fiDir<=220.){
		      if(tetaDir>=161.5-0.0035*pow(fiDir-180.,2)){
		   	 if(!pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]){
		     	   _pitchAngleHisto[iPsi].Fill(fiDir,tetaDir,GetRootHisto()->GetBinContent(i,j));
		     	   pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]=true;
		   	 }
		      }
		    }
	      	    if(fiDir>220. && fiDir<=320.){
		      if(tetaDir>=163.2-0.0035*pow(fiDir-270.,2)){
		  	 if(!pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]){
		    	   _pitchAngleHisto[iPsi].Fill(fiDir,tetaDir,GetRootHisto()->GetBinContent(i,j));
		    	   pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]=true;
		  	 }
		      }
		    }
	      	    if(fiDir>320. && fiDir<=360.){
		      if(tetaDir>=158.8-0.0035*pow(fiDir-360.,2)){
		  	 if(!(pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1])){
		    	   _pitchAngleHisto[iPsi].Fill(fiDir,tetaDir,GetRootHisto()->GetBinContent(i,j));
		    	   pth[(Int_t)(fiDir/0.1)+1][(Int_t)((tetaDir-140.)/0.1)+1]=true;
		    	 }
		      }
		    }*/
	     	  } // end of loop over iKsi
	  	} // end of loop over jPsi
	     } // end of loop over iPsi
          } // if(h->GetBinContent(i,j)!=0)
        } // end of loop over j
      } // end of loop over i
      TFile outRootFile((Histo2DAction<Float_t>::_outFileBase + ".root"), "UPDATE");
      outRootFile.cd();
      for (UInt_t i=0; i < _nPitchBins ;i++){
	  _pitchAngleHisto[i].Write();
	  cout<<i<<"\t"<<_pitchAngleHisto[i].GetName()<<endl;
      }
      outRootFile.Close();

      cout<<"outFile writing"<<endl;
    }
}