// ThinCylMS.cpp

#include "ThinCylMS.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include "trfbase/ETrack.h"
#include <iostream>
#include <cassert>
#include "SurfCylinder.h"
#include "trfbase/SurfacePtr.h"
#include "SurfCylinderParameters.h"

using trf::SurfacePtr;
using trf::TrackVector;
using trf::ETrack;
using trf::SurfCylinder;
using trf::ThinCylMS;

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

namespace {

// Creator.

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

}

//**********************************************************************
// Member functions.
//**********************************************************************

// Constructor.

ThinCylMS::ThinCylMS(double rad_length) : _rad_length(rad_length) { }

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

// Destructor.

ThinCylMS::~ThinCylMS(){}

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

// Return the creator.

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

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

// Write the object data.

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

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

// Interact.

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

  // This can only be used with cylinders... check that we have one..
  
  SurfCylinder cyl(10.0);
  SurfacePtr srf = theTrack.get_surface();
  assert( srf->get_pure_type() == cyl.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(rad_length))*(1+0.038*log(rad_length))

  

  const TrackVector& theVec = theTrack.get_vector();
  double trackMomentum = abs(theVec(IQPT)*cos(atan(theVec(ITLM))));

  double trueLength = _rad_length/abs(cos(theVec(IALF)))*sqrt(1.0 + 
                                  theVec(ITLM)*theVec(ITLM));

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

  // The MS covariance matrix can now be set.

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


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

  double stdThetaSqr = stdTheta*stdTheta;
  double tlamSqr = theVec(ITLM)*theVec(ITLM);
 
  newError(IALF,IALF) += stdThetaSqr*(1 + tlamSqr);
  newError(ITLM,ITLM) += stdThetaSqr*(1 + tlamSqr)*(1 + tlamSqr);
  newError(IQPT,IQPT) += stdThetaSqr*theVec(IQPT)*theVec(IQPT)*tlamSqr;
  newError(ITLM,IQPT) += 
    stdThetaSqr*theVec(ITLM)*theVec(IQPT) * (1.0 + tlamSqr);
  newError(IQPT,ITLM) = newError(ITLM,IQPT);

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

  theTrack.set_error( newError );

}

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