// Author: Andrew M. Rogers, NSCL 07/01/2006
// Original Design: Daniel Bazin, NSCL
//* Copyright (C) 2006-2008 Andrew M. Rogers

#include "TS800FpTrack.h"
#include "TS800.h"
#include "nrutil.h"
#include "PhysicalConstants.h"

#include <iostream>
#include <math.h>

#include <TBranch.h>
#include <TMath.h>

#include <TNamed.h>
#include <TList.h>

ClassImp(TS800FpTrack);

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TS800FpTrack                                                         //
//                                                                      //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

using namespace TMath;


//______________________________________________________________________________
TS800FpTrack::TS800FpTrack(const TS800FpTrack & fpTrack) : TObject(fpTrack)
{
  // -- Copy Constructor.
  ((TS800FpTrack&)fpTrack).Copy(*this);
}


//______________________________________________________________________________
void TS800FpTrack::Copy(TObject &fpTrack) const
{
  // -- Copy this method.
  TObject::Copy((TObject&)fpTrack);
  ((TS800FpTrack&)fpTrack).zfp    = zfp;
  ((TS800FpTrack&)fpTrack).anglea = anglea;
  ((TS800FpTrack&)fpTrack).angleb = angleb;
  ((TS800FpTrack&)fpTrack).order  = order;
  //  ((TS800FpTrack&)fpTrack).brho   = brho;
  ((TS800FpTrack&)fpTrack).mass   = mass;
  ((TS800FpTrack&)fpTrack).deltaM = deltaM;
  ((TS800FpTrack&)fpTrack).charge = charge;
  ((TS800FpTrack&)fpTrack).gecorr = gecorr;
  ((TS800FpTrack&)fpTrack).beta0  = beta0;
  ((TS800FpTrack&)fpTrack).name   = name;
}


//______________________________________________________________________________
void TS800FpTrack::InitClass(TString iname, TS800* iparent) 
{
  // -- Initialize the class.
  //

  name   = iname;
  parent = iparent;
  
  zfp    = 0;
  anglea = 0;
  angleb = 0;
  order  = 5;
  //  brho   = 0;
  mass   = 0;
  deltaM = 0;
  charge = 0;
  gecorr = 0;
  
}

//______________________________________________________________________________
void TS800FpTrack::InitTree(TTree *tree) 
{
  // -- Initialize the class to tree.
  //

  fChain   = tree;
  fCurrent = -1;
  //  TNamed *aState = (TNamed*)fChain->GetUserInfo()->FindObject("Analysis State");

  b_xfp     = fChain->GetBranch(name+".xfp");
  b_afp     = fChain->GetBranch(name+".afp");
  b_yfp     = fChain->GetBranch(name+".yfp");
  b_bfp     = fChain->GetBranch(name+".bfp");
  b_ata     = fChain->GetBranch(name+".ata");
  b_yta     = fChain->GetBranch(name+".yta");
  b_bta     = fChain->GetBranch(name+".bta");
  b_dta     = fChain->GetBranch(name+".dta");
  b_azita   = fChain->GetBranch(name+".azita");
  b_scatter = fChain->GetBranch(name+".scatter");
  b_energy  = fChain->GetBranch(name+".energy");
  b_ptot    = fChain->GetBranch(name+".ptot");
  b_ppar    = fChain->GetBranch(name+".ppar");
  b_ptra    = fChain->GetBranch(name+".ptra");
}

//______________________________________________________________________________
void TS800FpTrack::Clear(Option_t*) 
{
  // -- Clear data members.
  //

  xfp = sqrt(-1.0);
  afp = sqrt(-1.0);
  yfp = sqrt(-1.0);
  bfp = sqrt(-1.0);
  ata = sqrt(-1.0);
  yta = sqrt(-1.0);
  bta = sqrt(-1.0);
  dta = sqrt(-1.0);
  azita   = sqrt(-1.0);
  scatter = sqrt(-1.0);

  energy = sqrt(-1.0);
  ptot   = sqrt(-1.0);
  ppar   = sqrt(-1.0);
  ptra   = sqrt(-1.0);
}

//______________________________________________________________________________
void TS800FpTrack::GetEntry(int i) 
{
  // -- Get an entry for all class branches.
  // This method is redundant. One should be able to do,
  //      s800->b_fpTrack->GetEntry(i);

  b_xfp->GetEntry(i);
  b_afp->GetEntry(i);
  b_yfp->GetEntry(i);
  b_bfp->GetEntry(i);
  b_ata->GetEntry(i);
  b_yta->GetEntry(i);
  b_bta->GetEntry(i);
  b_dta->GetEntry(i);
  b_azita->GetEntry(i);
  b_scatter->GetEntry(i);

  b_energy->GetEntry(i);
  b_ptot->GetEntry(i);
  b_ppar->GetEntry(i);
  b_ptra->GetEntry(i);  
}


//______________________________________________________________________________
Int_t TS800FpTrack::Calculate(Long64_t entry) 
{
  // -- Calculate an entry.
  //

  const Double_t gap = 1073.;    // Distance between Crdc1 and Crdc2 in mm
  //  const Double_t amu=931.5016;   // Atomic Mass Unit 

  Double_t xsin;                 // Disperssive projection.
  Double_t ysin;                 // Non-disperssive projection.
  Double_t input[4];             // Input for COSY inverse map calculations.
  Double_t beta;                 //
  Double_t betaGamma0;           //
  Double_t gamma0;               //
  Double_t gamma;                //
  Double_t energy0;              // Relativistic kinetic energy.
  Double_t ptot0;                // Relativistic total spatial momentum.
  
  Double_t brho  = parent->kBrho;
  Double_t mass0 = mass*kAmu + deltaM;                  // Rest mass. [MeV/c^2]

  betaGamma0 = brho / 3.107 * charge / mass;
  gamma0     = sqrt(betaGamma0*betaGamma0 + 1);
  beta0      = betaGamma0 / gamma0;
  energy0    = mass0 * (gamma0 - 1);                    // [MeV]
  ptot0      = energy0 * sqrt(1 + 2 * mass0 / energy0); // [MeV/c]

  if(entry==0){
  printf("*********************************************************************\n");
  printf("Selected Isotope:  Z=%3i A=%3i deltaM=%12.8lf Brho:  %06f \n",charge,mass,deltaM,brho);
  printf("mass0=%06f  betaGamma0=%06f  gamma0=%06f  beta0=%06f  energy0=%06f  ptot0=%06f \n",
	 mass0,betaGamma0,gamma0,beta0,energy0,ptot0);
  }
  
  Clear();
  // Calculate focal plane coordinates
  afp = atan((parent->crdc2.x - parent->crdc1.x) / gap); // [rad]
  xfp = parent->crdc1.x + zfp * tan(afp);                // [mm]
  bfp = atan((parent->crdc2.y - parent->crdc1.y) / gap); // [rad]
  yfp = parent->crdc1.y + zfp * tan(bfp);                // [mm]
  // Prepare input for COSY inverse map
  // Dispersive direction (x-axis) is reversed for COSI input.
  // NOTE:Input for COSI is in [m] and [rad].
  input[0] = -xfp/1000; // in [m]
  input[1] = -afp;      // in [rad]
  input[2] =  yfp/1000; // in [m]
  input[3] =  bfp;      // in [rad]
  if (order > parent->s800map.maxorder) order = parent->s800map.maxorder;
  // Calculate target coordinates from inverse map
  // NOTE:  The parameters returned by the calculation are in the reference system
  //        used by COSI where x (dispersive) and z are reversed. To recover the 
  //        dispersive angle in the reference system of the FP we must invert the
  //        calculated ata.
  ata = -parent->s800map.Calculate(order, 0, input)*1000; // [mrad]
  yta =  parent->s800map.Calculate(order, 1, input)*1000; // [mm]
  bta =  parent->s800map.Calculate(order, 2, input)*1000; // [mrad]
  dta =  parent->s800map.Calculate(order, 3, input);      // [ÆE/E]
  // Adjustment to outgoing angles.
  ata += anglea*DegToRad()*1000; // [mrad]
  bta += angleb*DegToRad()*1000; // [mrad]
  // Calculate projections.
  xsin = sin(ata/1000.);         // [rad]
  ysin = sin(bta/1000.);         // [rad]
  // Calculate azita (angle in x-y plane).
  if      (xsin > 0 && ysin > 0) azita = atan(ysin/xsin)*RadToDeg();                      // [deg]
  else if (xsin < 0 && ysin > 0) azita = (Pi() - atan(ysin/fabs(xsin)))*RadToDeg();       // [deg]
  else if (xsin < 0 && ysin < 0) azita = (Pi() + atan(fabs(ysin)/fabs(xsin)))*RadToDeg(); // [deg]
  else if (xsin > 0 && ysin < 0) azita = (2.*Pi() - atan(fabs(ysin)/xsin))*RadToDeg();    // [deg]
  else azita = 0.0 ;                                                                      // [deg]
  // Calculate the scattering angle.
  scatter = asin(sqrt(xsin*xsin + ysin*ysin))*1000; // [mrad]

  energy     = (1 + dta) * energy0;                 // [MeV]
  gamma      = 1 + energy / mass0 / kAmu / kAmu; 
  beta       = sqrt(1 - 1 / gamma / gamma);
  deltabeta  = beta - beta0;
  
  ptot = energy * sqrt(1 + 2 * mass0 / energy);     // [MeV/c]
  ppar = ptot * cos(scatter / 1000);                // [MeV/c]
  ptra = ptot * sin(scatter / 1000);                // [MeV/c]

  // Apply corrections
  yta += fYtaShift;

  return 1;
}