#include <iostream>
#include <cmath>
#include <cassert>
#include <algorithm>
#include <functional>

#include "TRandom2.h"

#include "MapsSensor.hh"
#include "ToString.h"

//A "functional" object, used by mutualSensorMask
struct IlEstMort :
	public std::binary_function<const MapsSensor*, const MapsSensor::physXY, bool > {
	bool operator()(const MapsSensor* s, const MapsSensor::physXY p) const {
		return s->isDeadArea(p);
	}
};

MapsSensor::~MapsSensor() {
}

MapsSensor::physXY MapsSensor::getAlignment() const {
	return myAlignment;
}

void MapsSensor::setAlignment(MapsSensor::physXY alignment) {
	myAlignment = alignment;
}

void MapsSensor::registerTrackConfirm(unsigned threshold,
		MapsSensor::coord place, unsigned bx) {
	if (myHitsByThreshold.count(threshold) == 0) {
		myKnownThresholds.push_back(threshold);
		std::pair<unsigned, unsigned>* hits =
				new std::pair<unsigned, unsigned>(0, 0);
		std::pair<unsigned, unsigned>* samplerHits =
				new std::pair<unsigned, unsigned>(0, 0);
		std::pair<unsigned, unsigned>* shaperHits =
				new std::pair<unsigned, unsigned>(0, 0);

		myHitsByThreshold[threshold] = hits;
		mySamplerHitsByThreshold[threshold] = samplerHits;
		myShaperHitsByThreshold[threshold] = shaperHits;
	}

	if (myTimes.count(bx) == 0) {
		myTimes[bx] = new std::pair<unsigned, unsigned>(0, 0);
	}

	std::pair<unsigned, unsigned>* hits = myHitsByThreshold[threshold];
	std::pair<unsigned, unsigned>* times = myTimes[bx];
	//std::cout << (*hits).first << ", " << (*hits).second << "\n";
	(*hits).first++;
	(*times).first++;

	if (place.first < 84) {
		//it's a shaper hit
		std::pair<unsigned, unsigned>* shaperHits =
				myShaperHitsByThreshold[threshold];
		(*shaperHits).first++;
	} else {
		//it's a sampler hit
		std::pair<unsigned, unsigned>* samplerHits =
				mySamplerHitsByThreshold[threshold];
		(*samplerHits).first++;
	}
	myEfficientHits.push_back(place);
}

void MapsSensor::registerTrackConfirm(unsigned threshold,
		MapsSensor::coord place, unsigned bx, MapsSensor::physXY resid) {
	myFourthHitResids.push_back(resid);
	registerTrackConfirm(threshold, place, bx);
}
void MapsSensor::registerTrackMiss(unsigned threshold, unsigned bx) {
	if (myHitsByThreshold.count(threshold) == 0) {
		myKnownThresholds.push_back(threshold);
		std::pair<unsigned, unsigned>* hits =
				new std::pair<unsigned, unsigned>(0, 0);
		myHitsByThreshold[threshold] = hits;

		std::pair<unsigned, unsigned>* shaperHits =
				new std::pair<unsigned, unsigned>(0, 0);
		myShaperHitsByThreshold[threshold] = shaperHits;

		std::pair<unsigned, unsigned>* samplerHits =
				new std::pair<unsigned, unsigned>(0, 0);
		mySamplerHitsByThreshold[threshold] = samplerHits;
	}
	if (myTimes.count(bx) == 0) {
		myTimes[bx] = new std::pair<unsigned, unsigned>(0, 0);
	}
	std::pair<unsigned, unsigned>* hits = myHitsByThreshold[threshold];
	std::pair<unsigned, unsigned>* times = myTimes[bx];
	//std::cout << (*hits).first << ", " << (*hits).second << "\n";
	(*hits).second++;
	(*times).second++;
}

void MapsSensor::registerTrackMiss(unsigned threshold, MapsSensor::coord pred,
		unsigned bx) {
	myInefficientHits.push_back(pred);
	registerTrackMiss(threshold, bx);
	if (pred.first < 84) {
		//it's a shaper miss
		std::pair<unsigned, unsigned>* shaperHits =
				myShaperHitsByThreshold[threshold];
		(*shaperHits).second++;
	} else {
		//it's a sampler miss
		std::pair<unsigned, unsigned>* samplerHits =
				mySamplerHitsByThreshold[threshold];
		(*samplerHits).second++;
	}
	physXY predPlace;
	convertHitToPhysical(pred, predPlace);
	malign(predPlace);
	rotateGlobalToLocal(predPlace);
	myInefficientPlaces.push_back(predPlace);
}

void MapsSensor::registerTrackMiss(unsigned threshold, MapsSensor::physXY pred,
		unsigned bx) {
	myInefficientPlaces.push_back(pred);
	registerTrackMiss(threshold, bx);
	malign(pred);
	rotateGlobalToLocal(pred);
	coord predHit;
	if (!isDeadArea(pred)) {
		convertPhysicalToHit(pred, predHit);
		myInefficientHits.push_back(predHit);
		if (predHit.first < 84) {
			//it's a shaper miss
			std::pair<unsigned, unsigned>* shaperHits =
					myShaperHitsByThreshold[threshold];
			(*shaperHits).second++;
		} else {
			//it's a sampler miss
			std::pair<unsigned, unsigned>* samplerHits =
					mySamplerHitsByThreshold[threshold];
			(*samplerHits).second++;
		}
	}
}

void MapsSensor::registerGeneralHit(unsigned threshold,
		MapsSensor::coord place, unsigned bx) {
	myGeneralHits.push_back(place);
	std::cout<< "WARNING: " << __PRETTY_FUNCTION__
			<< " not yet fully implemented!!!\n";

}
double MapsSensor::getEfficiency(unsigned threshold, bool withShapers,
		bool withSamplers) {
	if (myHitsByThreshold.count(threshold) == 0)
		return -1.0;
	std::pair<unsigned, unsigned>* hits;

	if (withShapers && withSamplers)
		hits = myHitsByThreshold[threshold];
	else if (withShapers && !withSamplers)
		hits = myShaperHitsByThreshold[threshold];
	else if (withSamplers && !withShapers)
		hits = mySamplerHitsByThreshold[threshold];
	else {
		MapsException* me = new MapsException("Efficiency calculation called for no active area!");
		throw me;
	}
	return static_cast<double>((*hits).first) / ((*hits).first + (*hits).second);
}

void MapsSensor::getInefficiencyXYPlot(TH2F& plot, bool physicalXY) {
	if (!physicalXY) {
		plot.SetTitle(("XY inefficiency for sensor " + toString(myId)).c_str());
		plot.SetName(("hXYinefficiencySensor" + toString(myId)).c_str());
		plot.SetBins(168, 0, 168, 168, 0, 168);
		plot.SetXTitle("x pixel");
		plot.SetYTitle("y pixel");

		for (std::vector<MapsSensor::coord>::const_iterator it =
				myInefficientHits.begin(); it != myInefficientHits.end(); it++) {
			plot.Fill((*it).first, (*it).second);
		}
	} else {
		plot.SetTitle(("XY inefficiency for sensor " + toString(myId)).c_str());
		plot.SetName(("hXYinefficiencySensor" + toString(myId)).c_str());
		plot.SetBins(1000, -5.0, 5.0, 1000, -5.0, 5.0);
		plot.SetXTitle("x (mm)");
		plot.SetYTitle("y (mm)");

		for (std::vector<MapsSensor::physXY>::const_iterator it =
				myInefficientPlaces.begin(); it != myInefficientPlaces.end(); it++) {
			plot.Fill((*it).first, (*it).second);
		}
	}
}
void MapsSensor::getEfficiencyXYPlot(TH2F& plot) {
	plot.SetTitle(("XY efficiency for sensor " + toString(myId)).c_str());
	plot.SetName(("hXYefficiencySensor" + toString(myId)).c_str());
	plot.SetBins(168, 0, 168, 168, 0, 168);
	plot.SetXTitle("x pixel");
	plot.SetYTitle("y pixel");

	for (std::vector<MapsSensor::coord>::const_iterator it =
			myEfficientHits.begin(); it != myEfficientHits.end(); it++) {

		plot.Fill((*it).first, (*it).second);
	}
}
void MapsSensor::getFourthHitResidualPlot(TH2F& plot) {
	plot.SetTitle(("R_{4} for 3 hit track w/alignment, sensor " + toString(myId)).c_str());
	plot.SetName(("hXYfourthHitResidSensor" + toString(myId)).c_str());
	plot.SetBins(100, -1.0, 1.0, 100, -1.0, 1.0);
	plot.SetXTitle("x pixel residual");
	plot.SetYTitle("y pixel residual");

	for (std::vector<MapsSensor::physXY>::const_iterator it =
			myFourthHitResids.begin(); it != myFourthHitResids.end(); it++) {
		//plot.Fill((*it).first - myAlignment.first, (*it).second - myAlignment.second);
		//if(myId == 2)
		//	std::cout << toString(*it) << ", \t" << toString(*it - myAlignment) << "\n";
		plot.Fill((*it).first, (*it).second);
	}

}
void MapsSensor::getEfficiencyTimestamps(TH1F& plot) {
	plot.SetName(("hTimestampsEfficiencySensor"+ toString(myId)).c_str());
	plot.SetTitle(("Efficiency timestamps for sensor " + toString(myId)).c_str());
	plot.SetBins(128, 0, 8192);
	plot.SetYTitle("Entries/64");
	plot.SetXTitle("Time");
	//std::cout << __PRETTY_FUNCTION__ << ", filling histogram...\n";
	for (unsigned t(0); t < 8192; t++) {
		if (myTimes.count(t) != 0) {
			std::pair<unsigned, unsigned>* count = myTimes[t];
			plot.Fill(t, (*count).first);
		}
	}
}

void MapsSensor::getInefficiencyTimestamps(TH1F& plot) {
	plot.SetName(("hTimestampsInefficiencySensor"+ toString(myId)).c_str());
	plot.SetTitle(("Inefficiency timestamps for sensor " + toString(myId)).c_str());
	plot.SetBins(128, 0, 8192);
	plot.SetYTitle("Entries/64");
	plot.SetXTitle("Time");
	//std::cout << __PRETTY_FUNCTION__ << ", filling histogram...\n";
	for (unsigned t(0); t < 8192; t++) {
		if (myTimes.count(t) != 0) {
			std::pair<unsigned, unsigned>* count = myTimes[t];
			plot.Fill(t, (*count).second);
		}
	}
}

void MapsSensor::getResidualXYPlot(TH2F& plot) const {
	plot.SetName(("hResidualsSensor" + toString(myId)).c_str());
	plot.SetTitle(("Residuals for sensor " + toString(myId)).c_str());
	plot.SetXTitle("x residual (mm)");
	plot.SetYTitle("y residual (mm)");
	plot.SetBins(100, -1.0, 1.0, 100, -1.0, 1.0);
	for (std::vector<std::pair<double, double> >::const_iterator it =
			myResiduals.begin(); it != myResiduals.end(); it++) {
		plot.Fill((*it).first, (*it).second);
	}
}

void MapsSensor::getEfficiencyCurve(TH1F& plot, int bins, int low, int high,
		bool withShapers, bool withSamplers) {
	if (withShapers && !withSamplers) {
		plot.SetTitle(("Shaper efficiency for sensor " + toString(myId)).c_str());
		plot.SetName(("hShaperEfficiencySensor" + toString(myId)).c_str());
	} else if (withSamplers && !withShapers) {
		plot.SetTitle(("Sampler efficiency for sensor " + toString(myId)).c_str());
		plot.SetName(("hSamplerEfficiencySensor" + toString(myId)).c_str());
	} else if (withShapers && withSamplers) {
		plot.SetTitle(("Efficiency for sensor " + toString(myId)).c_str());
		plot.SetName(("hEfficiencySensor" + toString(myId)).c_str());
	}
	plot.SetXTitle("Threshold");
	plot.SetYTitle("Efficiency (%)");
	plot.SetBins(bins, low, high);
	for (std::vector<unsigned>::iterator it = myKnownThresholds.begin(); it
			!= myKnownThresholds.end(); it++) {
		plot.Fill(*it, 100*getEfficiency(*it, withShapers, withSamplers));
	}

}

void MapsSensor::getEfficiencyByGroup(TH2F& plot, unsigned threshold) {
	plot.SetTitle(("Group efficiency for sensor " + toString(myId)).c_str());
	plot.SetName(("hGroupEfficiencySensor" + toString(myId)).c_str());
	plot.SetXTitle("Group + (Region x 6)");
	plot.SetYTitle("Row");
	plot.SetBins(28, 0, 28, 168, 0, 168);

	bool doAllThresholds(false);
	if (threshold == 0)
		doAllThresholds = true;
	else {
		//no support for threshold by threshold yet, so too bad
		MapsException me("Specifying a threshold isn't yet supported. Sorry.");
		throw me;
	}

	//group eff = nHits / (nHits + nNoHits)
	//make an array to hold this data
	//first index: group, second row, third eff/notEff
	unsigned groupEff[29][168][2];
	memset(groupEff, 0, 2*168*29*sizeof(unsigned));
	unsigned region, group, pseudoGroup;
	//physXY hit;
	coord px;
	for (std::vector<MapsSensor::coord>::const_iterator it =
			myEfficientHits.begin(); it != myEfficientHits.end() ; ++it) {
		px = *it;
		try {
			//convertPhysicalToHit(hit, px);

			//decode region
			region = static_cast<unsigned>(px.first/42);
			assert(region < 4);
			//decode group
			group = static_cast<unsigned>((px.first % 42) / 6);
			assert(group < 8);

			pseudoGroup = region * 7 + group;
			assert(pseudoGroup < 29);
			++groupEff[pseudoGroup][px.second][0];
		} catch (MapsException& me) {
			std::cout << __PRETTY_FUNCTION__ << __LINE__ << ": hmm, got a ME. Input hit: " << px << ", " << me.what() << "\n";
		}
	}

	for (std::vector<MapsSensor::coord>::const_iterator it =
			myInefficientHits.begin(); it != myInefficientHits.end() ; ++it) {
		px = *it;
		//decode region
		region = static_cast<unsigned>(px.first/42);
		assert(region < 4);
		//decode group
		group = static_cast<unsigned>((px.first % 42) / 6);
		assert(group < 8);

		pseudoGroup = region * 7 + group;
		assert(pseudoGroup < 29);
		++groupEff[pseudoGroup][px.second][1];
	}

	for (unsigned row(0); row < 168; ++row) {
		for (unsigned pseudoGrp(0); pseudoGrp < 29; ++pseudoGrp) {
			if ((groupEff[pseudoGrp][row][0] + groupEff[pseudoGrp][row][1])
					!= 0) {
				plot.Fill(pseudoGrp, row,
						static_cast<double>(groupEff[pseudoGrp][row][0])
								/ (groupEff[pseudoGrp][row][0]
										+ groupEff[pseudoGrp][row][1]));
			}
		}
	}

}
void MapsSensor::setResidual(MapsSensor::physXY resid) {
	myResiduals.push_back(resid);
}

unsigned MapsSensor::getLowestThresh() const {
	unsigned lowest = 100000;
	for (std::vector<unsigned>::const_iterator it = myKnownThresholds.begin(); it
			!= myKnownThresholds.end(); it++) {
		if (*it < lowest)
			lowest = *it;
	}
	return lowest;
}
unsigned MapsSensor::getHighestThresh() const {
	unsigned highest = 0;
	for (std::vector<unsigned>::const_iterator it = myKnownThresholds.begin(); it
			!= myKnownThresholds.end(); it++) {
		if (*it > highest)
			highest = *it;
	}
	return highest;
}

bool MapsSensor::isDeadArea(const physXY xyTest) const {
	physXY xy = xyTest;
	malign(xy);
	rotateGlobalToLocal(xy);
	//	std::cout << "\tHit in:\t" << xyTest;
	//	std::cout << "\trotated:\t" << xy << "\n";
	//eliminate dead area in y
	if (fabs(xy.second) < 0.025 || fabs(xy.second) > 4.225)
		return true;

	//region 0's left memory column and beyond
	if (xy.first < -4.45)
		return true;
	//right outer bound of sensor
	if (xy.first > 4.7)
		return true;
	//region 01 memory column
	if (xy.first > -2.35 && xy.first < -2.1)
		return true;
	//region 12 memory column
	if (xy.first > 0 && xy.first < 0.25)
		return true;
	//region 23 memory column
	if (xy.first > 2.35 && xy.first < 2.6)
		return true;
	//otherwise we're in business :-)
	return false;

}

void MapsSensor::convertHitToPhysical(const coord& c, physXY& xy) const {
	//region please
	int r = c.first / 42;

	//x coordinate first
	//x = shift by region + move out its dead area, then half a pixel, then ...
	xy.first = (r - 2) * 2.35 + 0.25 + 0.025 + (c.first % 42) * 0.05;

	//y coordinate
	if (c.second > 83)
		xy.second = 0.05 + (c.second - 84) * 0.05;
	else
		xy.second = (c.second - 84) * 0.05;

	physXY local(xy);
	rotateLocalToGlobal(xy);
	align(xy);
	assert(!isDeadArea(xy));
	//	if (isDeadArea(xy)) {
	//		std::cout << "OOPS:" << c << " \t -> " << local << "!!\n";
	//	}
}

void MapsSensor::convertPhysicalToHit(const physXY& xyIn, coord& c) const
		throw(MapsException) {

	if (isDeadArea(xyIn)) {
		std::string desc("Physical xy ");
		desc.append(toString(xyIn));
		desc.append(" falls in dead area.");
		MapsException me(desc.c_str());
		throw me;
	}

	physXY xy = xyIn;
	malign(xy);
	rotateGlobalToLocal(xy);

	unsigned r = static_cast<unsigned>(floor(xy.first/2.35) + 2);

	//y coordinate
	if (xy.second > 0.025)
		c.second = static_cast<unsigned>(84 + floor((xy.second - 0.025)/0.05));
	else
		c.second = static_cast<unsigned>(84 + floor((xy.second + 0.025)/0.05));

	//x coordinate
	//define localX i.t.o the local region
	unsigned localX = static_cast<unsigned>(floor((xy.first - ((r - 2) * 2.35
			+ 0.25)) / 0.05));

	//so the sensor x = region * 42 + localX
	c.first = r * 42 + localX;

}

unsigned MapsSensor::decodeRegion(const coord&  input) const {
	return input.first / 42;
}

unsigned MapsSensor::decodeRegion(const physXY& input) const throw(MapsException) {
	coord c;
	convertPhysicalToHit(input, c);
	return decodeRegion(c);
}

void MapsSensor::rotateLocalToGlobal(physXY& local) const {
	MapsSensor::physXY localCpy = local;
	local.first = cos(myPhi) * localCpy.first + sin(myPhi) * localCpy.second;
	local.second = -1.0 * sin(myPhi) * localCpy.first + cos(myPhi)
			* localCpy.second;
	assert(fabs(pow(localCpy.first, 2) + pow(localCpy.second, 2) - pow(local.first, 2) - pow(local.second, 2)) < 0.01);
}

void MapsSensor::rotateGlobalToLocal(physXY& glob) const {
	MapsSensor::physXY globCpy = glob;
	glob.first = cos(myPhi) * globCpy.first - sin(myPhi) * globCpy.second;
	glob.second = sin(myPhi) * globCpy.first + cos(myPhi) * globCpy.second;
	assert(fabs(pow(globCpy.first, 2) + pow(globCpy.second, 2) - pow(glob.first, 2) - pow(glob.second, 2)) < 0.01);

}

void MapsSensor::align(physXY& maligned) const {
	maligned.first = maligned.first + myAlignment.first;
	maligned.second = maligned.second + myAlignment.second;
}

void MapsSensor::malign(physXY& aligned) const {
	aligned.first = aligned.first - myAlignment.first;
	aligned.second = aligned.second - myAlignment.second;
}

void MapsSensor::printEfficiencies(std::ostream& s) {
	double sampYes(0), shapYes(0);
	double sampNo(0), shapNo(0);
	for (std::vector<unsigned>::const_iterator it = myKnownThresholds.begin(); it
			!= myKnownThresholds.end(); ++it) {
		double efficiency = getEfficiency(*it);
		double shapEff = getEfficiency(*it, true, false);
		double sampEff = getEfficiency(*it, false, true);
		if (efficiency > -0.9) {
			s.precision(3);

			shapYes += (myShaperHitsByThreshold[*it])->first;
			shapNo += (myShaperHitsByThreshold[*it])->second;

			sampYes += (mySamplerHitsByThreshold[*it])->first;
			sampNo += (mySamplerHitsByThreshold[*it])->second;

			s << "\t Thresh: " << *it << "\t: " << efficiency * 100
					<< ",\t shap:\t" << shapEff * 100 << ",\t samp:\t"
					<< sampEff * 100 << "\n";
			s << "\t\tShaper hits:\t " << (myShaperHitsByThreshold[*it])->first << " / " << (myShaperHitsByThreshold[*it])->first + (myShaperHitsByThreshold[*it])->second
					<< "\tSampler hits:\t " << (mySamplerHitsByThreshold[*it])->first << " / " << (mySamplerHitsByThreshold[*it])->first + (mySamplerHitsByThreshold[*it])->second << "\n";
		}

	}
	s << "\tOverall: \tShapers:\t" << shapYes << " / " << shapNo + shapYes
			<< " = " << 100 * shapYes / (shapNo + shapYes);
	s << "\tSamplers:\t" << sampYes << " / " << sampNo + sampYes << " = "
			<< 100 * sampYes / (sampNo + sampYes) << "\n";
}

void MapsSensor::mutualSensorMask(
		const std::vector<MapsSensor*>& sensors, TH2F& mutual) {
	std::cout << __PRETTY_FUNCTION__ << std::endl;
	TRandom2 randy;
	double x, y;
	mutual.SetName("hExclusionArea");
	mutual.SetTitle("Mutual exclusion area");
	mutual.SetBins(1000, -5.0, 5.0, 1000, -5.0, 5.0);
	for (unsigned u(0); u < 100000; u++) {
		x = randy.Uniform(-5.0, 5.0);
		y = randy.Uniform(-5.0, 5.0);
		MapsSensor::physXY test(x, y);
		//		if (s1->isDeadArea(test) || s2->isDeadArea(test)) {
		//			mutual.Fill(x, y);
		//		}
		unsigned deads = std::count_if(sensors.begin(), sensors.end(),
				std::bind2nd(IlEstMort(), test));
		if (deads > 0) {
			mutual.Fill(x, y);
		}

	}
}


std::ostream& operator<<(std::ostream& s, const MapsSensor& ms) {
	return s << "Sensor id " << ms.myId << " [z=" << ms.myPosition
			<< "\tphi_d=" << ms.myPhi/3.14159265358979323846 * 180.0
			<< ",\tal=" << ms.myAlignment << "]";
}