// ThinXYPlaneMS.cpp

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

using namespace trf;

// Assign track parameter indices
enum {
  IV = SurfXYPlane::IV,
  IZ = SurfXYPlane::IZ,
  IDVDU = SurfXYPlane::IDVDU,
  IDZDU = SurfXYPlane::IDZDU,
  IQP = SurfXYPlane::IQP
};

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

namespace {

// Creator.

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

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

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

ThinXYPlaneMS::~ThinXYPlaneMS(){}

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

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


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

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

  SurfXYPlane XYpl( 10.0, 0.0 );
  SurfacePtr srf = theTrack.get_surface();
  assert( srf->get_pure_type() == XYpl.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(IDVDU);
  double g = theVec(IDZDU);

  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 uhat = sqrt(1-sin(theta)*sin(theta));
  double vhat = sin(theta)*cos(phi);
  double zhat = sin(theta)*sin(phi);

  double Qvu,Qzu,Qvz;

// The MS covariance matrix can now be set.

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


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

  double norm = sqrt(vhat*vhat + zhat*zhat);
  
  if( norm == 0. || uhat == 0. ) return;

  Qvu = (zhat/(norm*uhat))*(zhat/(norm*uhat));
  Qvu += pow((vhat/norm)*(1 + (norm*norm)/(uhat*uhat)),2.0);
  Qvu *= stdTheta;
  Qvu *= stdTheta;

  Qzu = (vhat/(norm*uhat))*(vhat/(norm*uhat));
  Qzu += pow((zhat/norm)*(1 + (norm*norm)/(uhat*uhat)),2.0);
  Qzu *= stdTheta;
  Qzu *= stdTheta;

  Qvz = - vhat*zhat/(uhat*uhat);
  Qvz += vhat*zhat/(norm*norm)*pow((1 + (norm*norm)/(uhat*uhat)),2.0);
  Qvz *= stdTheta;
  Qvz *= stdTheta;

  newError(IDVDU,IDVDU) += Qvu;
  newError(IDZDU,IDZDU) += Qzu;
  newError(IDVDU,IDZDU) += Qvz;
  
 
  theTrack.set_error( newError );

}

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

// Return the creator.

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

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

// Write the object data.

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

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