//////////////////////////////////////////////////////////////////////////
//                                                                      //
// PndMassFitter			  					//
//                                                                      //
// Author: D. Mishra, GSI,  2008		         		//
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#include <iostream>
#include "RhoFitter/PndMassFitter.h"
#include "RhoBase/TCandListIterator.h"

#include "RhoFitter/VtxFitterParticle.h"
#include "RhoFitter/VtxFitter_Init.h"
#include "RhoFitter/VtxFitterError.h"
#include "RhoBase/TFactory.h"

using namespace std;

ClassImp(PndMassFitter)

TBuffer &operator>>(TBuffer &buf, PndMassFitter *&obj)
{
   obj = (PndMassFitter *) buf.ReadObject(PndMassFitter::Class());
   return buf;
}

PndMassFitter::PndMassFitter( const TCandidate& b, double mass ) : VAbsFitter( b ) 
{
  m_fitIncludingVertex=1;
  m_atDecayPoint=KF_AT_DECAY_POINT;
  m_isFixMass=1;
  m_vertex_b=TVector3(0.,0.,0.);
  m_necessaryTrackNum = 2;
  m_invariantMass=mass;
  m_errorFlag = KF_NO_ERROR;
}
    
    
PndMassFitter::~PndMassFitter()
{
  //if(fHeadOfTree) delete fHeadOfTree;
}

void PndMassFitter::Fit() 
{
  TCandidate theHead=*fHeadOfTree;
  TCandListIterator iter=theHead.DaughterIterator();
  cout <<"Fit: head of Tree:"<<endl;
  cout<<(*fHeadOfTree)<<endl;
  
  fDaughters.Cleanup();
  
  TCandidate *tc;
  int num=0;
  while (tc=iter.Next())
  {
    TMatrixD cov=tc->Cov7();
    cout <<"PndMassFitter: Daughter "<<num++<<endl;
    cout <<(*tc)<<endl;
    //cov.Print();
    fDaughters.Add(*tc);
  }
  
  DoVertexFitMassConstraintIncludeVertex();
  //fHeadOfTree->SetP4(0.4,TVector3(0.2,0.3,0.4));
  
}
/**
  This is same as "fit()" function in "kfitterbase".
 **/
unsigned PndMassFitter::DoVertexFitMassConstraintWOIncludeVertex()
{
   if(m_errorFlag != KF_NO_ERROR)return m_errorFlag;
  //cout<<"======SetInputMatrix part Start========== "<<m_trackNum<<endl;
  m_trackNum = fDaughters.GetLength();
  if(m_trackNum < m_necessaryTrackNum){
    m_errorFlag = KF_TRACK_SIZE;
    return m_errorFlag;
  }
  cout<<"======SetInputMatrix part Start========== "<<m_trackNum<<endl;
  /*
  if(m_trackNum > KF_MAX_TRACK_NUMBER){
    m_errorFlag = KF_INPUT_TRACK_SIZE;
    return m_errorFlag;
  }*/
  
  SetInputMatrix(); //Set input matrix
  CalDgf(); //  Calculate Dgf 
  
  
  /** Fit function _fit() Starts **/
  cout<<"++++++ Fit function _fit() Starts +++++++ "<<endl;
  Double_t chiSq(0.);
  //  Double_t errInverse = 0; // if you need the determinant value
  Double_t *errInverse = 0;
  Double_t tmp_chiSq(KF_INIT_CHI2);
  TMatrixD tmp_al_1(m_al_1);
  TMatrixD tmp_V_al_1(m_V_al_1);
  TMatrixD    m_al_a = TMatrixD(KF_NUM7*m_trackNum,1);
  m_al_a = m_al_0;
  TMatrixD tmp_al_a(m_al_a);
  for(Int_t j=0;j<KF_MAX_ITERATION_NUMBER;++j)
    {
      MakeCoreMatrix(); // MakeCoreMatrix() STARTS 
      
      cout<<"++++++ Fit function _fit() Continues +++++++ "<<endl;

      m_V_D = (m_D*m_V_al_0*(m_D_T.Transpose(m_D))).Invert(errInverse);
      /**
	   if(errInverse != 0){
	   m_errorFlag = KF_INVERSE;
	   return m_errorFlag;
	   }
      **/
      
      m_lam    = m_V_D*(m_D*(m_al_0-m_al_1)+m_d);
      TMatrixD m_lam_tmp = m_lam;
      chiSq    = ((m_lam_tmp.T())*(m_D*(m_al_0-m_al_1)+m_d))(0,0);
      
      
      m_al_1   = m_al_0 - m_V_al_0*(m_D_T.Transpose(m_D))*m_lam;
     
      m_V_al_1 = m_V_al_0 - m_V_al_0*(m_D_T.Transpose(m_D))*m_V_D*m_D*m_V_al_0;
      
      if(tmp_chiSq > chiSq){
	tmp_chiSq  = chiSq;
	tmp_al_a   = tmp_al_1;
	tmp_al_1   = m_al_1;
	tmp_V_al_1 = m_V_al_1;
	if(j == KF_MAX_ITERATION_NUMBER-1){
	  m_al_a = tmp_al_1;
	  m_overIterationFlag = KF_OVER_ITERATION;
	}else continue;
      }else if(j != 0){
	chiSq    = tmp_chiSq;
	m_al_1   = tmp_al_1;
	m_al_a   = tmp_al_a;
	m_V_al_1 = tmp_V_al_1;
	break;
      }else if(j == 0){
	m_errorFlag = KF_INIT_CHISQ;
	return m_errorFlag;
      }
      
    } //Iteration Loop ends


  //  cout<<"Final Momenta are "<<m_al_1[0][0]<<" "<<m_al_1[1][0]<<" "<<m_al_1[2][0]<<" "<<m_al_1[7][0]<<" "<<m_al_1[8][0]<<" "<<m_al_1[9][0]<<endl;
  //  cout<<"Final Positions are "<<m_al_1[4][0]<<" "<<m_al_1[5][0]<<" "<<m_al_1[6][0]<<" "<<m_al_1[11][0]<<" "<<m_al_1[12][0]<<" "<<m_al_1[13][0]<<endl;
  cout<<"Chi2 is **********: "<<chiSq<<endl;
  

  //SetOutputMatrix(); //Set the output matrix  
   
   SetOutputToTCandidate();
}


unsigned PndMassFitter::DoVertexFitMassConstraintIncludeVertex()
{
  cout<<"Include Vertex in the Fitting ======== "<<endl;
  if(m_errorFlag != KF_NO_ERROR)return m_errorFlag;
  m_trackNum = fDaughters.GetLength();
  if(m_trackNum < m_necessaryTrackNum){
    m_errorFlag = KF_TRACK_SIZE;
    return m_errorFlag;
  }
 
  SetInputMatrix(); //Set input matrix
  
  CalDgf(); //Calculate Dgf
  
  /** Fit function _fit() Starts **/
  Double_t chiSq(0.);
  //  Double_t errInverse = 0; // if you need the determinant value
  Double_t *errInverse = 0;
  Double_t tmp_chiSq(KF_INIT_CHI2);
  TMatrixD tmp_al_1(m_al_1);
  TMatrixD tmp_V_al_1(m_V_al_1);
  TMatrixD    m_al_a = TMatrixD(KF_NUM7*m_trackNum+3,1);
  m_al_a = m_al_0;
  TMatrixD tmp_al_a(m_al_a);
  for(Int_t j=0;j<KF_MAX_ITERATION_NUMBER;++j)
    {
      MakeCoreMatrix(); //Make Core matrix
      
      cout<<"++++++ Fit function _fit() Continues +++++++ "<<endl;
      //      TMatrixD mD_tmp = m_D;
      m_V_D = (m_D*m_V_al_0*(m_D_T.Transpose(m_D))).Invert(errInverse);
      /*** will 
	   if(errInverse != 0){
	   m_errorFlag = KF_INVERSE;
	   return m_errorFlag;
	   }
      **/
      m_lam    = m_V_D*(m_D*(m_al_0-m_al_1)+m_d);
      TMatrixD m_lam_tmp = m_lam;
      chiSq    = ((m_lam_tmp.T())*(m_D*(m_al_0-m_al_1)+m_d))(0,0);
      
    
      m_al_1   = m_al_0 - m_V_al_0*(m_D_T.Transpose(m_D))*m_lam;
     
      m_V_al_1 = m_V_al_0 - m_V_al_0*(m_D_T.Transpose(m_D))*m_V_D*m_D*m_V_al_0;
      
      if(tmp_chiSq > chiSq){
	tmp_chiSq  = chiSq;
	tmp_al_a   = tmp_al_1;
	tmp_al_1   = m_al_1;
	tmp_V_al_1 = m_V_al_1;
	if(j == KF_MAX_ITERATION_NUMBER-1){
	  m_al_a = tmp_al_1;
	  m_overIterationFlag = KF_OVER_ITERATION;
	}else continue;
      }else if(j != 0){
	chiSq    = tmp_chiSq;
	m_al_1   = tmp_al_1;
	m_al_a   = tmp_al_a;
	m_V_al_1 = tmp_V_al_1;
	break;
      }else if(j == 0){
	m_errorFlag = KF_INIT_CHISQ;
	return m_errorFlag;
      }
      
    } //Iteration Loop ends
  
  //  cout<<"Final Momenta are "<<m_al_1[0][0]<<" "<<m_al_1[1][0]<<" "<<m_al_1[2][0]<<" "<<m_al_1[7][0]<<" "<<m_al_1[8][0]<<" "<<m_al_1[9][0]<<endl;
  //  cout<<"Final Positions are "<<m_al_1[4][0]<<" "<<m_al_1[5][0]<<" "<<m_al_1[6][0]<<" "<<m_al_1[11][0]<<" "<<m_al_1[12][0]<<" "<<m_al_1[13][0]<<endl;
  cout<<"Chi2 is **********: "<<chiSq<<endl;
 
   //SetOutputMatrix(); // Set the output matrix
   
   SetOutputToTCandidate();
  
}

unsigned PndMassFitter::SetInputMatrix()
{
  cout<<"Track NUMBER is ***** "<<m_trackNum<<endl;
  if(m_trackNum > KF_MAX_TRACK_NUMBER){
    m_errorFlag = KF_INPUT_TRACK_SIZE;
    return m_errorFlag;
  }
  if(m_fitIncludingVertex==0){
    unsigned index(0);
    TMatrixDSym tmp_V_al_0(KF_NUM7*m_trackNum);
    
    m_al_0.ResizeTo(KF_NUM7*m_trackNum,1);
    m_al_1.ResizeTo(KF_NUM7*m_trackNum,1);
    m_property.ResizeTo(m_trackNum,3);
    
    m_D.ResizeTo(1,KF_NUM7*m_trackNum);
    m_d.ResizeTo(1,1);
    m_V_D.ResizeTo(1,1);
    m_lam.ResizeTo(1,1);
    
    TMatrixDSym tmp_ErrCov(KF_NUM6);

    m_D_T.ResizeTo(KF_NUM7*m_trackNum,1);    

    Int_t nTra = fDaughters.GetLength();
    
    it =new VtxFitterParticle;
    
    for(Int_t i=0;i<nTra;i++)
    {
        TCandidate tc=fDaughters[i];
		
        m_al_0[index*KF_NUM7+0][0] = tc.P4().Px();
        m_al_0[index*KF_NUM7+1][0] = tc.P4().Py();
        m_al_0[index*KF_NUM7+2][0] = tc.P4().Pz();
        m_al_0[index*KF_NUM7+3][0] = tc.P4().E();
        m_al_0[index*KF_NUM7+4][0] = tc.Pos().X();
        m_al_0[index*KF_NUM7+5][0] = tc.Pos().Y();
        m_al_0[index*KF_NUM7+6][0] = tc.Pos().Z();
		
	tmp_V_al_0.SetSub(index*KF_NUM7, it->GetCovMat1( tc.Cov7() ) );

	//Charge, Mass and a
	m_property[index][0] =  tc.GetCharge()/3;
	m_property[index][1] =  tc.P4().M(); //get the mass:Dummy value set by hand:Dipak
	m_property[index][2] = -KF_PHOTON_VELOCITY*m_magField*tc.GetCharge()/3;
	++index;
	
    }
    delete it;
//       //Error between the tracks : will be done later :Dipak
//       if(m_correlationFlag == 1){
// 	      m_errorFlag = m_setCorrelation();
// 	      if(m_errorFlag != KF_NO_ERROR)return m_errorFlag;
//       }
    m_V_al_0.ResizeTo(tmp_V_al_0);
    
    m_V_al_0 = tmp_V_al_0;
    m_al_1 = m_al_0;
    m_V_al_1.ResizeTo(KF_NUM7*m_trackNum,KF_NUM7*m_trackNum);
    m_D      = m_V_al_1.GetSub(0,0,0,KF_NUM7*m_trackNum-1); 
  }else{//m_fitIncludingVertex ==1

    unsigned index(0);
    TMatrixDSym tmp_V_al_0(KF_NUM7*m_trackNum+3);
    m_al_0.ResizeTo(KF_NUM7*m_trackNum+3,1);
    m_al_1.ResizeTo(KF_NUM7*m_trackNum+3,1);
    m_property.ResizeTo(m_trackNum,3);
    
    m_D.ResizeTo(1,KF_NUM7*m_trackNum+3);
    m_d.ResizeTo(1,1);
    m_V_D.ResizeTo(1,1);
    m_lam.ResizeTo(1,1);

    m_D_T.ResizeTo(KF_NUM7*m_trackNum+3,1);


    Int_t nTra = fDaughters.GetLength();
    
    it =new VtxFitterParticle;
    
    for(Int_t i=0;i<nTra;i++)
    {
        TCandidate tc=fDaughters[i];
		
        m_al_0[index*KF_NUM7+0][0] = tc.P4().Px();
        m_al_0[index*KF_NUM7+1][0] = tc.P4().Py();
        m_al_0[index*KF_NUM7+2][0] = tc.P4().Pz();
        m_al_0[index*KF_NUM7+3][0] = tc.P4().E();
        m_al_0[index*KF_NUM7+4][0] = tc.Pos().X();
        m_al_0[index*KF_NUM7+5][0] = tc.Pos().Y();
        m_al_0[index*KF_NUM7+6][0] = tc.Pos().Z();
		
	tmp_V_al_0.SetSub(index*KF_NUM7, it->GetCovMat1( tc.Cov7() ) );

	//Charge, Mass and a
	m_property[index][0] =  tc.GetCharge()/3;
	m_property[index][1] =  tc.P4().M(); //get the mass:Dummy value set by hand:Dipak
	m_property[index][2] = -KF_PHOTON_VELOCITY*m_magField*tc.GetCharge()/3;
	++index;
	
    }
    delete it;
/*

    for(Int_t i=0;i<fArr1->GetEntriesFast();i++)
      {
	it = (VtxFitterParticle*)fArr1->At(i);
	m_al_0[index*KF_NUM7+0][0] = it->GetMomentum().Px();
	m_al_0[index*KF_NUM7+1][0] = it->GetMomentum().Py();
	m_al_0[index*KF_NUM7+2][0] = it->GetMomentum().Pz();
	m_al_0[index*KF_NUM7+3][0] = it->GetMomentum().E();
	m_al_0[index*KF_NUM7+4][0] = it->GetPosition().X();
	m_al_0[index*KF_NUM7+5][0] = it->GetPosition().Y();
	m_al_0[index*KF_NUM7+6][0] = it->GetPosition().Z();
	
	tmp_V_al_0.SetSub(index*KF_NUM7, it->GetErrorCovMatrix());

      //Charge, Mass and a
      m_property[index][0] =  it->GetCharge();
      m_property[index][1] =  it->GetMomentum().M(); //get the mass : this a dummy value set by hand
      m_property[index][2] = -KF_PHOTON_VELOCITY*m_magField*it->GetCharge();
      ++index;
    }
*/
    m_al_0[KF_NUM7*m_trackNum+0][0] = m_vertex_b.X();
    m_al_0[KF_NUM7*m_trackNum+1][0] = m_vertex_b.Y();
    m_al_0[KF_NUM7*m_trackNum+2][0] = m_vertex_b.Z();
    tmp_V_al_0.SetSub(KF_NUM7*m_trackNum,it->GetErrVertexOut());
   cout<<m_property[0][0]<<" m_property "<<m_property[0][2]<<" "<<m_property[1][2]<<endl;

   /***
    //   Dipak
    //error between track and track
    //setcorrelation function is in kmassfitter.cc
    //m_V_al_0 = tmp_V_al_0;
    if(m_correlationFlag == 1){
    m_errorFlag = m_setCorrelation();
    if(m_errorFlag != KF_NO_ERROR)return m_errorFlag;
    }
   *******/
   m_V_al_0.ResizeTo(tmp_V_al_0);
   m_V_al_1.ResizeTo(KF_NUM7*m_trackNum+3,KF_NUM7*m_trackNum+3);
   
   m_V_al_0 = tmp_V_al_0;

   m_al_1 = m_al_0;
   m_D      = m_V_al_1.GetSub(0,0,0,KF_NUM7*m_trackNum+3-1); 
  }
}

unsigned PndMassFitter::MakeCoreMatrix()
{
  if(m_fitIncludingVertex == 0)
    {
      TMatrixD al_1_prime(m_al_1);
      cout<<"MAKE COREMATRIX *** "<<al_1_prime.GetNrows()<<" "<<al_1_prime.GetNcols()<<" "<<m_al_1.GetNrows()<<" "<<m_al_1.GetNcols()<<" "<<m_trackNum<<" "<<m_invariantMass<<endl;
      TMatrixD Sum_al_1(4,1);
      Double_t   *energy = new Double_t[m_trackNum];
      Double_t    a;
     
      for(unsigned i=0;i<m_trackNum;++i){
	a = m_property[i][2];
	if(m_atDecayPoint == KF_NOT_DECAY_POINT)a = 0.;
	al_1_prime[i*KF_NUM7+0][0] -= a*(m_vertex_b.Y()-al_1_prime[i*KF_NUM7+5][0]);
	al_1_prime[i*KF_NUM7+1][0] += a*(m_vertex_b.X()-al_1_prime[i*KF_NUM7+4][0]);
	energy[i] = TMath::Sqrt(al_1_prime[i*KF_NUM7+0][0]*al_1_prime[i*KF_NUM7+0][0]+
				al_1_prime[i*KF_NUM7+1][0]*al_1_prime[i*KF_NUM7+1][0]+
				al_1_prime[i*KF_NUM7+2][0]*al_1_prime[i*KF_NUM7+2][0]+
				m_property[i][1]*m_property[i][1]);
      }
      
      for(unsigned i=0;i<m_trackNum;++i){
	if(m_isFixMass == KF_FIX_MASS){
	  Sum_al_1[3][0] += energy[i];
	}else{
	  Sum_al_1[3][0] += al_1_prime[i*KF_NUM7+3][0];
	}
	for(unsigned jj=0;jj<3;++jj){
	  Sum_al_1[jj][0] += al_1_prime[i*KF_NUM7+jj][0];
	}
      }
      m_d[0][0] = Sum_al_1[3][0]*Sum_al_1[3][0]-Sum_al_1[0][0]*Sum_al_1[0][0]
	-         Sum_al_1[1][0]*Sum_al_1[1][0]-Sum_al_1[2][0]*Sum_al_1[2][0]
	-         m_invariantMass*m_invariantMass; //Get the invariant mass properly
      
      for(unsigned i=0;i<m_trackNum;++i){
	if(energy[i] == 0.){
	  m_errorFlag = KF_DIV_ZERO;
	  break;
	}
	    a = m_property[i][2];
	    if(m_atDecayPoint == KF_NOT_DECAY_POINT)a = 0.;
	      
	    if(m_isFixMass == KF_FIX_MASS){
	      Double_t invE = 1./energy[i];
	      m_D[0][i*KF_NUM7+0] = 2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+0][0]*
					invE-Sum_al_1[0][0]);
	      m_D[0][i*KF_NUM7+1] = 2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+1][0]*
					invE-Sum_al_1[1][0]);
	      m_D[0][i*KF_NUM7+2] = 2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+2][0]*
					invE-Sum_al_1[2][0]);
	      m_D[0][i*KF_NUM7+3] = 0.;
	      m_D[0][i*KF_NUM7+4] =-2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+1][0]*
					invE-Sum_al_1[1][0])*a;
	      m_D[0][i*KF_NUM7+5] = 2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+0][0]*
					invE-Sum_al_1[0][0])*a;
	      m_D[0][i*KF_NUM7+6] = 0.;
	    }else{
	      m_D[0][i*KF_NUM7+0] = -2.*Sum_al_1[0][0];
	      m_D[0][i*KF_NUM7+1] = -2.*Sum_al_1[1][0];
	      m_D[0][i*KF_NUM7+2] = -2.*Sum_al_1[2][0];
	      m_D[0][i*KF_NUM7+3] =  2.*Sum_al_1[3][0];
	      m_D[0][i*KF_NUM7+4] =  2.*Sum_al_1[1][0]*a;
	      m_D[0][i*KF_NUM7+5] = -2.*Sum_al_1[0][0]*a;
	      m_D[0][i*KF_NUM7+6] =  0.;
	    }
	  }
	  delete [] energy;
    }else{
    
    TMatrixD al_1_prime(m_al_1);
    TMatrixD Sum_al_1(7,1);
    Double_t   *energy = new Double_t[m_trackNum];
    Double_t    a;

    for(unsigned i=0;i<m_trackNum;++i){
      a = m_property[i][2];
      
      al_1_prime[i*KF_NUM7+0][0] -= a*(al_1_prime[KF_NUM7*m_trackNum+1][0]-
				       al_1_prime[i*KF_NUM7+5][0]);
      al_1_prime[i*KF_NUM7+1][0] += a*(al_1_prime[KF_NUM7*m_trackNum+0][0]-
				       al_1_prime[i*KF_NUM7+4][0]);
      energy[i] = TMath::Sqrt(al_1_prime[i*KF_NUM7+0][0]*al_1_prime[i*KF_NUM7+0][0]+
			      al_1_prime[i*KF_NUM7+1][0]*al_1_prime[i*KF_NUM7+1][0]+
			      al_1_prime[i*KF_NUM7+2][0]*al_1_prime[i*KF_NUM7+2][0]+
			      m_property[i][1]*m_property[i][1]);
      Sum_al_1[6][0] =+ a;
      
    }
    for(unsigned i=0;i<m_trackNum;++i){
      if(energy[i] == 0.){
	m_errorFlag = KF_DIV_ZERO;
	break;
      }
      
      if(m_isFixMass == KF_FIX_MASS){
	Double_t invE = 1./energy[i];
	Sum_al_1[3][0] += energy[i];
	Sum_al_1[4][0] += al_1_prime[i*KF_NUM7+1][0]*m_property[i][2]*invE;
	Sum_al_1[5][0] += al_1_prime[i*KF_NUM7+0][0]*m_property[i][2]*invE;
      }else{
	Sum_al_1[3][0] += al_1_prime[i*KF_NUM7+3][0];	
      }
      for(unsigned jj=0;jj<3;++jj){
	Sum_al_1[jj][0] += al_1_prime[i*KF_NUM7+jj][0];
      }
      
    }
    m_d[0][0] = Sum_al_1[3][0]*Sum_al_1[3][0]-Sum_al_1[0][0]*Sum_al_1[0][0]
      -         Sum_al_1[1][0]*Sum_al_1[1][0]-Sum_al_1[2][0]*Sum_al_1[2][0]
      -         m_invariantMass*m_invariantMass;
    
    for(unsigned i=0;i<m_trackNum;++i){
      if(energy[i] == 0.){
	m_errorFlag = KF_DIV_ZERO;
	break;
      }
      a = m_property[i][2];
      if(m_isFixMass == KF_FIX_MASS){
	Double_t invE = 1./energy[i];
	m_D[0][i*KF_NUM7+0] = 2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+0][0]*
				  invE-Sum_al_1[0][0]);
	m_D[0][i*KF_NUM7+1] = 2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+1][0]*
				  invE-Sum_al_1[1][0]);
	m_D[0][i*KF_NUM7+2] = 2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+2][0]*
				  invE-Sum_al_1[2][0]);
	m_D[0][i*KF_NUM7+3] = 0.;
	m_D[0][i*KF_NUM7+4] =-2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+1][0]*
				  invE-Sum_al_1[1][0])*a;
	m_D[0][i*KF_NUM7+5] = 2.*(Sum_al_1[3][0]*al_1_prime[i*KF_NUM7+0][0]*
				  invE-Sum_al_1[0][0])*a;
	m_D[0][i*KF_NUM7+6] = 0.;
      }else{
	m_D[0][i*KF_NUM7+0] = -2.*Sum_al_1[0][0];
	m_D[0][i*KF_NUM7+1] = -2.*Sum_al_1[1][0];
	m_D[0][i*KF_NUM7+2] = -2.*Sum_al_1[2][0];
	m_D[0][i*KF_NUM7+3] =  2.*Sum_al_1[3][0];
	m_D[0][i*KF_NUM7+4] =  2.*Sum_al_1[1][0]*a;
	m_D[0][i*KF_NUM7+5] = -2.*Sum_al_1[0][0]*a;
	m_D[0][i*KF_NUM7+6] =  0.;
      }
    }
    m_D[0][KF_NUM7*m_trackNum+0] = 2.*(Sum_al_1[3][0]*Sum_al_1[4][0]-
				       Sum_al_1[1][0]*Sum_al_1[6][0]);
    m_D[0][KF_NUM7*m_trackNum+1] =-2.*(Sum_al_1[3][0]*Sum_al_1[5][0]-
				       Sum_al_1[0][0]*Sum_al_1[6][0]);
    m_D[0][KF_NUM7*m_trackNum+2] = 0.;
    delete [] energy;
    
  }

  return m_errorFlag;
}

unsigned PndMassFitter::SetOutputMatrix()
{
/*
  TVector3 h3v;
  unsigned index1(0);
  for(Int_t i=0;i<fArr1->GetEntriesFast();i++)
    {
      it = (VtxFitterParticle*)fArr1->At(i);
      h3v.SetX(m_al_1[index1*KF_NUM7+0][0]);
      h3v.SetY(m_al_1[index1*KF_NUM7+1][0]);
      h3v.SetZ(m_al_1[index1*KF_NUM7+2][0]);
      if(m_isFixMass == KF_FIX_MASS){
	//Filling Momentum
	it->SetMomentum(TLorentzVector(h3v, sqrt(h3v.Mag2()+0.005*0.005)));
      }else{
	it->SetMomentum(TLorentzVector(h3v, m_al_1[index1*KF_NUM7+3][0]));
      }
      //Filling Position
      it->SetPosition(TVector3(m_al_1[index1*KF_NUM7+4][0],
			       m_al_1[index1*KF_NUM7+5][0],
			       m_al_1[index1*KF_NUM7+6][0]));
      
      //Errors on Momentum will be filling later : Dipak
      if(m_errorFlag != KF_NO_ERROR)break;
      ++index1;
    }
  if(m_fitIncludingVertex == 0){
    it->SetVertex(m_vertex_b);
  }else{
    it->SetVertex(TVector3(m_al_1[KF_NUM7*m_trackNum+0][0],m_al_1[KF_NUM7*m_trackNum+1][0],m_al_1[KF_NUM7*m_trackNum+2][0])); //SetVertex

    //Error of the Vertex and Error betweem vertex and tracks will be implemented later : Dipak
  }
  */
  return m_errorFlag;
}

//Fill output to TCandidate
unsigned PndMassFitter::SetOutputToTCandidate()
{
	TVector3 h3v;
	cout<<"# of Tracks are in TCandidate ************ "<<fDaughters.GetLength()<<endl;
	unsigned index1(0);
	TMatrixD trCov(7,7);
	
	TLorentzVector sum(0,0,0,0);
	
	//TClonesArray &cands = *fCandidates;
	for(Int_t i=0;i<fDaughters.GetLength();i++)
	{
                TLorentzVector lv;
		double px=m_al_1[index1*KF_NUM7+0][0];
		double py=m_al_1[index1*KF_NUM7+1][0];
		double pz=m_al_1[index1*KF_NUM7+2][0];
                double E =sqrt(fDaughters[i].Mass()*fDaughters[i].Mass()+px*px+py*py+pz*pz);
		
		if(!(m_isFixMass == KF_FIX_MASS))
		{
		  E=m_al_1[index1*KF_NUM7+3][0];
		}
		
		cout <<"SetOutputToTCandidate"<<px<<" "<<py<<" "<<pz<<" "<<E<<endl;
		
		lv.SetXYZT(px,py,pz,E);
		
		sum+=lv;
		
		/*
		Int_t ncands = cands.GetEntriesFast();
		tcand = new (cands[ncands]) TCandidate(); //Filling TCandidate
// 		it = (VtxFitterParticle*)fArr1->At(i);
		h3v.SetX(m_al_1[index1*KF_NUM7+0][0]);//px
		h3v.SetY(m_al_1[index1*KF_NUM7+1][0]);//py
		h3v.SetZ(m_al_1[index1*KF_NUM7+2][0]);//pz
		tcand->SetP3(h3v);
		if(m_isFixMass == KF_FIX_MASS){
	//Filling Momentum
			tcand->SetP4(TLorentzVector(h3v, sqrt(h3v.Mag2()+0.139*0.139)));//filling 4-mom
		}else{
			tcand->SetP4(TLorentzVector(h3v, m_al_1[index1*KF_NUM7+3][0]));
		}
      //Filling Position
		tcand->SetPosition(TVector3(m_al_1[index1*KF_NUM7+4][0],
				m_al_1[index1*KF_NUM7+5][0],
				m_al_1[index1*KF_NUM7+6][0]));
      
		//convert 6x6 cov matrices to 7x7
		trCov = it->GetFitError(tcand->P4(),m_V_al_1.GetSub(index1*KF_NUM6,
					(index1+1)*KF_NUM6-1,
					index1*KF_NUM6,(index1+1)*KF_NUM6-1));
		//Convert cov matrices px,py,pz,E,x,y,z ==> x,y,z,px,py,pz,E
		//because TCandidate uses the above format
		TMatrixD trCovC = it->GetConverted7(trCov);
		tcand->SetCov7(trCovC); //filled in TCandidate
		
		if(m_errorFlag != KF_NO_ERROR)break;
		*/
		++index1;
	}
	//Will be filled later since there is no function available to store 
	//Vertex info in TCandidate
	/*	
	if(m_fitIncludingVertex == 0){
		it->SetVertex(m_vertex_b);
	}else{
		it->SetVertex(TVector3(m_al_1[KF_NUM7*m_trackNum+0][0],m_al_1[KF_NUM7*m_trackNum+1][0],m_al_1[KF_NUM7*m_trackNum+2][0])); //SetVertex

    //Error of the Vertex and Error between vertex and tracks will be implemented later : Dipak
	}*/
	
	cout <<"sum: "<<sum.Px()<<" "<<sum.Py()<<" "<<sum.Pz()<<" "<<sum.M()<<endl;
        //cout <<"vtx: "<<m_v_a[0][0]<<" "<<m_v_a[1][0]<<" "<<m_v_a[2][0]<<endl;
        fHeadOfTree->SetP4(sum);
	//fHeadOfTree->SetPos(TVector3( m_v_a[0][0], m_v_a[1][0],m_v_a[2][0]));
        fHeadOfTree->SetPos(TVector3(m_al_1[KF_NUM7*m_trackNum+0][0],m_al_1[KF_NUM7*m_trackNum+1][0],m_al_1[KF_NUM7*m_trackNum+2][0]));
        cout <<"SetOutputToTCandidate head of Tree:"<<(*fHeadOfTree)<<endl;
	return m_errorFlag;
}


unsigned PndMassFitter::CalDgf()
{
  m_dgf = 1;
  return m_dgf;
}

void PndMassFitter::SetInvariantMass(Double_t m)
{m_invariantMass = m;}

void PndMassFitter::SetAtDecayPoint()
{m_atDecayPoint = KF_AT_DECAY_POINT;}

void PndMassFitter::SetNotDecayPoint()
{m_atDecayPoint = KF_NOT_DECAY_POINT;}

Double_t
PndMassFitter::GetInvariantMass()
{return m_invariantMass;}

unsigned PndMassFitter::GetDecayPoint()
{return m_atDecayPoint;}


//void PndMassFitter::Finish()
//{cout<<"I'm Outting from PndMassFitter======="<<endl;}