// ThinZPlaneMS.cpp

#include "ThinZPlaneMS.h"
#include <cassert>
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include "trfutil/TRFMath.h"
#include "trfutil/trfstream.h"
#include "trfbase/ETrack.h"
#include "trfbase/SurfacePtr.h"
#include "SurfZPlane.h"

using namespace trf;

// Assign track parameter indices
enum{
  IX = SurfZPlane::IX,
  IY = SurfZPlane::IY,
  IDXDZ = SurfZPlane::IDXDZ,
  IDYDZ = SurfZPlane::IDYDZ,
  IQP = SurfZPlane::IQP
};

//**********************************************************************
// Free functions.
//**********************************************************************

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "ThinZPlaneMS" );
  double rad_length = data.get_double("rad_length");
  return ObjPtr( new ThinZPlaneMS(rad_length) );
}

}
//**********************************************************************

ThinZPlaneMS::ThinZPlaneMS( double radLength ):_radLength(radLength){}

//**********************************************************************

ThinZPlaneMS::~ThinZPlaneMS(){}

//**********************************************************************

double ThinZPlaneMS::get_rad_length() const {return _radLength;}

//**********************************************************************

Interactor* ThinZPlaneMS::new_copy() const { return new ThinZPlaneMS(*this); }

//**********************************************************************

void ThinZPlaneMS::interact( ETrack& theTrack ) const
{

  // This can only be used with zplanes... check that we have one..

  SurfZPlane zpl(10.0);
  SurfacePtr srf = theTrack.get_surface();
  assert( srf->get_pure_type() == zpl.get_pure_type() );

  const TrackError& cleanError = theTrack.get_error();
  TrackError newError(cleanError);

  // set the rms scattering appropriate to this momentum

  // Theta = (0.0136 GeV)*(z/p)*(sqrt(radLength))*(1+0.038*log(radLength))



  const TrackVector& theVec = theTrack.get_vector();
  float trackMomentum = theVec(IQP);

  double f = theVec(IDXDZ);
  double g = theVec(IDYDZ);

  double pi = acos(-1.0);
  double theta = atan(sqrt(f*f + g*g));
  double phi = f!=0 ? atan(sqrt((g*g)/(f*f))) : PI2;
  if ( f==0 && g<0 ) phi=3*PI2;
  if((f<0)&&(g>0))
    phi = pi - phi;
  if((f<0)&&(g<0))
    phi = pi + phi;
  if((f>0)&&(g<0))
    phi = 2*pi - phi;

  double cost = cos(theta);

  if( cost <= 0. ) return;

  double trueLength = _radLength/cost;

  double stdTheta = (0.0136)*trackMomentum*sqrt(trueLength)*
                         (1 + 0.038*log(trueLength));  

  double zhat = sqrt(1-sin(theta)*sin(theta));
  double xhat = sin(theta)*cos(phi);
  double yhat = sin(theta)*sin(phi);

  double Qxz,Qyz,Qxy;

// The MS covariance matrix can now be set.

  // **************** code for matrix ***********************//


  // Insert values for upper triangle... use symmetry to set lower.

  double norm = sqrt(xhat*xhat + yhat*yhat);

  if( norm == 0. || zhat == 0. )
    return;

  Qxz = (yhat/(norm*zhat))*(yhat/(norm*zhat));
  Qxz += pow((xhat/norm)*(1 + (norm*norm)/(zhat*zhat)),2.0);
  Qxz *= stdTheta;
  Qxz *= stdTheta;

  Qyz = (xhat/(norm*zhat))*(xhat/(norm*zhat));
  Qyz += pow((xhat/norm)*(1 + (norm*norm)/(zhat*zhat)),2.0);
  Qyz *= stdTheta;
  Qyz *= stdTheta;

  Qxy = -xhat*yhat/(zhat*zhat);
  Qxy += xhat*yhat/(norm*norm)*pow((1 + (norm*norm)/(zhat*zhat)),2.0);
  Qxy *= stdTheta;
  Qxy *= stdTheta;

  newError(IDXDZ,IDXDZ) += Qxz;
  newError(IDYDZ,IDYDZ) += Qyz;
  newError(IDXDZ,IDYDZ) += Qxy;
  
//**********************************************************************

  theTrack.set_error( newError );

}

//**********************************************************************

// Return the creator.

ObjCreator ThinZPlaneMS::get_creator() {
  return create;
}

//**********************************************************************

// Write the object data.

ObjData ThinZPlaneMS::write_data() const {
  ObjData data( get_type_name() );
  data.add_double( "rad_length", get_rad_length() );
  return data;
}

//**********************************************************************