//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TpcRecoTester
//      see TpcRecoTester.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//
//
//-----------------------------------------------------------

// Panda Headers ----------------------

// This Class' Header ------------------
#include "TpcRecoTester.h"

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

// Collaborating Class Headers --------
#include "TpcCluster.h"
#include "Kalman.h"
#include "TpcClusterRadius.h"
#include "TpcConfMapRecoHit.h"
#include "TpcConfMapFit.h"
#include "DetPlane.h"
#include "TpcConfTrackFinder.h"
#include "TrackCand.h"
#include "Track.h"

#include "TGraph.h"
#include "TFile.h"
#include "TRandom.h"
#include "TH1I.h"
#include "DebugLogger.h"
#include "dbgstream.h"


// Class Member definitions -----------

ClassImp(TpcRecoTester)

void
TpcRecoTester::testConfMap(){
  TpcCluster mycl(TVector3(5,5,3),1,0);
  TpcConfMapRecoHit hit(&mycl);
  hit.getHitCoord(DetPlane()).Print();
  hit.setReferencePoint(1,1);
  hit.getHitCoord(DetPlane()).Print();

  //DebugLogger::Instance();

  LOG<<"blablabla"<<std::endl;

  odbgstream dbg(std::cout.rdbuf());
  dbg<<"Hi! this is from dgbstream"<<std::endl;
  //SetLevel lev=setlevel(2);
  dbg << level(2) << "an error message" <<std::endl;
  
}

void
TpcRecoTester::testConfMapFit(){
  TpcConfMapFit myfit;
  TMatrixT<double> cov(2,2);
  //cov[0][0]=10;
  //cov[1][1]=10;
  //cov[0][1]=-1;
  //cov[1][0]=-1;
  //myfit.setCov(cov);
  myfit.Print();
  TpcCluster mycl(TVector3(5,5,3),1,0);
  TpcConfMapRecoHit hit(&mycl);
  
  
 
  std::cout<<"Residual:"<<std::endl;
  hit.residualVector(&myfit,myfit.getState()).Print();

  Kalman myfitter;
  double chi2=myfitter.getChi2Hit(&hit,&myfit);
  std::cout<<"Chi2="<<chi2<<std::endl;

}

void
TpcRecoTester::testBasicConfFit(){
  TpcConfMapRecoHit hit1(0.5,0.51);
  TpcConfMapRecoHit hit2(0.7,0.68);
  TpcConfMapRecoHit hit3(1.2,1.22);
  TpcConfMapRecoHit hit4(1.2,1.24);
  TpcConfMapRecoHit hit5(1.2,1.26);
  TpcConfMapRecoHit hit6(1.2,1.28);

  Track* trk=new Track(new TpcConfMapFit());
  TMatrixT<double> par(2,1);
  par[0][0]=1;
  par[1][0]=0;
  trk->getTrackRep(0)->setState(par);
  trk->getTrackRep(0)->Print();
  
   Kalman myfitter;
  double chi2=myfitter.getChi2Hit(&hit1,trk->getTrackRep(0));
  std::cout<<"Chi2 for hit1="<<chi2<<std::endl;

  trk->addHit(&hit1,2,0);
  myfitter.continueTrack(trk);
  trk->getTrackRep(0)->Print();

  
  chi2=myfitter.getChi2Hit(&hit2,trk->getTrackRep(0));
  std::cout<<"Chi2 for hit2="<<chi2<<std::endl;

  trk->addHit(&hit2,2,1);
  myfitter.continueTrack(trk);
  trk->getTrackRep(0)->Print();
  
  chi2=myfitter.getChi2Hit(&hit3,trk->getTrackRep(0));
  std::cout<<"Chi2 for hit3="<<chi2<<std::endl;
  chi2=myfitter.getChi2Hit(&hit4,trk->getTrackRep(0));
 std::cout<<"Chi2 for hit4="<<chi2<<std::endl;
  chi2=myfitter.getChi2Hit(&hit5,trk->getTrackRep(0));
 std::cout<<"Chi2 for hit5="<<chi2<<std::endl;
  chi2=myfitter.getChi2Hit(&hit6,trk->getTrackRep(0));
 std::cout<<"Chi2 for hit6="<<chi2<<std::endl;
}

int
TpcRecoTester::testConfFinder(){

  std::vector<TpcCluster*> cll; // clusterlist
  maketoytracks(5,cll);

  //std::cout<<cll.size()<<" clusters produced."<<std::endl;

  TpcConfTrackFinder _finder;
  _finder.configure(1,1,2, // proximity cuts
		    3,     // chi2cut
		    3,     // numhitsforfit 
		    true,
		    0.05,0.05, // merge cuts
		    4.);   // proximity merge cut
  
  std::vector<TrackCand*> cands;
  _finder.buildTracks(cll,cands);

  //std::cout<<cands.size()<<" track candidates found."<<std::endl;
  for(unsigned int i=0; i<cands.size(); ++i){
    //std::cout<<" candidate"<<i<<" with "<<cands[i]->getNHits()<<" hits"<<std::endl;
  }
  return cands.size();
}

void
TpcRecoTester::testRotated(){
  TpcConfMapRecoHit hit1(new TpcCluster(TVector3(5,5,3),1,0));
  std::cout<<hit1.getHitCoord(DetPlane())[0][0]<<" "
	   <<hit1.getXcf()<<std::endl;
  hit1.setRotated();
  std::cout<<hit1.getHitCoord(DetPlane())[0][0]<<" "
	   <<hit1.getXcf()<<std::endl;
  hit1.setRotated(false);
  std::cout<<hit1.getHitCoord(DetPlane())[0][0]<<" "
	   <<hit1.getXcf()<<std::endl;
}


void
TpcRecoTester::testConfFinderStat(){
  TH1I* histo=new TH1I("nfound","nfound",30,0,30);
  for(int ev=0;ev<1000;++ev){
    histo->Fill(testConfFinder());
  }
  TFile* outfile=new TFile("toystat.root","RECREATE");
  histo->Write();
  outfile->Close();
}


void
TpcRecoTester::maketoytracks(int n,std::vector<TpcCluster*>& cll)
{
//produce hits
  TGraph* hits=new TGraph();
  TGraph* confhits=new TGraph();
  TGraph* hitsxz=new TGraph();

  unsigned int hitcounter=0;
  double sig=0.01; // sigma in space

  for(int t=0;t<n;++t){// track loop
    
    unsigned int hitsintrk=0;

    double r=gRandom->Uniform(30,70);
    std::cout<<"Radius r="<<r<<std::endl;
    TVector2 start(0,0);
    double dirstartx;
    double dirstarty;
    gRandom->Circle(dirstartx,dirstarty,15);
    //double phistart=TMath::Pi()/180*85;
    //dirstartx=cos(phistart);
    //dirstarty=sin(phistart);

    TVector2 dir(dirstartx,dirstarty);
    TVector2 radial=dir.Unit().Rotate(TMath::Pi()/2)*r;
    TVector2 origin=start+radial;
    
    double dphi=0.5/r; // a hit every 5 mm 
    double dz_dphi=gRandom->Uniform(1.,100.);


    for(int i=1; i<60; ++i){// hit loop 1 degrees
      double phi=i*dphi;
      TVector2 point=origin-radial.Rotate(phi);
      double xhit=point.X()+gRandom->Gaus(0,sig);
      double yhit=point.Y()+gRandom->Gaus(0,sig);
      double zhit=phi*dz_dphi+gRandom->Gaus(0,sig);
      double r=sqrt(xhit*xhit+yhit*yhit);
      if(r<5) continue;
      hits->SetPoint(hitcounter,xhit,yhit);
      hitsxz->SetPoint(hitcounter,xhit,zhit);
      TpcCluster* cl=new TpcCluster(TVector3(xhit,yhit,zhit),1,hitcounter);
      TpcConfMapRecoHit conf(cl);
      conf.setRotated();
      cll.push_back(cl);
      confhits->SetPoint(hitcounter,conf.getXcf(),conf.getHitCoord(DetPlane())[0][0]);
      ++hitcounter;
      ++hitsintrk;
    }
    //std::cout<<hitsintrk<<" hits in track"<<t<<std::endl;
  }
  TFile* outfile=new TFile("toyhits.root","RECREATE");
  hits->Write("hits");
  hitsxz->Write("hitsxz");
  confhits->Write("confhits");

  outfile->Close();
}