#include "L1Algo.h"

void L1Algo::Init( fscal geo[] )
{
  int ind=0;
  {
    L1FieldValue B[3];
    fvec z[3];
    for( int i=0; i<3; i++){
      z[i] = geo[ind++]; B[i].x = geo[ind++];   B[i].y = geo[ind++];  B[i].z = geo[ind++];
      std::cout<<"L1Algo Input Magnetic field:"<<z[i][0]<<" "<<B[i].x[0]<<" "<<B[i].y[0]<<" "<<B[i].z[0]<<std::endl;
    }
    vtxFieldRegion.Set(B[0], z[0], B[1], z[1], B[2], z[2] );
    vtxFieldValue = B[0];
  }
  //vStations.clear();
  NStations = (int) geo[ind++];
  NMvdStations = (int) geo[ind++];
  std::cout<<"L1Algo Input "<<NStations<<" Stations:"<<std::endl;
  for( int i=0; i<NStations; i++ ){
    L1Station &st = vStations[i];
    st.z = geo[ind++];
    st.materialInfo.thick = geo[ind++];
    st.Rmin =  geo[ind++];
    st.Rmax =  geo[ind++];
    st.materialInfo.RL    = geo[ind++];
    st.materialInfo.RadThick = st.materialInfo.thick/st.materialInfo.RL;
    st.materialInfo.logRadThick = log(st.materialInfo.RadThick);
    
    double f_phi   = geo[ind++];
    double f_sigma = geo[ind++];
    double b_phi   = geo[ind++];
    double b_sigma = geo[ind++];
    double c_f = cos(f_phi);
    double s_f = sin(f_phi);
    double c_b = cos(b_phi);
    double s_b = sin(b_phi);

    st.frontInfo.cos_phi = c_f;
    st.frontInfo.sin_phi = s_f;
    st.frontInfo.sigma2 = f_sigma*f_sigma;

    st.backInfo.cos_phi = c_b;
    st.backInfo.sin_phi = s_b;
    st.backInfo.sigma2 = b_sigma*b_sigma;

    if( fabs(b_phi-f_phi)<.1 ){
      double th = b_phi-f_phi;
      double det = cos(th);
      det *=det;
      st.XYInfo.C00 = ( s_b*s_b*f_sigma*f_sigma + s_f*s_f*b_sigma*b_sigma )/det;
      st.XYInfo.C10 =-( s_b*c_b*f_sigma*f_sigma + s_f*c_f*b_sigma*b_sigma )/det;
      st.XYInfo.C11 = ( c_b*c_b*f_sigma*f_sigma + c_f*c_f*b_sigma*b_sigma )/det;
    }
    else{
      double det = c_f*s_b - s_f*c_b;
      det *=det;
      st.XYInfo.C00 = ( s_b*s_b*f_sigma*f_sigma + s_f*s_f*b_sigma*b_sigma )/det;
      st.XYInfo.C10 =-( s_b*c_b*f_sigma*f_sigma + s_f*c_f*b_sigma*b_sigma )/det;
      st.XYInfo.C11 = ( c_b*c_b*f_sigma*f_sigma + c_f*c_f*b_sigma*b_sigma )/det;
    }

//    st.xInfo.cos_phi = c_f/(c_f*s_b - c_b*s_f);
//    st.xInfo.sin_phi =-c_b/(c_f*s_b - c_b*s_f);
    st.xInfo.cos_phi = -s_f/(c_f*s_b - c_b*s_f);
    st.xInfo.sin_phi = s_b/(c_f*s_b - c_b*s_f);

    st.xInfo.sigma2 = st.XYInfo.C00;

    st.yInfo.cos_phi = c_b/(c_b*s_f - c_f*s_b);
    st.yInfo.sin_phi =-c_f/(c_b*s_f - c_f*s_b);
    st.yInfo.sigma2 = st.XYInfo.C11;

    int N= (int) geo[ind++];
    for( int iC=0; iC<N; iC++ ) st.fieldSlice.cx[iC] = geo[ind++];
    for( int iC=0; iC<N; iC++ ) st.fieldSlice.cy[iC] = geo[ind++];
    for( int iC=0; iC<N; iC++ ) st.fieldSlice.cz[iC] = geo[ind++];
    std::cout<<"    "<<st.z[0] <<" "<<st.materialInfo.thick[0]<<" "<<st.materialInfo.RL[0]<<", "
        <<N<<" field coeff."<<std::endl;
    std::cout<<"       "<<f_phi<<" "<<f_sigma <<" "<<b_phi<<" "<<b_sigma <<std::endl;
  }

  fTrackingLevel = (int) geo[ind++];
  fMomentumCutOff = geo[ind++];
  fGhostSuppression = (int) geo[ind++];

  {
    fvec By0 = vStations[NStations-1].fieldSlice.cy[0];
    fvec z0  = vStations[NStations-1].z;
    fvec sy = 0., Sy = 0.;
    for( int i=NStations-1; i>=0; i-- ){
      L1Station &st = vStations[i];
      fvec dz = st.z-z0;
      fvec By = vStations[i].fieldSlice.cy[0];
      Sy += dz*sy + dz*dz*By/2.;
      sy += dz*By;
      st.Sy = Sy;
      z0 = st.z;
    }
  }
  std::cout<<"L1Algo initialized"<<std::endl;
}

void L1Algo::GetHitCoor(const L1StsHit& _h, fscal &_x, fscal &_y, fscal &_z, const L1Station &sta)
{
  fscal u = vStsStrips[_h.f];
  fscal v = vStsStripsB[_h.b];
  fscal x,y;
  StripsToCoor(u,v,x,y,sta);
  _x = x;
  _y = y;
  _z = vStsZPos[_h.iz];
};

  /// convert strip positions to coordinates
void L1Algo::StripsToCoor(const fscal &u, const fscal &v, fscal &_x, fscal &_y, const L1Station &sta) // TODO: Actually sta.yInfo.sin_phi is same for all stations, so ...
{
  fvec x,y;
  StripsToCoor(u,v,x,y,sta);
  _x = x[0];
  _y = y[0];
};

void L1Algo::StripsToCoor(const fvec &u, const fvec &v, fvec &x, fvec &y, const L1Station &sta) // TODO: Actually sta.yInfo.sin_phi is same for all stations, so ...
{
  // only for same-z
//   x = u;
//   y = (sta.yInfo.cos_phi*u + sta.yInfo.sin_phi*v);
  x = sta.xInfo.sin_phi*u + sta.xInfo.cos_phi*v;
  y = sta.yInfo.cos_phi*u + sta.yInfo.sin_phi*v;
};

  /// full the hit point by hit information.
L1HitPoint L1Algo::CreateHitPoint(const L1StsHit &hit, char ista)
{
  L1Station &sta = vStations[ista];
  L1Strip &u = vStsStrips[hit.f];
  L1Strip &v = vStsStripsB[hit.b];
  fscal x, y;
  StripsToCoor( u, v, x, y, sta);
  fscal z = vStsZPos[hit.iz];

  return  L1HitPoint(x,y,z,v,u,hit.n);
};