#include "PndMvdChargeWeightedPixelMapping.h"
#include "TGeoManager.h"
#include "TGeoBBox.h"
#include "TClonesArray.h"
#include "TMath.h"

PndMvdHit PndMvdChargeWeightedPixelMapping::GetCluster()
{
	Double_t col = 0, row = 0, charge = 0;
	Double_t tempCol = 0, tempRow = 0;
	Int_t count = 0;
	
	if (fDigiArray.size() == 1){
		if (fDigiArray[0].GetCharge() > 0){
			col = fDigiArray[0].GetPixelColumn() + ((Int_t)(fDigiArray[0].GetFE()%10)) * fParams[0];	
			if (col < 0){
				col -= 0.5;
				col += fParams[1] / 2;
			}
			else col += 0.5;
			row = fDigiArray[0].GetPixelRow()+ (fDigiArray[0].GetFE()/10) * fParams[1];
			if (row < 0){
				row -= 0.5;
				row += fParams[2] /2;
			}
			else row += 0.5;
			if (fVerbose > 1)
				std::cout << "GetCluster:col/row " << col << " " << row << std::endl;
			count = 1;
			charge = fDigiArray[0].GetCharge();
		}
	}
	else {						
		//cout << "Multiple Hits!" << std::endl;
		for (uint i = 0; i < fDigiArray.size(); i++){
			//cout << "ActCol / Row" << col << " " << row << " added Col/Row " << fDigiArray[i].GetPixelColumn() << " " << fDigiArray[i].GetPixelRow() << endl;
			if (fDigiArray[i].GetCharge() > 0){
				tempCol = fDigiArray[i].GetPixelColumn() + (Int_t)(fDigiArray[0].GetFE()%10) * fParams[0];
				tempRow = fDigiArray[i].GetPixelRow() + fDigiArray[0].GetFE()/10 * fParams[1];
				col += (tempCol*fDigiArray[i].GetCharge());
				row += (tempRow*fDigiArray[i].GetCharge());
				charge += fDigiArray[i].GetCharge();
				count++;
			}
		}
		if (count > 0){
			if (charge > 0){
				col /= charge;
				row /= charge;
			}
			col += 0.5;
			row += 0.5;
		}
		else col = row = 0;
	}
	if (fVerbose > 1){
		std::cout << "Col: " << col << " Row: " << row << std::endl;
	}
	Double_t resultD[3];
	resultD[0] = col * fParams[2];
	resultD[1] = row * fParams[3];
	resultD[2] = 0;
	
	Double_t resultFinal[3];
	TVector3 offset = GetSensorDimensions(fDigiArray[0].GetDetName().Data());
	resultD[0] -= offset.x();
	resultD[1] -= offset.y();
	//resultD[2] -= offset.z();
	if (fVerbose > 1)
		std::cout << "Local Position: " << resultD[0] << " " << resultD[1] << std::endl;
	TGeoHMatrix trans = GetTransformation(fDigiArray[0].GetDetName().Data());
	//cout << "Transformation for: " << fDigiArray[0].GetDetName() << std::endl;
	//trans.Print("");
	trans.LocalToMaster(resultD, resultFinal);
	//result.setXYZ(resultFinal[0], resultFinal[1], resultFinal[2]);
	TVector3 dpos(fParams[2]/TMath::Sqrt(12.0),fParams[3]/sqrt(12.0),0);
	TVector3 pos(resultFinal[0], resultFinal[1], resultFinal[2]);
	
	return (PndMvdHit(0,fDigiArray[0].GetDetName().Data(), pos, dpos, fDigiArray[0].GetIndex(), charge, fDigiArray.size()));
}

TGeoHMatrix PndMvdChargeWeightedPixelMapping::GetTransformation(std::string detName)
{
  gGeoManager->cd(fGeoH->GetPath(detName.c_str()));
  TGeoHMatrix* transMat = gGeoManager->GetCurrentMatrix();
  if (fVerbose > 2)
  	transMat->Print("");
  return *transMat;
}

TVector3 PndMvdChargeWeightedPixelMapping::GetSensorDimensions(std::string detName)
{
	gGeoManager->cd(fGeoH->GetPath(detName.c_str()));
	TGeoVolume* actVolume = gGeoManager->GetCurrentVolume();
	TGeoBBox* actBox = (TGeoBBox*)(actVolume->GetShape());
	TVector3 result;
	result.SetX(actBox->GetDX());
	result.SetY(actBox->GetDY());
	result.SetZ(actBox->GetDZ());
	
	//result.Dump();
	
	return result;
}