// Simone Bianco 19/05/2010

// This Class' Header ------------------
#include "PndMvdScattLinFitTask.h"
#include "PndScatTrack.h"

// C/C++ Headers ----------------------
#include <iostream>

// Collaborating Class Headers --------
#include "FairRootManager.h"
#include "TClonesArray.h"

//#include "PndLinTrack.h"
#include "../../../pnddata/TrackData/PndTrackCand.h"
#include "../../../pnddata/SdsData/PndSdsHit.h"
#include "../../../pnddata/MvdData/PndMvdHit.h"
#include "../../../pnddata/TrackData/PndTrackCandHit.h"

#include "TFile.h"
#include "TGeoTrack.h"
#include "TGeoManager.h"
#include "TLorentzVector.h"

// Fit Classes -----------
#include <TMath.h>
#include <TVector3.h>
#include <TRandom2.h>
//#include <TStyle.h>
//#include <TCanvas.h>
#include <TF2.h>
#include <TH1.h>
#include <TVirtualFitter.h>
#include <TPolyLine3D.h>
#include <Math/Vector3D.h>

using namespace ROOT::Math;
using namespace std;


PndMvdScattLinFitTask::PndMvdScattLinFitTask()
  : FairTask("3D-Straight-Line-Fit")
{
  fTCandBranchName = "MVDHitsStrip";
  fRecoBranchName = "MVDHitsStrip";
  fZcarbon = 110.;
}

PndMvdScattLinFitTask::PndMvdScattLinFitTask(Double_t posZcarbon)
  : FairTask("3D-Straight-Line-Fit")
{
  fTCandBranchName = "MVDHitsStrip";
  fRecoBranchName = "MVDHitsStrip";
  fZcarbon = posZcarbon;
}

PndMvdScattLinFitTask::~PndMvdScattLinFitTask()
{

}

InitStatus PndMvdScattLinFitTask::Init()
{
  //Get ROOT Manager
  FairRootManager* ioman= FairRootManager::Instance();

  if(ioman==0)
    {
      Error("PndMvdScattLinFitTask::Init","RootManager not instantiated!");
      return kERROR;
    }

  //Get input collection
  
  fTCandArray=(TClonesArray*) ioman->GetObject(fTCandBranchName);
  
  if(fTCandArray==0)
    {
      Error("PndMvdScattLinFitTask::Init","trackcand-array not found!");
      return kERROR;
    }
  
  
  fTrackArray = new TClonesArray("PndScatTrack");
  ioman->Register("MVDTrack", "PndMvd", fTrackArray, kTRUE);
  
  std::cout << "-I- PndMvdScattLinFitTask: Initialisation successfull" << std::endl;
  
  fEvent = 0;

  check = 0;
  track = 0;

  return kSUCCESS;
}


void PndMvdScattLinFitTask::Exec(Option_t* opt)
{
  track = 0;
  Int_t ntcand=fTCandArray->GetEntriesFast();
  // std::cout << "Entries: " << ntcand << std::endl;
  std::map<TString,Int_t> Sensors;
  std::map<Double_t,TString> SensorsPos;

  Double_t eloss[6] = {0.,0.,0.,0.,0.,0.};

  Int_t sumHits = 0;

  {
    //std::cout<<"PndMvdScattLinFitTask::Exec"<<std::endl;
    // Reset output Array
    if(fTrackArray==0) Fatal("PndMvdScattLinFitTask::Exec","No TrackArray");
    fTrackArray->Delete();
    
    TString name;

    
    // Detailed output
    //std::cout<<" -I- PndMvdScattLinFitTask: contains "<<ntcand<<" TCandidates"<<std::endl;
    {
      //std::cout<< " Detailed Debug info on the candidates:"<<std::endl;
      unsigned int detid=12345, index=12345;
      for(Int_t itr=0;itr<ntcand;++itr){
	
	PndMvdHit *theHit = (PndMvdHit*)   fTCandArray->At(itr);
	
	index = theHit->GetRefIndex();
	detid = theHit->GetDetectorID();

	name = theHit->GetDetName();
	
	Sensors[name] += 1;

	//	if (!SensorsPos[theHit->GetZ()]) SensorsPos[theHit->GetZ()] = theHit->GetDetName();

	if (SensorsPos.size() < 6) SensorsPos[theHit->GetZ()] = name;

	sumHits++;

      }
    }
  }

  const Int_t sizeMap = Sensors.size();
  
  Int_t Num[sizeMap];
  TString DetNames[sizeMap];

  Int_t k = 0;
  
  if (SensorsPos.size()>=6 && check == 0)
    {
      cout << "Number of Sensors:" << SensorsPos.size() << endl;
      for (std::map<Double_t,TString>::iterator it=SensorsPos.begin();it!=SensorsPos.end();++it)
	{
	  cout << "position: " << it->first << " name: " << (it->second) << endl;
	}
      check = 1;

      cout << "prova" << endl;
      cout << SensorsPos[0] << endl;
    }

  Int_t ibef = 0, iaft = 0; 
      
  if (sizeMap == 6 && sumHits == 6)
    
    {

      PndMvdHit *Hits[6];
            
      
      for(Int_t it1=0; it1<ntcand; it1++) // on first det
	{

	  Int_t c = 1;

	  PndMvdHit *theHit = (PndMvdHit*)   fTCandArray->At(it1);
 	  	  
	  TString theName = theHit->GetDetName();

	  for (std::map<Double_t,TString>::iterator it=SensorsPos.begin();it!=SensorsPos.end();++it)
	    {
	      if (theName == it->second) 
		{
		  //Hits[c] = (PndMvdHit*)   fTCandArray->At(it1);
		  
		  TVector3 posizioni(theHit->GetX(),theHit->GetY(),theHit->GetZ());
		  TVector3 errori(theHit->GetDx(),theHit->GetDy(),theHit->GetDz());

		  Hits[c] = new PndMvdHit(theHit->GetDetectorID(),theHit->GetDetName(), posizioni,errori,0,theHit->GetCharge(),theHit->GetNDigiHits(),0);
		}
	      c++;
	    }
 
	}

      //      cout << "I " << Hits[1]->GetDx() << endl;

      // To print info on sensors

      for (Int_t ww=1;ww<=6;ww++)
	{
	  //std::cout << Hits[ww]->GetZ() << " " << Hits[ww]->GetDx() << " " << Hits[ww]->GetDy() << std::endl;
	}


 
      // just faking data to check if everything works well
     
  //     Hits[0]->SetX(0.);
//       Hits[0]->SetY(0.);
//       Hits[1]->SetX(0.1);
//       Hits[1]->SetY(0.);
//       Hits[2]->SetX(0.100669);
//       Hits[2]->SetY(0.);
   
//       Hits[3]->SetX(0.1839);
//       Hits[3]->SetY(0.);
//       Hits[4]->SetX(0.18458996);
//       Hits[4]->SetY(0.);
//       Hits[5]->SetX(0.24788);
//       Hits[5]->SetY(0.);


      Test(Hits[1],Hits[2],Hits[3],0);
      Test(Hits[4],Hits[5],Hits[6],1);
      


	    
    }

  if (fEvent%1000==0) std::cout << "Event n. " << fEvent << endl;

  fEvent++;

  return;
}

void PndMvdScattLinFitTask::Test(PndMvdHit *p1, PndMvdHit *p2, PndMvdHit *p3, Int_t stat) // if stat = 0 -> before, if stat = 1 -> after
{

  Double_t slopeX,slopeY,coordX,coordY,buffer;

  //cout << p1->GetDx() << " " << p2->GetDx() << " " << p3->GetDx() << endl;
  Bool_t c1x=0,c2x=0,c3x=0,c1y=0,c2y=0,c3y=0;

  
  // checking if the sensor are double sided
  
  if (TMath::Abs(p1->GetDx()) < 0.5)  c1x = 1;
  if (TMath::Abs(p1->GetDy()) < 0.5) c1y = 1;
  if (TMath::Abs(p2->GetDx()) < 0.5) c2x = 1;
  if (TMath::Abs(p2->GetDy()) < 0.5) c2y = 1;
  if (TMath::Abs(p3->GetDx()) < 0.5) c3x = 1;
  if (TMath::Abs(p3->GetDy()) < 0.5) c3y = 1;


  if ((c1x+c2x+c3x+c1y+c2y+c3y)>4)
    {
      cout << "ERROR: Too many sensors! More than two points..." << endl;
      cout << " c1x : " << c1x << " err " << p1->GetDx() << " z: " << p1->GetZ() << endl;
      cout << " c1y : " << c1y << " err " << p1->GetDy() << " z: " << p1->GetZ()  << endl;
      cout << " c2x : " << c2x << " err " << p2->GetDx() << " z: " << p2->GetZ()  << endl;
      cout << " c2y : " << c2y << " err " << p2->GetDy() << " z: " << p2->GetZ()  << endl;
      cout << " c3x : " << c3x << " err " << p3->GetDx() << " z: " << p3->GetZ()  << endl;
      cout << " c3y : " << c3y << " err " << p3->GetDy() << " z: " << p3->GetZ()  << endl;
      return;
    }
  else
    {


      // looking at x coordinates

      if (c1x && c2x)
	{
	  slopeX = (p2->GetX() - p1->GetX()) / (p2->GetZ() - p1->GetZ());
	  // x1 = slopeX*z1 + buffer
	  buffer = (p1->GetX()) - ((p1->GetZ())*slopeX);
	  coordX = slopeX*fZcarbon + buffer;
	  //cout << "X I" << endl;
	}
      if (c1x && c3x)
	{
	  slopeX = (p3->GetX() - p1->GetX()) / (p3->GetZ() - p1->GetZ());
	  buffer = (p1->GetX()) - ((p1->GetZ())*slopeX);
	  coordX = slopeX*fZcarbon + buffer;
	  // cout << "X II" << endl;
	}
      if (c2x && c3x)
	{
	  slopeX = (p3->GetX() - p2->GetX()) / (p3->GetZ() - p2->GetZ());
	  buffer = (p2->GetX()) - ((p2->GetZ())*slopeX);
	  coordX = slopeX*fZcarbon + buffer;
	  // cout << "X III" << endl;
	}
	

      // looking at y coordinates

      if (c1y && c2y)
	{
	  slopeY = (p2->GetY() - p1->GetY()) / (p2->GetZ() - p1->GetZ());
	  // y1 = slopeY*z1 + buffer
	  buffer = (p1->GetY()) - ((p1->GetZ())*slopeY);
	  coordY = slopeY*fZcarbon + buffer;
	  // cout << "Y I" << endl;
	}
      if (c1y && c3y)
	{
	  slopeY = (p3->GetY() - p1->GetY()) / (p3->GetZ() - p1->GetZ());
	  buffer = (p1->GetY()) - ((p1->GetZ())*slopeY);
	  coordY = slopeY*fZcarbon + buffer;
	  //  cout << "Y II" << endl;
	}
      if (c2y && c3y)
	{
	  slopeY = (p3->GetY() - p2->GetY()) / (p3->GetZ() - p2->GetZ());
	  buffer = (p2->GetY()) - ((p2->GetZ())*slopeY);
	  coordY = slopeY*fZcarbon + buffer;
	  // cout << "Y III" << endl;
	}
      
      // cout << "----------------------------------------" << endl;
    //   cout << "P1 " << p1->GetX() << " "  << p1->GetY() << " "  << p1->GetZ() << endl;
//       cout << "Err " << p1->GetDx() << " "  << p1->GetDy() << " "  << p1->GetDz() << endl;

//       cout << "P2 "  << p2->GetX() << " "  << p2->GetY() << " "  << p2->GetZ() << endl;
//       cout << "Err " << p2->GetDx() << " "  << p2->GetDy() << " "  << p2->GetDz() << endl;

//       cout << "P3 "  << p3->GetX() << " "  << p3->GetY() << " "  << p3->GetZ() << endl;
//       cout << "Err " << p3->GetDx() << " "  << p3->GetDy() << " "  << p3->GetDz() << endl;

//       cout << "SlopeX: " << slopeX << " CoordX: " << coordX << endl;
//       cout << "SlopeY: " << slopeY << " CoordY: " << coordY << endl;
      
      PndScatTrack* sl = new PndScatTrack(slopeX,slopeY,coordX,coordY,fZcarbon,p1->GetEloss(),p2->GetEloss(),p3->GetEloss(),stat);

      new((*fTrackArray)[track]) PndScatTrack(*(sl));
      track++;

    }
  
  

}

ClassImp(PndMvdScattLinFitTask);