#include "CbmSttHoughAccumulatorNew.h"

TH2F const *CbmSttHoughAccumulatorNew::GetTH2F(Int_t cluster, Int_t thisZ) 
{
    TH2F
	*retval;

    if ((cluster == -1) || ((cluster >= 0) && (cluster < fNumberOfClusters)))
    {
	FillTH2F(cluster, thisZ);
	retval = &fHistogram;
    }
    else
    {
	retval = NULL;
    }

    return retval;
}

TH3F const *CbmSttHoughAccumulatorNew::GetTH3F(Int_t cluster) 
{
    TH3F
	*retval;

    if ((cluster == -1) || ((cluster >= 0) && (cluster < fNumberOfClusters)))
    {
	FillTH3F(cluster);
	retval = &fHistogram3d;

	retval->GetXaxis()->SetTitle("Distance (cm)");
	retval->GetYaxis()->SetTitle("Angle (rad)");
	retval->GetZaxis()->SetTitle("Radius (cm)");
    }
    else
    {
	retval = NULL;
    }


    return retval;
}

void CbmSttHoughAccumulatorNew::GetHighestPeakEntryList(vector<Int_t> &resultVect)
{
    CbmSttHoughCellNew *
	tmp = GetHighestPeak();
    
    if (tmp != NULL)
    {
	for (Int_t teller = 0; teller < tmp->Size(); teller++)
	{
	    Int_t
		element = tmp->GetHit(teller);

	    resultVect.push_back(element);
	}
    }
}

void CbmSttHoughAccumulatorNew::FillTH2F(Int_t cluster, Int_t thisZ) 
{
    fHistogram.Reset();
    fHistogram.SetName("hough accumulator");
    fHistogram.SetTitle("hough accumulator");
    fHistogram.SetBins(fXBins, fXLow, fXHigh, fYBins, fYLow, fYHigh);
    fHistogram.SetFillColor((cluster % 7) + 1);
    fHistogram.SetMarkerColor((cluster % 7) + 1);
    fHistogram.SetLineColor((cluster % 7) + 1);

    if (cluster == fNumberOfClusters - 1)
	fHistogram.SetLineColor(14);
	
    //cout << "setting color: " << cluster % 10 << endl;

    fHistogram.SetMaximum(GetHighestPeakContents());
    fHistogram.SetMinimum(5);
    fHistogram.GetXaxis()->SetRangeUser(-1.5, 0.5);
    fHistogram.GetYaxis()->SetRangeUser(1.50, 1.60);

    if (fAccumulator[thisZ] != NULL)
    {
	for (Int_t thisY = 0; thisY < GetYBins(); thisY++)
	{
	    if (fAccumulator[thisZ][thisY] != NULL)
	    {
		for (Int_t thisX = 0; thisX < GetXBins(); thisX++)
		{
		    //cout << "here1" << endl;
		    if (fAccumulator[thisZ][thisY][thisX] != NULL)
		    {
			//cout << "here2" << endl;
			Double_t
			    xval = fXLow + thisX * ((fXHigh - fXLow) / fXBins),
			    yval = fYLow + thisY * ((fYHigh - fYLow) / fYBins);
			
			//cout << "this hit belons to cluster: " << fAccumulator[thisZ][thisY][thisX]->GetClusterNumber() << endl;
			if ((cluster == -1) || fAccumulator[thisZ][thisY][thisX]->GetClusterNumber() == cluster)
			{
			    //cout << "here4" << endl;
			    for (Int_t teller = 0; teller < fAccumulator[thisZ][thisY][thisX]->Size(); teller++)
			    {
				//cout << "here5" << endl;
				fHistogram.Fill(xval, yval);
			    }
			}
		    }
		}
	    }
	}
    }
    //cout << "done" << endl;
}

void CbmSttHoughAccumulatorNew::FillTH3F(Int_t cluster) 
{
    fHistogram3d.Reset();
    fHistogram3d.SetName("hough accumulator");
    fHistogram3d.SetTitle("hough accumulator");
    fHistogram3d.SetBins(fXBins, fXLow, fXHigh, fYBins, fYLow, fYHigh, fZBins, fZLow, fZHigh);
    fHistogram3d.SetFillColor((cluster % 7) + 1);
    fHistogram3d.SetLineColor((cluster % 7) + 1);

//    if (cluster == fNumberOfClusters - 1)
//	fHistogram.SetLineColor(14);
	
//    fHistogram3d.SetMaximum(GetHighestPeakContents());
//    fHistogram3d.SetMinimum(5);
//    fHistogram3d.GetXaxis()->SetRangeUser(-1.5, 0.5);
//    fHistogram3d.GetYaxis()->SetRangeUser(1.50, 1.60);

    if (fAccumulator != 0)
    {
	for (Int_t thisZ = 0; thisZ < GetZBins(); thisZ++)
	{
	    if (fAccumulator[thisZ] != NULL)
	    {
		for (Int_t thisY = 0; thisY < GetYBins(); thisY++)
		{
		    if (fAccumulator[thisZ][thisY] != NULL)
		    {
			for (Int_t thisX = 0; thisX < GetXBins(); thisX++)
			{
			    //cout << "here1" << endl;
			    if (fAccumulator[thisZ][thisY][thisX] != NULL)
			    {
				//cout << "here2" << endl;
				Double_t
				    xval = fXLow + thisX * ((fXHigh - fXLow) / fXBins),
				    yval = fYLow + thisY * ((fYHigh - fYLow) / fYBins),
				    zval = fZLow + thisZ * ((fZHigh - fZLow) / fZBins);
				    
				
				//cout << "this hit belons to cluster: " << fAccumulator[thisZ][thisY][thisX]->GetClusterNumber() << endl;
				if ((cluster == -1) || fAccumulator[thisZ][thisY][thisX]->GetClusterNumber() == cluster)
				{
				    //cout << "here4" << endl;
				    for (Int_t teller = 0; teller < fAccumulator[thisZ][thisY][thisX]->Size(); teller++)
				    {
					//cout << "here5" << endl;
					fHistogram3d.Fill(xval, yval, zval);
				    }
				}
			    }
			}
		    }
		}
	    }
	    //cout << "done" << endl;
	}
    }
}


Int_t CbmSttHoughAccumulatorNew::GetHighestPeakCluster() const
{
    Int_t retval;

    CbmSttHoughCellNew *
	tmp = GetHighestPeak();

    if (tmp == NULL)
    {
	retval = -1;
    }
    else
    {
	retval = tmp->GetClusterNumber();
    }

    return retval;
}

Int_t CbmSttHoughAccumulatorNew::GetHighestPeakContents() const
{
    Int_t retval;

    CbmSttHoughCellNew *
	tmp = GetHighestPeak();

    if (tmp == NULL)
    {
	retval = -1;
    }
    else
    {
	retval = tmp->Size();
    }

    return retval;
}

void CbmSttHoughAccumulatorNew::ClearAccumulator()
{
    if (fAccumulator != NULL)
    {
	for (Int_t zteller = 0; zteller < fZBins; zteller++)
	{
	    if (fAccumulator[zteller] != NULL)
	    {
		for (Int_t yteller = 0; yteller < fYBins; yteller++)
		{
		    if (fAccumulator[zteller][yteller] != NULL)
		    {
			for (Int_t xteller = 0; xteller < fXBins; xteller++)
			{
			    if (fAccumulator[zteller][yteller][xteller] != NULL)
			    {
				//cout << "deleting cell" << endl;
				delete fAccumulator[zteller][yteller][xteller];
				fAccumulator[zteller][yteller][xteller] = NULL;
			    }
			}
			//cout << "deleting row1" << endl;
			delete [] fAccumulator[zteller][yteller];
			fAccumulator[zteller][yteller] = NULL;
		    }
		}
		//cout << "deleting row2" << endl;
		delete [] fAccumulator[zteller];
		fAccumulator[zteller] = NULL;
	    }
	}
    }

    fNumberOfClusters = 0;
    maxCellPointer = NULL;
}

CbmSttHoughAccumulatorNew::CbmSttHoughAccumulatorNew(Int_t threshold, Bool_t verbose, 
						     Int_t xbins, Float_t xlow, Float_t xhigh,
						     Int_t ybins, Float_t ylow, Float_t yhigh, 
						     Int_t zbins, Float_t zlow, Float_t zhigh)
{
    fWrappingX = kFALSE;
    fWrappingY = kFALSE;
    fWrappingZ = kFALSE;
    
    fVerbose = verbose;
    fThreshold = threshold;
    
    fAccumulator = new CbmSttHoughCellNew ***[zbins];
    for (Int_t teller = 0; teller < zbins; teller++)
    {
	fAccumulator[teller] = NULL;
    }

    fXBins = xbins;
    fYBins = ybins;
    fZBins = zbins;
    
    fXLow = xlow;
    fXHigh = xhigh;
    fYLow = ylow;
    fYHigh = yhigh;
    fZLow = zlow;
    fZHigh = zhigh;
    maxCellPointer = NULL;
    fNumberOfClusters = 0;
}

void CbmSttHoughAccumulatorNew::operator=(CbmSttHoughAccumulatorNew const &other)
{
    Destroy();
    Copy(other);
}

CbmSttHoughAccumulatorNew::CbmSttHoughAccumulatorNew(CbmSttHoughAccumulatorNew const &other)
{
    if (this != &other)
    {
	Destroy();
	Copy(other);
    }
}

CbmSttHoughAccumulatorNew::CbmSttHoughAccumulatorNew()
{
    fWrappingX = kFALSE;
    fWrappingY = kFALSE;
    fWrappingZ = kFALSE;
    
    fVerbose = kFALSE;
    fThreshold = -1;
    
    fXBins = -1;
    fYBins = -1;
    fZBins = -1;
    
    fXLow = -1.;
    fXHigh = -1.;
    fYLow = -1.;
    fYHigh = -1.;
    fZLow = -1.;
    fZHigh = -1.;
    fNumberOfClusters = 0;

    maxCellPointer = NULL;
}

CbmSttHoughAccumulatorNew::~CbmSttHoughAccumulatorNew()
{
    Destroy();
}

void CbmSttHoughAccumulatorNew::Destroy()
{
    ClearAccumulator();

    if (fAccumulator != NULL)
    {
	delete [] fAccumulator;
	fAccumulator = NULL;
    }
}

void CbmSttHoughAccumulatorNew::Copy(CbmSttHoughAccumulatorNew const &other)
{
    for (Int_t zteller = 0; zteller < other.GetZBins(); zteller++)
    {
	if (other.fAccumulator[zteller] != NULL)
	{
	    for (Int_t yteller = 0; yteller < other.GetYBins(); yteller++)
	    {
		if (other.fAccumulator[zteller][yteller] != NULL)
		{
		    for (Int_t xteller = 0; xteller < other.GetXBins(); xteller++)
		    {
			if (other.fAccumulator[zteller][yteller][xteller] != NULL)
			{
			    fAccumulator[zteller][yteller][xteller] = other.fAccumulator[zteller][yteller][xteller];
			}
		    }	    
		}
	    }
	}
    }
    
    fWrappingX = other.IsWrappingX();
    fWrappingY = other.IsWrappingY();
    fWrappingZ = other.IsWrappingZ();
    
    fVerbose = other.IsVerbose();
    fThreshold = other.GetThreshold();
    
    fXBins = other.GetXBins();
    fYBins = other.GetYBins();
    fZBins = other.GetZBins();
    
    fXLow = other.GetXLow();
    fXHigh = other.GetXHigh();
    
    fYLow = other.GetYLow();
    fYHigh = other.GetYHigh();
    
    fZLow = other.GetZLow();
    fZHigh = other.GetZHigh();

    fNumberOfClusters = other.fNumberOfClusters;

    Int_t
	xmax = other.GetHighestPeakXBin(),
	ymax = other.GetHighestPeakYBin(),
	zmax = other.GetHighestPeakZBin();

    maxCellPointer = fAccumulator[zmax][ymax][xmax];
}

void CbmSttHoughAccumulatorNew::Cluster(Bool_t merge)
{
    vector<CbmSttHoughCellNew *>
	localMaxVector;
    
    for (Int_t thisZ = 0; thisZ < GetZBins(); thisZ++)
    {
	if (fAccumulator[thisZ] != NULL)
	{
	    for (Int_t thisY = 0; thisY < GetYBins(); thisY++)
	    {
		if (fAccumulator[thisZ][thisY] != NULL)
		{
		    for (Int_t thisX = 0; thisX < GetXBins(); thisX++)
		    {
			if (fAccumulator[thisZ][thisY][thisX] != NULL)
			{
			    Int_t
				maximum = 0,
				maximumX = 0,
				maximumY = 0,
				maximumZ = 0;
	
			    // loop over neighbours
			    for (Int_t neighbourZ = thisZ - 1; neighbourZ < thisZ + 2; neighbourZ++)
			    {
				for (Int_t neighbourY = thisY - 1; neighbourY < thisY + 2; neighbourY++)
				{
				    for (Int_t neighbourX = thisX - 1; neighbourX < thisX + 2; neighbourX++)
				    {
					if (!((neighbourX == thisX) && (neighbourY == thisY) && (neighbourZ == thisZ)))
					{
					    if ((neighbourZ >= 0) && (neighbourZ < GetZBins()) && 
						(neighbourY >= 0) && (neighbourY < GetYBins()) && 
						(neighbourX >= 0) && (neighbourX < GetXBins())) 
					    {
						if (fAccumulator[neighbourZ] != NULL)	
						{
						    if (fAccumulator[neighbourZ][neighbourY] != NULL)	
						    {
							if (fAccumulator[neighbourZ][neighbourY][neighbourX]!= NULL)
							{
							    if (fAccumulator[neighbourZ][neighbourY][neighbourX]->Size() > maximum)
							    {
								maximum = fAccumulator[neighbourZ][neighbourY][neighbourX]->Size();
								maximumX = neighbourX;
								maximumY = neighbourY;
								maximumZ = neighbourZ;
							    }
							}
						    }
						}
					    }
					}
				    }
				}
			    }

			    if (maximum > fAccumulator[thisZ][thisY][thisX]->Size())
			    {
				// inform stronger cell that this cell belongs to him
				fAccumulator[maximumZ][maximumY][maximumX]->AddClusterLink(fAccumulator[thisZ][thisY][thisX]);
			    }
			    else
			    {
				localMaxVector.push_back(fAccumulator[thisZ][thisY][thisX]);
			    }
			}
		    }
		}
	    }
	}
    }

    Int_t
	myMaximum = 0;

    // loop over local maxima
    for (Int_t localMaxCounter = 0; localMaxCounter < localMaxVector.size(); localMaxCounter++)
    {
	localMaxVector[localMaxCounter]->MergeClusterLinks(merge, localMaxCounter);
	if (localMaxVector[localMaxCounter]->Size() > myMaximum)
	{
	    maxCellPointer = localMaxVector[localMaxCounter];
	    myMaximum = localMaxVector[localMaxCounter]->Size();
	}
    }


    fNumberOfClusters = localMaxVector.size();
}


Double_t CbmSttHoughAccumulatorNew::GetHighestPeakX() const
{
    Int_t
	xBin = GetHighestPeakXBin();
    
    return fXLow + xBin * ((fXHigh - fXLow) / fXBins);
}

Int_t CbmSttHoughAccumulatorNew::GetHighestPeakXBin() const
{
    Int_t retval;

    CbmSttHoughCellNew *
	tmp = GetHighestPeak();

    if (tmp == NULL)
    {
	retval = -1;
    }
    else
    {
	retval = GetHighestPeak()->GetX();
    }

    return retval;
}

Double_t CbmSttHoughAccumulatorNew::GetHighestPeakY() const
{
    Int_t
	yBin = GetHighestPeakYBin();
    
    return fYLow + yBin * ((fYHigh - fYLow) / fYBins);
}

Int_t CbmSttHoughAccumulatorNew::GetHighestPeakYBin() const
{
    Int_t retval;

    CbmSttHoughCellNew *
	tmp = GetHighestPeak();

    if (tmp == NULL)
    {
	retval = -1;
    }
    else
    {
	retval = GetHighestPeak()->GetY();
    }

    return retval;
}

Double_t CbmSttHoughAccumulatorNew::GetHighestPeakZ() const
{
    Int_t
	zBin = GetHighestPeakZBin();
    
    return fZLow + zBin * ((fZHigh - fZLow) / fZBins);
}

Int_t CbmSttHoughAccumulatorNew::GetHighestPeakZBin() const
{
    Int_t retval;

    CbmSttHoughCellNew *
	tmp = GetHighestPeak();

    if (tmp == NULL)
    {
	retval = -1;
    }
    else
    {
	retval = GetHighestPeak()->GetZ();
    }

    return retval;
}


CbmSttHoughCellNew *CbmSttHoughAccumulatorNew::GetHighestPeak() const
{
    CbmSttHoughCellNew 
	*retval = NULL;

    if (maxCellPointer != NULL)
	if (maxCellPointer->Size() > fThreshold)
	    retval = maxCellPointer;

    return retval;
}

void CbmSttHoughAccumulatorNew::RemoveHit(int elementNumber, Bool_t onlyThisCell, Int_t xbin, Int_t ybin, Int_t zbin)
{
    Int_t
	maximum = 0;
    
    if (fAccumulator != NULL)
    {
	for (Int_t zteller = 0; zteller < fZBins; zteller++)
	{
	    if (fAccumulator[zteller] != NULL)
	    {
		for (Int_t yteller = 0; yteller < fYBins; yteller++)
		{
		    if (fAccumulator[zteller][yteller] != NULL)
		    {
			for (Int_t xteller = 0; xteller < fXBins; xteller++)
			{
			    if (fAccumulator[zteller][yteller][xteller] != NULL)
			    {
				if (((zteller == zbin) && (yteller == ybin) && (xteller == xbin)) || (onlyThisCell == kFALSE))
				{
				    fAccumulator[zteller][yteller][xteller]->RemoveHit(elementNumber);
				}

				if (fAccumulator[zteller][yteller][xteller]->Size() > maximum)
				{
				    maximum = fAccumulator[zteller][yteller][xteller]->Size();
				    maxCellPointer = fAccumulator[zteller][yteller][xteller];
				}
			    }
			}
		    }
		}
	    }
	}
    }
}

Int_t CbmSttHoughAccumulatorNew::GetContents(Int_t xbin, Int_t ybin, Int_t zbin)
{
    Int_t
	retval = -1;

    if (fAccumulator != NULL)
    {
	if (fAccumulator[zbin] != NULL)
	{
	    if (fAccumulator[zbin][ybin] != NULL)
	    {
		if (fAccumulator[zbin][ybin][xbin] != NULL)
		{
		    retval = fAccumulator[zbin][ybin][xbin]->Size();
		}
	    }
	}
    }

    return retval;
}

Int_t CbmSttHoughAccumulatorNew::ConvertX(Double_t valX)
{
    return (Int_t (((valX - fXLow) / (fXHigh - fXLow) * fXBins) + 0.5));
}

Int_t CbmSttHoughAccumulatorNew::ConvertY(Double_t valY)
{
    return (Int_t (((valY - fYLow) / (fYHigh - fYLow) * fYBins) + 0.5));
}

Int_t CbmSttHoughAccumulatorNew::ConvertZ(Double_t valZ)
{
    return (Int_t (((valZ - fZLow) / (fZHigh - fZLow) * fZBins) + 0.5));
}
void CbmSttHoughAccumulatorNew::AddHit(Double_t valX, Int_t iFirst, 
				       Double_t valY, Int_t iSecond) 
{
    Int_t
	binX = ConvertX(valX),
	binY = ConvertY(valY),
	binZ = 0;

    AddHitInternal(binX, binY, binZ, iFirst, iSecond, -1);
}

void CbmSttHoughAccumulatorNew::AddHit(Double_t valX, Int_t iFirst, 
				       Double_t valY, Int_t iSecond, 
				       Double_t valZ, Int_t iThird) 
{
    Int_t
	binX = ConvertX(valX),
	binY = ConvertY(valY),
	binZ = ConvertZ(valZ);

    AddHitInternal(binX, binY, binZ, iFirst, iSecond, iThird);
}

void CbmSttHoughAccumulatorNew::AddHitInternal(Int_t binX, Int_t binY, Int_t binZ, Int_t iFirst, Int_t iSecond, Int_t iThird)
{
    if ((binZ >= 0) && (binZ < fZBins) && (binY >= 0) && (binY < fYBins) && (binX >= 0) && (binX < fXBins))
    {
	//cout << "creating structures" << endl;
	//cout << "binZ: " << binZ << endl;
	
	//cout << "start: " << fAccumulator[binZ] << endl;
	if (fAccumulator[binZ] == NULL)
	{
	    //cout << "creating column" << endl;
	    fAccumulator[binZ] = new CbmSttHoughCellNew ** [fYBins];
	    
	    for (Int_t teller = 0; teller < fYBins; teller++)
	    {
		fAccumulator[binZ][teller] = NULL; 
	    }
	}
	//cout << "start2: " << fAccumulator[binZ][binY] << endl;
	if (fAccumulator[binZ][binY] == NULL)
	{
	    //cout << "creating row" << endl;
	    fAccumulator[binZ][binY] = new CbmSttHoughCellNew * [fXBins];
	    
	    for (Int_t teller = 0; teller < fXBins; teller++)
	    {
		fAccumulator[binZ][binY][teller] = NULL; 
	    }
	}
	if (fAccumulator[binZ][binY][binX] == NULL)
	{
	    //cout << "creating new cell" << endl;
	    CbmSttHoughCellNew
		tmp(binX, binY, binZ);
	    
	    fAccumulator[binZ][binY][binX] = new CbmSttHoughCellNew(binX, binY, binZ);
	    //cout << "checking: " << fAccumulator[binZ][binY][binX]->GetX() << endl;
	}
	
	//cout << "adding hit " << binZ << " " << binY << " " << binX << endl;
	fAccumulator[binZ][binY][binX]->AddHit(iFirst, iSecond, iThird);
	
	//cout << "adapting maximum" << endl;
	if (maxCellPointer != NULL)
	{
	    //cout << "bad choice" << endl;
	    if (fAccumulator[binZ][binY][binX]->Size() > maxCellPointer->Size())
	    {
		//cout << "passed test" << endl;
		maxCellPointer = fAccumulator[binZ][binY][binX];
	    }
	}
	else
	{
	    //cout << "should end here" << endl;
	    maxCellPointer = fAccumulator[binZ][binY][binX];
	}
	
	//cout << "done, maximum at " << maxCellPointer->GetX() << " " << maxCellPointer->GetY() << " " << maxCellPointer->GetZ() << endl;
    }
}
ClassImp(CbmSttHoughAccumulatorNew)