// --------------------------------------------------------------------------------------
// -----                 CbmRichRingFinderHough source file                         -----
// ----- Algorithm idea: G.A. Ososkov (ososkov@jinr.ru) and Simeon Lebedev (salebedev@jinr.ru)                            -----
// ----- Implementation: Simeon Lebedev (salebedev@jinr.ru)  and Andrei Lebedev (alebedev@jinr.ru)-----
/*
This program performs the procedure of the ring recognition from data obtained from
the CBM RICH detector.

the main program calls sequentially the following procedures:
1) init() initialization: input data, find max and min values of X and Y to define the
histogramming range.
2) find_clusters(): coarse histogramming of source data, cluster them by the nearest
neighbour algoritm; then make a decimation of points in every area taking each
second point to speed up calculations.
3)Hough transform (HT) consists in calculating centers and radii of circles drawn
through every possible triplet of points and then 2D histogramming these centers.
Each time we test distances between points and obtained radii of a
triplet to be within prescribed limits.

Eliminate From 2D histogram all bins with the number of points less than given limit are eliminated
Then all separated areas are clustered.

In each of found areas centers and radii are approximated according to the following rules:

a.  Initiate by marking all bins as non-searched;

b.  Look for bins with maximum values x_max, y_max.

c.  If values in all bins adjacent to x_max, y_max are not
exceeding them, calculate the center as the center of gravity (see
item 9 below) and mark these bins as searched.

d.  Go on until exhausting all non-marked bins.
*/
#include <iostream>
#include <cmath>

#include "CbmRichRingFinderHough.h"
#include "CbmRichHit.h"
#include "CbmRichLightSpot.h"
#include "CbmRichRing.h"
#include "CbmTrackParam.h"

//--------------------------------------------------------------------------
// -----   Default constructor   -------------------------------------------
CbmRichRingFinderHough::CbmRichRingFinderHough()
{
	fVerbose = 1;
	fh_dist = new TH1F("h1","distance",50,0,5);
}

// -----   Standard constructor   ------------------------------------------
CbmRichRingFinderHough::CbmRichRingFinderHough  ( Int_t verbose )
{
	fVerbose = verbose;
	  fh_dist = new TH1F("h1","distance",50,0,5);
}

// -----   Destructor   ----------------------------------------------------
CbmRichRingFinderHough::~CbmRichRingFinderHough()
{
}


void CbmRichRingFinderHough::Init()
{
	cout << "CbmRichRingFinderHough::init() "<<endl;
	InitArray();
}

Int_t CbmRichRingFinderHough::DoFind(TClonesArray* rHitArray,
				     TClonesArray* rProjArray,
		       		     TClonesArray* rRingArray) {
	fData.clear();
	fGuidance.clear();
	for (int i = 0; i < fNIterations; i++) {
		fFoundRings[0][i].clear();
		fFoundRings[1][i].clear();
	}
 	++fNEvent;
	cout << "-------------------------    Event no. " << fNEvent << "   -------------------------"<<endl;

	cout << "Read Data begin" << endl;

	std::vector<CbmRichMyPoint> UpH;
	std::vector<CbmRichMyPoint> DownH;


	if ( !rHitArray) {
    		cout << "-E- CbmRichRingFinderTrack::DoFind: Hit array missing! " << rHitArray << endl;
		return -1;
	}
	const Int_t nhits = rHitArray->GetEntriesFast();
	if(!nhits) {
		cout << "No hits in this event." << endl;
		return -1;
	}
	for(Int_t iHit = 0; iHit < nhits; iHit++) {
		CbmRichHit * hit = (CbmRichHit*)rHitArray->At(iHit);
		if(hit){
			CbmRichMyPoint tempPoint;
			if (hit->Y() >= 0) kYMIN = 80;
			else kYMIN = -250;
			tempPoint.fX = hit->X() - kXMIN;
			tempPoint.fY = hit->Y() - kYMIN;
			tempPoint.fId = iHit;
			tempPoint.fRefIndex = hit->GetRefIndex();
			if (hit->Y() >= 0)
				UpH.push_back(tempPoint);
			else DownH.push_back(tempPoint);
		}
	}
	cout << "fData.size() = "<< fData.size()<<endl;


	const Int_t npoints = rProjArray->GetEntriesFast();
	if(!npoints) {
		cout << "No projections in this event." << endl;
		return -1;
	}

	for(Int_t iPoint = 0; iPoint < npoints; iPoint++) {
		CbmTrackParam * point = (CbmTrackParam*)rProjArray->At(iPoint);
		if(point) {
			CbmRichMyPoint tempPoint;
			tempPoint.fX = point->GetX();
			tempPoint.fY = point->GetY();
			tempPoint.fId = iPoint;
			fGuidance.push_back(tempPoint);
			//cout<<"X="<<fGuidance[iPoint].fX <<" Y="<< fGuidance[iPoint].fY<<endl;
		}
	}
	cout <<"fGuidance.size() = "<< fGuidance.size()<< endl;

	cout <<"Read Data end"<< endl;
	fData = UpH;
	kYMIN = 80;
	kYMAX = 250;
	cout<<"UpH.size() = "<< fData.size()<<endl;
	CbmRichHit *crr = new CbmRichHit;
	int iIter;
	for (iIter = 0; iIter < fNIterations; iIter++){
		std::sort(fData.begin(), fData.end(), CbmRichMyPoint::CmpUp);
		fCurIteration = iIter;
		cout << "Current iteration = "<< fCurIteration << endl;
		InitHist(fNBinsX[fCurIteration], fNBinsY[fCurIteration]);
		FindClusters(fNBinsX[fCurIteration], fNBinsY[fCurIteration]);
		InitClusters(fNBinsX[fCurIteration], fNBinsY[fCurIteration]);
		HoughTransform(fHTCut[fCurIteration], fHTNBinsX[fCurIteration], fHTNBinsY[fCurIteration]);
		FindHTClusters(fHTNBinsX[fCurIteration], fHTNBinsY[fCurIteration], fHitCut[fCurIteration],0);
		int iH;
		cout <<"Number of found rings "<<iIter<<" iteration = " <<fFoundRings[0][fCurIteration].size() <<endl;
		int erazeCount = 0;
		for(unsigned int i = 0; i < fFoundRings[0][fCurIteration].size(); i++) {
			for (iH = 0; iH < fFoundRings[0][fCurIteration][i].GetNoOfHits(); iH ++)
				for (unsigned int j = 0 ; j < fData.size(); j++){

					crr = fFoundRings[0][fCurIteration][i].GetHit(iH);
					if (crr->Z() == fData[j].fId){
                                           	fData[j].fId = 7777777;
						erazeCount++;
					}
				}
  		}
		sort(fData.begin(),fData.end(),CmpSortMyPoint());
		fData.resize(fData.size() - erazeCount);
	}
	fData.clear();
	fData = DownH;
	cout<<"UpH.size() = "<< fData.size()<<endl;
	kYMIN = -250;
	kYMAX = -80;


	for (iIter = 0; iIter < fNIterations; iIter++){
		std::sort(fData.begin(), fData.end(), CbmRichMyPoint::CmpUp);
		fCurIteration = iIter;
		cout << "Current iteration = "<< fCurIteration << endl;
		InitHist(fNBinsX[fCurIteration], fNBinsY[fCurIteration]);
		FindClusters(fNBinsX[fCurIteration], fNBinsY[fCurIteration]);
		InitClusters(fNBinsX[fCurIteration], fNBinsY[fCurIteration]);
		HoughTransform(fHTCut[fCurIteration], fHTNBinsX[fCurIteration], fHTNBinsY[fCurIteration]);
		FindHTClusters(fHTNBinsX[fCurIteration], fHTNBinsY[fCurIteration], fHitCut[fCurIteration],1);
		int iH;
		cout <<"Number of found rings "<<iIter<<" iteration = " <<fFoundRings[1][fCurIteration].size() <<endl;
		int erazeCount = 0;
		for(unsigned int i = 0; i < fFoundRings[1][fCurIteration].size(); i++) {
			for (iH = 0; iH < fFoundRings[1][fCurIteration][i].GetNoOfHits(); iH ++)
				for (unsigned int j = 0 ; j < fData.size(); j++){
					//CbmRichHit *crr = new CbmRichHit;
					crr = fFoundRings[1][fCurIteration][i].GetHit(iH);
					if (crr->Z() == fData[j].fId){
						fData[j].fId = 7777777;
						erazeCount++;
					}
					//delete crr;
				}
  		}
		sort(fData.begin(),fData.end(),CmpSortMyPoint());
		fData.resize(fData.size() - erazeCount);
	}



	//Add found rings to output file
	cout<<"Write found rings to output file"<<endl;
	unsigned int iRing;
	int foundRingsCount = 0;


	int countUsedHits[200];
	for (int iUD = 0; iUD < 2; iUD++){
		for (iIter = 0; iIter < fNIterations; iIter++)
			for(iRing=0; iRing < fFoundRings[iUD][iIter].size(); iRing++) {

				/*for (int iT = 0 ; iT < fFoundRings[iUD][iIter][iRing].GetNoOfHits(); iT++)countUsedHits[iT] = 0;

				for(int iRing1=0; iRing1 < fFoundRings[iUD][iIter].size(); iRing1++)
				if (fFoundRings[iUD][iIter][iRing].GetCenterX()-fFoundRings[iUD][iIter][iRing1].GetCenterX() < 20 &&
				fFoundRings[iUD][iIter][iRing].GetCenterY()-fFoundRings[iUD][iIter][iRing1].GetCenterY() < 20 &&
				iRing != iRing1){
					for(int iH1 = 0; iH1 < fFoundRings[iUD][iIter][iRing].GetNoOfHits(); iH1++)
						for(int iH2 = 0; iH2 < fFoundRings[iUD][iIter][iRing1].GetNoOfHits(); iH2++){
							if (fFoundRings[iUD][iIter][iRing].GetHitIndex(iH1) ==
							fFoundRings[iUD][iIter][iRing1].GetHitIndex(iH2)) countUsedHits[iH1]++;
					}
				}
				int contNumberOfUsedHits = 0;
				for(int iH1 = 0; iH1 < fFoundRings[iUD][iIter][iRing].GetNhits(); iH1++)
					if (countUsedHits[iH1] == 0 )contNumberOfUsedHits ++;

				cout<< "contNumberOfUsedHits = "<<contNumberOfUsedHits<<endl;
				//fFoundRings[iUD][iIter][iRing].fNUsedHits = 1 -
				//           (double)contNumberOfUsedHits / (double) fFoundRings[iUD][iIter][iRing].GetNhits();*/
				double mind = 999.0;
				if(iUD == 0) kYMIN = 80; else kYMIN = -250;
				fFoundRings[iUD][iIter][iRing].SetCenterX(fFoundRings[iUD][iIter][iRing].GetCenterX() + kXMIN);
				fFoundRings[iUD][iIter][iRing].SetCenterY(fFoundRings[iUD][iIter][iRing].GetCenterY() + kYMIN);
				//fFoundRings[iUD][iIter][iRing].RingFitCOP();

				for (unsigned int iG = 0 ; iG < fGuidance.size() ; iG++){
					double r = pow( pow(fFoundRings[iUD][iIter][iRing].GetCenterX() - fGuidance[iG].fX, 2) +
						    pow(fFoundRings[iUD][iIter][iRing].GetCenterY() - fGuidance[iG].fY, 2) , 0.5 );
					if (mind > r ) mind = r;
				}
				fh_dist->Fill(mind);
				//cout <<"%%% = "<<<<endl;
				//if (mind < 1.2/* && (double)contNumberOfUsedHits / (double) fFoundRings[iUD][iIter][iRing].GetNhits()  > 0.8*/) {
					new ((*rRingArray)[foundRingsCount]) CbmRichRing();
					CbmRichRing * r = (CbmRichRing *)rRingArray->At(foundRingsCount);
					r->SetCenterX(fFoundRings[iUD][iIter][iRing].GetCenterX());
					r->SetCenterY(fFoundRings[iUD][iIter][iRing].GetCenterY());
					r->SetRadius(fFoundRings[iUD][iIter][iRing].GetRadius());
					for (int ith = 0; ith < fFoundRings[iUD][iIter][iRing].GetNoOfHits();ith ++){
						r->AddHit(fFoundRings[iUD][iIter][iRing].GetHit(ith));
					}
					cout<<"(x,y)="<<fFoundRings[iUD][iIter][iRing].GetCenterX()<<
							" "<< fFoundRings[iUD][iIter][iRing].GetCenterY()<<
							endl;
					foundRingsCount++;
				//}
			}
	}

	cout <<"fcount = "<<foundRingsCount<< endl;
	//cout << "Number of input hits: " << fHitCollection->GetEntriesFast() << endl;
	cout << "Number of output rings: " << rRingArray->GetEntriesFast() << endl;

}

void CbmRichRingFinderHough::Finish()
{
	c2 = new TCanvas("c2","c2",800,1000);
	fh_dist->Draw();
}
void CbmRichRingFinderHough::SetParameters(int NIterations,
					   int NBinsX[], int NBinsY[],
                                           int HTNBinsX[], int HTNBinsY[],
					   int HTCut[], int HitCut[],
					   double MaxDistance[], double MinDistance[],
				           double MaxRadius[], double MinRadius[],
					   int NParts[], int SqLen)
{
	cout <<"SetParameters begin"<< endl;
	fNIterations = NIterations;
	fNBinsX.push_back(NBinsX[0]);//first iteration axis X
	fNBinsY.push_back(NBinsY[0]);//first iteration axis Y
	fNBinsX.push_back(NBinsX[1]);//second iteration axis X
	fNBinsY.push_back(NBinsY[1]);//second iteration axis Y
	fNBinsX.push_back(NBinsX[2]);//third iteration axis X
	fNBinsY.push_back(NBinsY[2]);//third iteration axis Y

	fHTNBinsX.push_back(HTNBinsX[0]);//first iteration axis X
	fHTNBinsY.push_back(HTNBinsY[0]);//first iteration axis Y
	fHTNBinsX.push_back(HTNBinsX[1]);//second iteration axis X
	fHTNBinsY.push_back(HTNBinsY[1]);//second iteration axis Y
	fHTNBinsX.push_back(HTNBinsX[2]);//third iteration axis X
	fHTNBinsY.push_back(HTNBinsY[2]);//third iteration axis Y

	fHTCut.push_back(HTCut[0]);//first iteration
	fHTCut.push_back(HTCut[1]);//second iteration
	fHTCut.push_back(HTCut[2]);//third iteration

	fHitCut.push_back(HitCut[0]);//first iteration
	fHitCut.push_back(HitCut[1]);//second iteration
	fHitCut.push_back(HitCut[2]);//third iteration

	fMaxDistance.push_back(MaxDistance[0]);//first iteration
	fMinDistance.push_back(MinDistance[0]);//first iteration
	fMaxDistance.push_back(MaxDistance[1]);//second iteration
	fMinDistance.push_back(MinDistance[1]);//second iteration
	fMaxDistance.push_back(MaxDistance[2]);//third iteration
	fMinDistance.push_back(MinDistance[2]);//third iteration

	fMinRadius.push_back(MinRadius[0]);//first iteration
	fMaxRadius.push_back(MaxRadius[0]);//first iteration
	fMinRadius.push_back(MinRadius[1]);//secod iteration
	fMaxRadius.push_back(MaxRadius[1]);//second iteration
	fMinRadius.push_back(MinRadius[2]);//third iteration
	fMaxRadius.push_back(MaxRadius[2]);//third iteration

	fNParts.push_back(NParts[0]);//first iteration
	fNParts.push_back(NParts[1]);//second iteration
	fNParts.push_back(NParts[2]);//third iteration


	fSqLen = SqLen;
	cout <<"SetParameters end"<< endl;
}
void  CbmRichRingFinderHough::InitArray()
{
     	int i ,j;
	cout<<"InitArray begin" << endl;
	fFoundRings.resize(2);
	for (i=0; i<2;i++) fFoundRings[i].resize(fNIterations);

	fTempHist.resize(kNBINSX);
        for(i=0;i<kNBINSX;i++) fTempHist[i].resize(kNBINSY);

	fHistClusterNumber.resize(kNBINSX);
       	for (i = 0; i < kNBINSX; i++)  fHistClusterNumber[i].resize(kNBINSY);

	fHist.resize(kNBINSX);
       	for (i = 0; i < kNBINSX; i++)   fHist[i].resize(kNBINSY);
	for (i = 0; i < kNBINSX; i++)
		for (j = 0; j < kNBINSY; j++) fHist[i][j].reserve(kMAX_POINTS_IN_BIN);

	fAr1.resize(kNBINSX);
       	for (i = 0; i < kNBINSX; i++)  fAr1[i].resize(kNBINSY);

	fAr2.resize(kNBINSX);
       	for (i = 0; i < kNBINSX; i++) fAr2[i].resize(kNBINSY);

	fCluster.resize(kMAX_CLUSTERS);
       	for (i = 0; i < kMAX_CLUSTERS; i++)  fCluster[i].resize(kMAX_PARTS);
	for (i = 0; i < kMAX_CLUSTERS; i++)
		for (j = 0; j < kMAX_PARTS; j++) fCluster[i][j].reserve(kMAX_POINTS_IN_CLUSTER);


	fHTHist.resize(kNBINSXHT);
       	for (i = 0; i < kNBINSXHT; i++)  fHTHist[i].resize(kNBINSYHT);

	fHTHistClusterNumber.resize(kNBINSXHT);
       	for (i = 0; i < kNBINSXHT; i++) fHTHistClusterNumber[i].resize(kNBINSYHT);

	fHTHist3.resize(kNBINSXHT);
       	for (i = 0; i < kNBINSXHT; i++)  fHTHist3[i].resize(kNBINSYHT);

	fHTCluster.resize(kMAX_CLUSTERS);
       	for (i = 0; i < kMAX_CLUSTERS; i++) fHTCluster[i].resize(kMAX_POINTS_IN_CLUSTER);

	fHTCheck.resize(kNBINSXHT);
       	for (i = 0; i < kNBINSXHT; i++) fHTCheck[i].resize(kNBINSYHT);

	fHTRadius.resize(kNBINSXHT);
       	for (i = 0; i < kNBINSXHT; i++)  fHTRadius[i].resize(kNBINSYHT);

	fRingHits.resize(kNBINSXHT);
       	for (i = 0; i < kNBINSXHT; i++)   fRingHits[i].resize(kNBINSYHT);
	for (i = 0; i < kNBINSXHT; i++)
		for (j = 0; j < kNBINSYHT; j++) fRingHits[i][j].reserve(kMAX_POINTS);


	fData.reserve(kMAX_POINTS);
	fGuidance.reserve(kMAX_GUIDANCE);

	cout<<"InitArray end" << endl;
}

void CbmRichRingFinderHough::DeleteArray()
{

}
void CbmRichRingFinderHough::InitHist(double nbinsx, double nbinsy)
{
	double dx, dy;
	int i, j;
	double ix, iy;

	cout << "InitHist begin"<<endl;
	//init arrays
	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++){
			fHistClusterNumber[i][j] = -1;
			fHist[i][j].clear();
		}


	for (i = 0; i < kMAX_CLUSTERS; i++)
		for (j = 0; j< fNParts[fCurIteration]; j++)
		fCluster[i][j].clear();

	//calculate bin size
 	dx = (kXMAX - kXMIN) / nbinsx;
	dy = (kYMAX - kYMIN) / nbinsy;

	//init fHist vector
	for (unsigned int ii = 0; ii < fData.size(); ii++){
		modf(fData[ii].fX  / dx, &ix);
		modf(fData[ii].fY  / dy, &iy);
		if (ix < nbinsx && ix > 0 && iy < nbinsy && iy > 0)
			fHist[(int)ix][(int)iy].push_back(ii);
	}

	//init fTempHist vector
	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++)
			if (fHist[i][j].size() > 0) fTempHist[i][j] = 1; else
				fTempHist[i][j] = 0;


	for (i = fSqLen; i < nbinsx - fSqLen; i++)
		for (j = fSqLen; j< nbinsy - fSqLen; j++){
			if (fHist[i][j].size() > 0 && fTempHist[i][j] == 1){
				for (int x1 = i - fSqLen; x1 < i + fSqLen  ; x1++)
					for (int y1 = j - fSqLen; y1 < j + fSqLen ; y1++)
						if (fHist[x1][y1].size() == 0)
							fHist[x1][y1].resize(1);
			}
		}

	cout<<"InitHist end"<< endl;
}

void CbmRichRingFinderHough::FindClusters(double nbinsx, double nbinsy)
{
	int i,j;
	cout << "FindCluster begin" << endl;
	fNClusters = 0;
	//init ar1[][] and ar2[][]
	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++){
			fAr1[i][j] = fHist[i][j].size();
			fAr2[i][j] = fHistClusterNumber[i][j];
		}

	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++){
			if (fAr1[i][j] > 0 && fAr2[i][j] == -1){
				Wave(i,j, nbinsx, nbinsy);
				fNClusters++;
			}
		}
	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++){
			fHistClusterNumber[i][j] = fAr2[i][j];
		}
	cout<<"FindCluster end"<<endl;

}

// ar1[][] - number of point in the bin
// ar2[][] - cluster number
void CbmRichRingFinderHough::Wave(int x, int y, double nbinsx,double nbinsy)
{
    fAr2[x][y] = fNClusters;

    if (x-1 > 0 && fAr1[x-1][y] > 0 && fAr2[x-1][y] == -1 )
    Wave(x-1,y,nbinsx,nbinsy);

    if (y+1 < nbinsy && fAr1[x][y+1] > 0 && fAr2[x][y+1] == -1 )
    Wave(x,y+1,nbinsx,nbinsy);

    if ( x+1 < nbinsx && fAr1[x+1][y] > 0 && fAr2[x+1][y] == -1)
    Wave(x+1,y,nbinsx,nbinsy);

    if (y-1 > 0 && fAr1[x][y-1] > 0 && fAr2[x][y-1] == -1 )
    Wave(x,y-1,nbinsx,nbinsy);

    //////////////////////////////////////////////////////////////

    if (x-1 > 0 && y-1 > 0 && fAr1[x-1][y-1] > 0 && fAr2[x-1][y-1] == -1 )
    Wave(x-1,y-1,nbinsx,nbinsy);

    if (x+1 < nbinsx && y-1 > 0 && fAr1[x+1][y-1] > 0 && fAr2[x+1][y-1] == -1)
    Wave(x+1,y-1,nbinsx,nbinsy);

    if (x-1 > 0 && y+1 < nbinsy && fAr1[x-1][y+1] > 0 && fAr2[x-1][y+1] == -1 )
    Wave(x-1,y+1,nbinsx,nbinsy);

    if (x+1 < nbinsx && y+1 < nbinsy && fAr1[x+1][y+1] > 0 && fAr2[x+1][y+1] == -1)
    Wave(x+1,y+1,nbinsx,nbinsy);
}


void CbmRichRingFinderHough::InitClusters(double nbinsx, double nbinsy)
{
	int i, j;
	unsigned int k;
        cout << "InitClusters begin" << endl;

	//if bin doesn't consist hits we clear it to make size()==0
	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++)
			if (fTempHist[i][j] == 0) fHist[i][j].clear();

	int tempAr[kMAX_CLUSTERS];
	for (i = 0; i < kMAX_CLUSTERS;i++) tempAr[i] = 0;

	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++)
			if (fHist[i][j].size() > 0){
				for(k = 0; k < fHist[i][j].size(); k++){
					tempAr[fHistClusterNumber[i][j]]++;
					if (tempAr[fHistClusterNumber[i][j]]%fNParts[fCurIteration]==0)
						fCluster[fHistClusterNumber[i][j]][0].push_back(fHist[i][j][k]);
					if (tempAr[fHistClusterNumber[i][j]]%fNParts[fCurIteration]==1)
						fCluster[fHistClusterNumber[i][j]][1].push_back(fHist[i][j][k]);
					if (tempAr[fHistClusterNumber[i][j]]%fNParts[fCurIteration]==2)
						fCluster[fHistClusterNumber[i][j]][2].push_back(fHist[i][j][k]);
				}
			}
	cout << "InitClusters end" << endl;
}


void CbmRichRingFinderHough::CalculateRingParameters(double x[], double y[], double *xc, double *yc, double *r)
{
	register double t1, t2, t3, t4, t5, t6, t8, t9,
		t10, t11, t14, t16, t19, t21, t41;

	t1 = x[1] * x[1];
	t2 = x[2] * x[2];
	t3 = y[1] * y[1];
	t4 = y[2] * y[2];
	t5 = t1 - t2 + t3 - t4;
	t6 = y[0] - y[1];
	t8 = x[0] * x[0];
	t9 = y[0] * y[0];
	t10 = t8 - t1 + t9 - t3;
	t11 = y[1] - y[2];
	t14 = x[1] - x[2];
	t16 = x[0] - x[1];
	t19 = 0.10e1 / (t14 * t6 - t16 * t11);

	*xc = 0.5e0 * (t5 * t6 - t10 * t11) * t19;
	*yc = 0.5e0 * (t10 * t14 - t5 * t16) * t19;

	t21 = pow(x[0] - *xc, 0.2e1);
	t41 = pow(y[0] - *yc, 0.2e1);
	*r = pow(t21 + t41, 0.5e0);


}

void CbmRichRingFinderHough::HoughTransform(int htcut, double nbinsx, double nbinsy)
{
	int i,j;
	unsigned int i1,i2,i3;
	double ix, iy, dx, dy;
	double x[3],y[3];
	double xc, yc,r;
	int check[kNBINSXHT][kNBINSYHT];
	cout << "HoughTransform begin" << endl;
	//calculate HT bin size
	dx = (kXMAX - kXMIN) / nbinsx;
	dy = (kYMAX - kYMIN) / nbinsy;
	cout << "init begin" << endl;
	//init arrays
	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++){
			fHTHist[i][j] = 0;
			fHTRadius[i][j] = 0;
			check[i][j] = 0;
		}
	cout << "init end" << endl;
	double minDistance2 = fMinDistance[fCurIteration]*fMinDistance[fCurIteration];
	double maxDistance2 = fMaxDistance[fCurIteration]*fMaxDistance[fCurIteration];
	for (i = 0; i < fNClusters; i++)
	for (int iPart = 0; iPart < fNParts[fCurIteration]; iPart++)
		for ( i1 = 0; i1 < fCluster[i][iPart].size(); i1++){
			CbmRichMyPoint mpnt;
			std::vector<CbmRichMyPoint>::iterator itmin;
			std::vector<CbmRichMyPoint>::iterator itmax;

			//find all hits which are in the corridor
			mpnt.fX = fData[fCluster[i][iPart][i1]].fX - fMaxDistance[fCurIteration];
			itmin = std::lower_bound(fData.begin(),
					fData.end(), mpnt, CbmRichMyPoint::CmpUp);

			mpnt.fX = fData[fCluster[i][iPart][i1]].fX + fMaxDistance[fCurIteration];
			itmax = std::lower_bound(fData.begin(),
					fData.end(), mpnt, CbmRichMyPoint::CmpUp)- 1 ;

			//first index of the hit in the corridor
			int indmin = itmin - fData.begin();
			int indmax = itmax - fData.begin();

			for (i2 = i1 + 1; i2 < fCluster[i][iPart].size(); i2++)
			if ((unsigned int)fCluster[i][iPart][i1] <=indmax && fCluster[i][iPart][i1] >= indmin &&
			    (unsigned int)fCluster[i][iPart][i2] <= indmax && fCluster[i][iPart][i2] >= indmin &&
			    fData[fCluster[i][iPart][i1]].fY - fMaxDistance[fCurIteration] <= fData[fCluster[i][iPart][i2]].fY &&
			    fData[fCluster[i][iPart][i1]].fY + fMaxDistance[fCurIteration] >= fData[fCluster[i][iPart][i2]].fY){
				for (i3 = i2 + 1; i3 < fCluster[i][iPart].size(); i3++)
					if((unsigned int)fCluster[i][iPart][i3] <=indmax && fCluster[i][iPart][i3] >= indmin&&
					fData[fCluster[i][iPart][i1]].fY - fMaxDistance[fCurIteration] <= fData[fCluster[i][iPart][i3]].fY &&
			    		fData[fCluster[i][iPart][i1]].fY + fMaxDistance[fCurIteration] >= fData[fCluster[i][iPart][i3]].fY){
						/*double r12 = (fData[fCluster[i][iPart][i1]].fX - fData[fCluster[i][iPart][i2]].fX)*
							     (fData[fCluster[i][iPart][i1]].fX - fData[fCluster[i][iPart][i2]].fX )+
							     (fData[fCluster[i][iPart][i1]].fY - fData[fCluster[i][iPart][i2]].fY)*
						             (fData[fCluster[i][iPart][i1]].fY - fData[fCluster[i][iPart][i2]].fY);

						double r13 = (fData[fCluster[i][iPart][i1]].fX - fData[fCluster[i][iPart][i3]].fX)*
						             (fData[fCluster[i][iPart][i1]].fX - fData[fCluster[i][iPart][i3]].fX)+
							     (fData[fCluster[i][iPart][i1]].fY - fData[fCluster[i][iPart][i3]].fY)*
							     (fData[fCluster[i][iPart][i1]].fY - fData[fCluster[i][iPart][i3]].fY);

						double r23 = (fData[fCluster[i][iPart][i2]].fX - fData[fCluster[i][iPart][i3]].fX)*
						             (fData[fCluster[i][iPart][i2]].fX - fData[fCluster[i][iPart][i3]].fX)+
							     (fData[fCluster[i][iPart][i2]].fY - fData[fCluster[i][iPart][i3]].fY)*
							     (fData[fCluster[i][iPart][i2]].fY - fData[fCluster[i][iPart][i3]].fY);

						if (r13 > minDistance2 && r13 < maxDistance2 &&
						    r23 > minDistance2 && r23 < maxDistance2 &&
						    r12 > minDistance2 && r12 < maxDistance2){*/
							x[0] = fData[fCluster[i][iPart][i1]].fX;
							x[1] = fData[fCluster[i][iPart][i2]].fX;
							x[2] = fData[fCluster[i][iPart][i3]].fX;
							y[0] = fData[fCluster[i][iPart][i1]].fY;
							y[1] = fData[fCluster[i][iPart][i2]].fY;
							y[2] = fData[fCluster[i][iPart][i3]].fY;

							CalculateRingParameters(x, y, &xc, &yc, &r);

							if (xc >0 && xc < (kXMAX - kXMIN) && yc > 0 && yc < (kYMAX - kYMIN) &&
							    r > fMinRadius[fCurIteration] && r < fMaxRadius[fCurIteration]) {
								modf(xc / dx, &ix);
								modf(yc / dy, &iy);
								if (ix < nbinsx && ix > 0 && iy < nbinsy && iy > 0 ){
									fHTHist[(int)ix][(int)iy]++;
									fHTRadius[(int)ix][(int)iy] += r;
									int xx =(int)ix;
									int yy =(int)iy;
									if (check[xx][yy] == 0){
										for (unsigned int kkk = 0; kkk < fData.size(); kkk++)
											fRingHits[xx][yy][kkk] = 0;
										check[xx][yy] = 1;
									}
									//what Hits
									fRingHits[(int)ix][(int)iy][fCluster[i][iPart][i1]]++;
									fRingHits[(int)ix][(int)iy][fCluster[i][iPart][i2]]++;
									fRingHits[(int)ix][(int)iy][fCluster[i][iPart][i3]]++;
								}
							}
						//}
					}
				//}
			}
		}


	//init HThist3
	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++)
			fHTHist3[i][j] = fHTHist[i][j];


	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++)
			if (fHTHist[i][j] < htcut) fHTHist[i][j] = 0;

	cout << "HoughTransform end" << endl;
}

void CbmRichRingFinderHough::FindHTClusters(double nbinsx, double nbinsy,int hitcut,int UpOrDown)
{
	int i,j;

        cout << "FindHTClusters begin" << endl;
	fNClusters = 0;

	//init ar1[][] and ar2[][]
	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++){
			fAr1[i][j] = fHTHist[i][j];
			fHTHistClusterNumber[i][j] = -1;
			fAr2[i][j] = fHTHistClusterNumber[i][j];
		}

	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++){
			if (fAr1[i][j] > 0 && fAr2[i][j] == -1){
				Wave(i,j, nbinsx, nbinsy);
				fNClusters++;
			}
		}

	for (int iCluster = 0;  iCluster < kMAX_CLUSTERS;  iCluster++)
		fHTCluster[iCluster].clear();

	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++){
			fHTHist[i][j] = fAr1[i][j];
			fHTHistClusterNumber[i][j] = fAr2[i][j];
			if (fHTHistClusterNumber[i][j] != -1 && fHTHistClusterNumber[i][j] < kMAX_CLUSTERS){
				CbmRichHTCluster tempHTCluster;
				tempHTCluster.fHist = fHTHist3[i][j];
				tempHTCluster.fX = i;
				tempHTCluster.fY = j;
				fHTCluster[ fHTHistClusterNumber[i][j] ].push_back(tempHTCluster);
			}
		}

	//sort array
	for (i= 0; i < fNClusters; i++) sort(fHTCluster[i].begin(), fHTCluster[i].end(), CmpSortHTCluster());

	double dx = (kXMAX - kXMIN) / nbinsx;
	double dy = (kYMAX - kYMIN) / nbinsy;

	for (i = 0; i < nbinsx; i++)
		for (j = 0; j < nbinsy; j++)
			fHTCheck[i][j] = 0;

	int kk,k;
	CbmRichHit* crh = new CbmRichHit;
	for (j = 0; j < fNClusters; j++){
		for (kk = fHTCluster[j].size() - 1; kk >= 0; kk--){

 			if ( fHTCluster[j][kk].fX  >= 0 && fHTCluster[j][kk].fX  < kNBINSXHT &&
			     fHTCluster[j][kk].fY >= 0 && fHTCluster[j][k].fY < kNBINSYHT &&
			     fHTCheck[ fHTCluster[j][kk].fX ][ fHTCluster[j][kk].fY ] == 0 ){

				k = kk;
			 	if (fHTCluster[j][k].fX - 1 > 0 && fHTCluster[j][k].fX + 1 < kNBINSXHT &&
				    fHTCluster[j][k].fY - 1 > 0 && fHTCluster[j][k].fY + 1 < kNBINSYHT)

				if (fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY ] <
				              	fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY ] &&
				    fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY ] <
				              	fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY ] &&
			   	    fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY - 1] <
				         	fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY ] &&
				    fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY + 1] <
				         	fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY ] &&
				    fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY - 1] <
				         	fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY ] &&
				    fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY + 1] <
				         	fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY ] &&
				    fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY - 1] <
				         	fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY ] &&
				    fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY + 1] <
				         	fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY ] ){

					int sum;
					double xcenter, ycenter;

					sum =     fHTHist3[ fHTCluster[j][k].fX - 1]   [ fHTCluster[j][k].fY ] +
						  fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY ] +
			   			  fHTHist3[ fHTCluster[j][k].fX ]   [ fHTCluster[j][k].fY - 1] +
						  fHTHist3[ fHTCluster[j][k].fX ]   [ fHTCluster[j][k].fY + 1] +
						  fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY - 1] +
						  fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY + 1] +
						  fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY - 1] +
						  fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY + 1] +
						  fHTHist3[ fHTCluster[j][k].fX]    [ fHTCluster[j][k].fY];


					xcenter = dx * ( (fHTCluster[j][k].fX - 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY ] +
							  (fHTCluster[j][k].fX + 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY ] +
							  (fHTCluster[j][k].fX  + 0.5) * fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY - 1] +
							  (fHTCluster[j][k].fX  + 0.5) * fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY + 1] +
							  (fHTCluster[j][k].fX - 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY - 1] +
							  (fHTCluster[j][k].fX - 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY + 1] +
							  (fHTCluster[j][k].fX + 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY - 1] +
							  (fHTCluster[j][k].fX + 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY + 1] +
							  (fHTCluster[j][k].fX + 0.5) * fHTHist3[ fHTCluster[j][k].fX][ fHTCluster[j][k].fY] ) / sum;

					ycenter = dy * ( (fHTCluster[j][k].fY - 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY ] +
							  (fHTCluster[j][k].fY + 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY ] +
							  (fHTCluster[j][k].fY + 0.5) * fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY - 1] +
							  (fHTCluster[j][k].fY  + 0.5) * fHTHist3[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY + 1] +
							  (fHTCluster[j][k].fY - 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY - 1] +
							  (fHTCluster[j][k].fY - 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY + 1] +
							  (fHTCluster[j][k].fY + 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY - 1] +
							  (fHTCluster[j][k].fY + 1 + 0.5) * fHTHist3[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY + 1] +
							  (fHTCluster[j][k].fY + 0.5) * fHTHist3[ fHTCluster[j][k].fX][ fHTCluster[j][k].fY] ) / sum;

					CbmRichRing tempRing;
					tempRing.SetCenterX(xcenter);
					tempRing.SetCenterY(ycenter);
					tempRing.SetRadius(fHTRadius[ fHTCluster[j][k].fX ][fHTCluster[j][k].fY] / fHTHist[fHTCluster[j][k].fX][fHTCluster[j][k].fY]);
					int hist[20];
					for(int oH = 0 ; oH <20 ; oH++) hist[oH] = 0;
					
					//double ir;
					int ii;
					for (unsigned int ih=0; ih < fData.size(); ih++){
						int summ = fRingHits[fHTCluster[j][k].fX][fHTCluster[j][k].fY][ih];/* +
						fRingHits[fHTCluster[j][k].fX - 1][fHTCluster[j][k].fY][ih] +
						fRingHits[fHTCluster[j][k].fX + 1][fHTCluster[j][k].fY][ih] +
						fRingHits[fHTCluster[j][k].fX ][fHTCluster[j][k].fY - 1][ih] +
						fRingHits[fHTCluster[j][k].fX ][fHTCluster[j][k].fY + 1][ih] +
						fRingHits[fHTCluster[j][k].fX - 1][fHTCluster[j][k].fY + 1][ih] +
						fRingHits[fHTCluster[j][k].fX - 1][fHTCluster[j][k].fY - 1][ih] +
						fRingHits[fHTCluster[j][k].fX + 1][fHTCluster[j][k].fY + 1][ih] +
						fRingHits[fHTCluster[j][k].fX + 1][fHTCluster[j][k].fY - 1][ih];*/

						if(UpOrDown == 0) kYMIN = 80; else kYMIN = -250;
						if (summ >=hitcut){
							CbmRichHit* crh = new CbmRichHit;
							crh->SetXYZ(fData[ih].fX + kXMIN, fData[ih].fY + kYMIN, fData[ih].fId);
							crh->SetRefIndex(fData[ih].fRefIndex);
							tempRing.AddHit(crh);

						}

						/*	double r = pow((fData[ih].fX - xcenter)*(fData[ih].fX - xcenter) +
						               		(fData[ih].fY - ycenter)*(fData[ih].fY - ycenter), 0.5);

							if (r > 3.5 && r < 6.5) {
								double ir ;
								modf(r / (6.5 / 20.0), &ir);
								if (ir < 20 && ir > 0 ) hist[ (int) ir ]++;
							}

						}*/

						/*if (fRingHits[fHTCluster[j][k].fX][fHTCluster[j][k].fY][ih] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);

						if (fRingHits[fHTCluster[j][k].fX - 1][fHTCluster[j][k].fY][ih] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);

						if (fRingHits[fHTCluster[j][k].fX + 1][fHTCluster[j][k].fY][ih] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);

						if (fRingHits[fHTCluster[j][k].fX ][fHTCluster[j][k].fY - 1][ih] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);

						if (fRingHits[fHTCluster[j][k].fX ][fHTCluster[j][k].fY + 1][ih] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);

						if (fRingHits[fHTCluster[j][k].fX - 1][fHTCluster[j][k].fY + 1][ih] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);

						if (fRingHits[fHTCluster[j][k].fX - 1][fHTCluster[j][k].fY - 1][ih] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);

						if (fRingHits[fHTCluster[j][k].fX + 1][fHTCluster[j][k].fY + 1][ih] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);

						if (fRingHits[fHTCluster[j][k].fX + 1][fHTCluster[j][k].fY - 1][ih] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);

						if (fRingHits[fHTCluster[j][k].fX - 1][fHTCluster[j][k].fY][ih + 1] >= hitcut)
							tempRing.AddHitIndex(fData[ih].fId);*/
					}

					/*int max = 0;
					for (i = 0; i < 20; i++)
						if (hist[i] > max) {
							max = hist[i];
							ii = i;
						}
					unsigned int maxsum = hist[ii];
					if (maxsum >=4){
						if (ii+1 < 20) maxsum += hist[ii+1];
						if (ii-1 > 0) maxsum += hist[ii-1];
					}
					if (maxsum >= 10)*/
						fFoundRings[UpOrDown][fCurIteration].push_back(tempRing);

					fHTCheck[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY ] = 1;
				    	fHTCheck[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY ] = 1;
			   	        fHTCheck[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY - 1] = 1;
					fHTCheck[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY + 1] = 1;
				        fHTCheck[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY - 1] = 1;
				        fHTCheck[ fHTCluster[j][k].fX - 1][ fHTCluster[j][k].fY + 1] = 1 ;
				        fHTCheck[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY - 1] = 1;
				        fHTCheck[ fHTCluster[j][k].fX + 1][ fHTCluster[j][k].fY + 1] = 1;
					fHTCheck[ fHTCluster[j][k].fX ][ fHTCluster[j][k].fY ] = 1;
				}
			}
		}
	}
	cout<<"FindHTClusters end"<<endl;
}

ClassImp(CbmRichRingFinderHough)