// $Id: AliHLTTPCCATrackParamVector.cxx,v 1.1.1.1 2010/07/26 20:55:38 ikulakov Exp $
// **************************************************************************
// This file is property of and copyright by the ALICE HLT Project          *
// ALICE Experiment at CERN, All rights reserved.                           *
//                                                                          *
// Primary Authors: Sergey Gorbunov <sergey.gorbunov@kip.uni-heidelberg.de> *
//                  Ivan Kisel <kisel@kip.uni-heidelberg.de>                *
//                  for The ALICE HLT Project.                              *
//                                                                          *
// Developed by:   Igor Kulakov <I.Kulakov@gsi.de>                          *
//                 Maksym Zyzak <M.Zyzak@gsi.de>                            *
//                                                                          *
// Permission to use, copy, modify and distribute this software and its     *
// documentation strictly for non-commercial purposes is hereby granted     *
// without fee, provided that the above copyright notice appears in all     *
// copies and that both the copyright notice and this permission notice     *
// appear in the supporting documentation. The authors make no claims       *
// about the suitability of this software for any purpose. It is            *
// provided "as is" without express or implied warranty.                    *
//                                                                          *
//***************************************************************************


#include "AliHLTTPCCATrackParamVector.h"
#include "AliHLTTPCCAMath.h"
#include "AliHLTTPCCAParam.h"
#include "ITSCAHitsV.h"
#include "ITSCATarget.h"

#include <iostream>
#include <iomanip>
#ifndef NVALGRIND
#include <valgrind/memcheck.h>
#endif
#include <assert.h>
#include "debug.h"


void AliHLTTPCCATrackParamVector::InitByTarget( const ITSCATarget& t )
{
  SetX( t.X1() );
  SetY( t.X2() );
  SetZ( t.X0() );
  SetTx( 0 );
  SetTy( 0 );
  SetQP( 0 );
  SetCov( 0, t.Err2X1() );
  SetCov( 1, 0 );
  SetCov( 2, t.Err2X2() );
  SetCov( 3, 0 );
  SetCov( 4, 0 );
  SetCov( 5, 1.f );
  SetCov( 6, 0 );
  SetCov( 7, 0 );
  SetCov( 8, 0 );
  SetCov( 9, 1.f );
  SetCov(10, 0 );
  SetCov(11, 0 );
  SetCov(12, 0 );
  SetCov(13, 0 );
  SetCov(14, t.Err2QMom() );

  SetChi2( 0 );
    
  SetNDF( -5 + t.NDF() );

  SetField( 0, t.B(), t.X0() );
}


void AliHLTTPCCATrackParamVector::InitByHit( const ITSCAHitV& hit, const AliHLTTPCCAParam& param, const sfloat_v& dQP )
{
  SetX( hit.X1() );
  SetY( hit.X2() );
  SetZ( hit.X0() );
  SetTx( 0 );
  SetTy( 0 );
  SetQP( 0 );

  const int ista0 = hit.IStation();
  const L1Station sta0 = param.Station(ista0);
  const L1XYMeasurementInfo &info = sta0.XYInfo;

  SetCov( 0, info.C00 );
  SetCov( 1, info.C10 );
  SetCov( 2, info.C11 );
  SetCov( 3, 0 );
  SetCov( 4, 0 );
  SetCov( 5, 1.f );
  SetCov( 6, 0 );
  SetCov( 7, 0 );
  SetCov( 8, 0 );
  SetCov( 9, 1.f );
  SetCov(10, 0 );
  SetCov(11, 0 );
  SetCov(12, 0 );
  SetCov(13, 0 );
  SetCov(14, dQP*dQP );

  SetChi2( 0 );
    
  SetNDF( -3 );

  L1FieldValue B;
  sta0.fieldSlice.GetFieldValue( hit.X1(), hit.X2(), B );
  SetField( 1, B, sta0.z );

  if ( ista0 > 0 ) {
    const L1Station staC = param.Station(ista0/2); // The field will be used to propaget to the target. Therefore take station in a middle between target and hit.
    const sfloat_v tx1 = hit.X1()/hit.X0(); // suppose target is at 0
    const sfloat_v tx2 = hit.X2()/hit.X0(); // suppose target is at 0
    staC.fieldSlice.GetFieldValue( tx1*staC.z, tx2*staC.z, B );
    SetField( 0, B, staC.z );
  }
}


sfloat_m AliHLTTPCCATrackParamVector::Transport( const ITSCAHitV& hit, const L1FieldRegion& F, const AliHLTTPCCAParam& param, const sfloat_m &mask )
{
  sfloat_m active = mask & hit.IsValid();
  const char ista = hit.IStation();
  active &= TransportToX0WithMaterial( hit.X0(), F, param.Station(ista).materialInfo, QP(), active );
  return active;
}

sfloat_m AliHLTTPCCATrackParamVector::Transport( const ITSCAHitV& hit, const AliHLTTPCCAParam& param, const sfloat_m &mask )
{
  sfloat_m active = mask & hit.IsValid();
//  active &= Rotate(  -fAlpha + hit.Angle(), tR, .999f, active );
  const char ista = hit.IStation();

  L1FieldRegion f;
  {
    const L1Station st = param.Station(hit.IStation());
    L1FieldValue b2;
    const sfloat_v z2 = st.z;
    st.fieldSlice.GetFieldValue( hit.X1(), hit.X2(), b2 );
    if ( fZB[1][0] == 10e10f ) {
      f.Set( b2, z2, fB[0], fZB[0] );
      fB[1] = b2;
      fZB[1] = z2;
    } else {
      f.Set( b2, z2, fB[1], fZB[1], fB[0], fZB[0] );
      fB[0] = fB[1];
      fB[1] = b2;
      fZB[0] = fZB[1];
      fZB[1] = z2;
    }
  }
  active &= TransportToX0WithMaterial( hit.X0(), f, param.Station(ista).materialInfo, QP(), active );
  return active;
}

sfloat_m AliHLTTPCCATrackParamVector::Filter( const ITSCAHitV& hit, const AliHLTTPCCAParam& param, const sfloat_m &mask )
{
  sfloat_m active = mask & hit.IsValid();
  const char ista = hit.IStation();
  active &= Filter( hit.FStrip(), param.Station(ista).frontInfo, active );
  active &= Filter( hit.BStrip(), param.Station(ista).backInfo, active );
  return active;
}

sfloat_m AliHLTTPCCATrackParamVector::Transport( const ITSCAHit& hit, const AliHLTTPCCAParam& param, const sfloat_m &mask )
{
  sfloat_m active = mask;
  const char ista = hit.IStation();

  L1FieldRegion f;
  {
    const L1Station st = param.Station(hit.IStation());
    L1FieldValue b2;
    const sfloat_v z2 = st.z;
    st.fieldSlice.GetFieldValue( hit.X1(), hit.X2(), b2 );
    if ( fZB[1][0] == 10e10f ) {
      f.Set( b2, z2, fB[0], fZB[0] );
      fB[1] = b2;
      fZB[1] = z2;
    } else {
      f.Set( b2, z2, fB[1], fZB[1], fB[0], fZB[0] );
      fB[0] = fB[1];
      fB[1] = b2;
      fZB[0] = fZB[1];
      fZB[1] = z2;
    }
    //     //   // dbg
    // if( fNDF == -1 ) {
    //  cout << " AA ";
    //   f.Set( param.VtxFieldValue(), 0, fB[1], fZB[1], fB[0], fZB[0] ); 
    //   fB[0] = fB[1];
    //   fB[1] = b2;
    //   fZB[0] = fZB[1];
    //   fZB[1] = z2;
    // }

  }

  active &= TransportToX0WithMaterial( hit.X0(), f, param.Station(ista).materialInfo, QP(), active );
  return active;
}

sfloat_m AliHLTTPCCATrackParamVector::Filter( const ITSCAHit& hit, const AliHLTTPCCAParam& param, const sfloat_m &mask )
{
  sfloat_m active = mask;
  const char ista = hit.IStation();
  active &= Filter( hit.FStrip(), param.Station(ista).frontInfo, active );
  active &= Filter( hit.BStrip(), param.Station(ista).backInfo, active );
  return active;
}

sfloat_m AliHLTTPCCATrackParamVector::AddTarget( const ITSCATarget& target, const sfloat_m &mask )
{
  sfloat_m active = mask;
  L1FieldRegion fld;
  if ( fZB[0][0] == 10e10f ) {
    fld.Set( fB[1], fZB[1], target.B(), target.X0() );
    SetField( 0, target.B(), target.X0() );
  }
  else {
    fld.Set( fB[1], fZB[1], fB[0], fZB[0], target.B(), target.X0() );
  }

  sfloat_v eX, eY, J04, J14;
  sfloat_v dz = target.X0() - X0();
  active &= TransportJXY0ToX0( target.X0(), fld, eX, eY, J04, J14, active );
  sfloat_v J[6];
  J[0]= dz; J[1] = 0; J[2]= J04;
  J[3] = 0; J[4]= dz; J[5]= J14;
  active &= FilterVtx( target.X1(), target.X2(), L1XYMeasurementInfo( target.Err2X1(), 0, target.Err2X2() ), eX, eY, J, active );
  fNDF (static_cast<short_m>(active)) += target.NDF();

  return active;
}

#include <iostream>

std::istream &operator>>( std::istream &in, AliHLTTPCCATrackParamVector &t )
{ // TODO
  UNUSED_PARAM1(t);
  // sfloat_v::Memory x, s, p[5], c[15], fChi2;
  // short_v::Memory fNDF;
  // for ( int j = 0; j < ushort_v::Size; ++j ) {
  //   in >> x[j];
  //   in >> s[j];
  //   for ( int i = 0; i < 5; i++ ) in >> p[i][j];
  //   for ( int i = 0; i < 15; i++ ) in >> c[i][j];
  //   in >> fChi2[j];
  //   in >> fNDF[j];
  // }
  // t.fX.load( x );
  // t.fSignCosPhi.load( s );
  // for ( int i = 0; i < 5; i++ ) t.fP[i].load( p[i] );
  // for ( int i = 0; i < 5; i++ ) t.fC[i].load( c[i] );
  // t.Chi2.load( fChi2 );
  // t.NDF.load( fNDF );
  return in;
}

std::ostream &operator<<( std::ostream &out, const AliHLTTPCCATrackParamVector &t )
{ 
    out << " AliHLTTPCCATrackParamVector " << std::endl;
    for ( int i = 0; i < 5; i++ ) out << t.Par(i) << std::endl;
    out << std::endl;
    // for ( int i = 0; i < 15; i++ ) out << t.Cov(i) << std::endl;
    // out << std::endl;

  return out;
}