#include "MapsTrack.hh"
#include <cmath>
#include "MapsException.hh"
#include "TMath.h"
#include <algorithm>
#include <functional>

MapsTrack::~MapsTrack() {

}

//struct sensorHitPrinter :
//public std::binary_function<MapsSensor*, MapsTrack::coord>, void> {
//	void operator()(MapsSensor* s, MapsTrack::coord p) const {
//		std::cout << *s;
//		std::cout << "\tInput:\t" << p;
//		MapsTrack::physXY place;
//		s->convertHitToPhysical(p, place);
//		std::cout << "\t--> " << place << "\n";
//	}
//};

const std::map<MapsSensor*, MapsTrack::coord>& MapsTrack::getHits() {
	return myHits;
}

void MapsTrack::setPixelPitch(const double& pitch) {
	myPixelPitch = pitch;
}

void MapsTrack::setTrackError(const std::pair<double, double>& error) {
	myTrackError = error;
}

void MapsTrack::setHits(std::map<MapsSensor*, MapsTrack::coord> hits) {
	myHits = hits;
	getGlobalHits();
}
double MapsTrack::sigmaX() const {
	double x(0);
	double x2(0);
	unsigned count(0);
	for (std::map<MapsSensor*, MapsTrack::physXY>::const_iterator it =
			getGlobalHits().begin(); it != getGlobalHits().end(); ++it) {
		const physXY& c = (*it).second;
		x += c.first;
		x2 += c.first * c.first;
		count++;
	}
	return sqrt(static_cast<double>(x2)/count - static_cast<double>(x*x)/(count
			*count));
}
//and Y
double MapsTrack::sigmaY() const {
	double y(0);
	double y2(0);
	unsigned count(0);
	for (std::map<MapsSensor*, physXY>::const_iterator it = getGlobalHits().begin(); it != getGlobalHits().end(); ++it) {
		const physXY& c = (*it).second;
		y += c.second;
		y2 += c.second * c.second;
		count++;
	}
	return sqrt(static_cast<double>(y2)/count - static_cast<double>(y*y)/(count
			*count));
}
//their meanX and Y
double MapsTrack::meanX() const {
	double x(0);
	unsigned count(0);
	for (std::map<MapsSensor*, physXY>::const_iterator it = getGlobalHits().begin(); it != getGlobalHits().end(); ++it) {
		const physXY& c = (*it).second;
		x += c.first;
		count++;
	}
	return static_cast<double>(x)/count;
}
double MapsTrack::meanY() const {
	double y(0);
	unsigned count(0);
	for (std::map<MapsSensor*, physXY>::const_iterator it = getGlobalHits().begin(); it != getGlobalHits().end(); ++it) {
		const physXY& c = (*it).second;
		y += c.second;
		count++;
	}
	return static_cast<double>(y)/count;
}

MapsSensor* MapsTrack::fourthSensor() const {
	return myFourthSensor;
}
void MapsTrack::setFourthSensorThresh(const unsigned& fourthThresh) {
	myFourthThresh = fourthThresh;
}

void MapsTrack::tellSensorsOfResiduals() {
	if (!myToldSensorsResids) {
		for (std::map<MapsSensor*, physXY>::const_iterator it = getGlobalHits().begin(); it != getGlobalHits().end(); it++) {
			std::pair<double, double> resid = simpleResidual((*it).first);
			((*it).first)->setResidual(resid);
		}
		myToldSensorsResids = true;
	}
}

void MapsTrack::tellSensorsOfResiduals(const MapsSensor* const requiredLeft,
		const MapsSensor* const requiredRight) {
	if (leftSensor() == requiredLeft && rightSensor() == requiredRight)
		tellSensorsOfResiduals();
}

void MapsTrack::tellSensorsOfHits() {
	std::cout << __PRETTY_FUNCTION__ << ":\t Not yet implemented!\n";
}
MapsSensor* MapsTrack::missingSensor(std::vector<MapsSensor*> sensors) const {
	//loop over the supplied ids
	for (std::vector<MapsSensor*>::const_iterator it = sensors.begin(); it
			!= sensors.end(); it++) {
		MapsSensor* sensorToCheck = *it;
		//does the hit map contain it
		if (myHits.count(sensorToCheck) == 0)
			return sensorToCheck;
	}

	//all ids are contained in the map, so return null
	return 0;
}

double MapsTrack::theta() const {

	std::pair<double, double> p = fitParameters(0);
	std::pair<double, double> q = fitParameters(1);
	return acos(1.0/sqrt(pow(p.second, 2) + pow(q.second, 2) + 1));

}

MapsSensor* MapsTrack::leftSensor() const {
	double leftZ(0);
	MapsSensor* leftSensor = 0;
	for (std::map<MapsSensor*, coord>::const_iterator it = myHits.begin(); it
			!= myHits.end(); it++) {
		MapsSensor* s = (*it).first;
		if (leftSensor == 0) {
			leftZ = s->zPosition();
			leftSensor = s;
		} else if (s->zPosition() < leftZ) {
			leftZ = s->zPosition();
			leftSensor = s;
		}
	}
	return leftSensor;
}

MapsSensor* MapsTrack::rightSensor() const {
	double rightZ(0);
	MapsSensor* rightSensor = 0;
	for (std::map<MapsSensor*, coord>::const_iterator it = myHits.begin(); it
			!= myHits.end(); it++) {
		MapsSensor* s = (*it).first;
		if (rightSensor == 0) {
			rightZ = s->zPosition();
			rightSensor = s;
		} else if (s->zPosition() > rightZ) {
			rightZ = s->zPosition();
			rightSensor = s;
		}
	}
	return rightSensor;
}

std::pair<double, double> MapsTrack::fitParameters(const unsigned& dimension) const {
	//sum over i of 1 = N
	unsigned n(myHits.size());
	//sum over i of the z positions
	double sumZi(0);
	//and the z squareds
	double sumZiSq(0);

	//sum over x_i
	double sumXi(0);
	//sum over x_i * z_i
	double sumXiZi(0);

	for (std::map<MapsSensor*, MapsTrack::physXY>::const_iterator it =
			getGlobalHits().begin(); it != getGlobalHits().end(); ++it) {
		MapsSensor* s = (*it).first;
		//PRE-PHYSICAL XY
		//std::pair<double, double> xy((*it).second);
		//POST
		//		std::pair<double, double> xy;
		//		s->convertHitToPhysical((*it).second, xy);
		std::pair<double, double> xy((*it).second);
		//NEW: sensors now do their own alignment
		std::pair<double, double> c = xy;
		sumZi += s->zPosition();
		sumZiSq += s->zPosition() * s->zPosition();
		if (dimension == 0) {
			sumXi += c.first;
			sumXiZi += c.first * s->zPosition();
		} else if (dimension == 1) {
			sumXi += c.second;
			sumXiZi += c.second * s->zPosition();
		} else {
			throw 0;
		}
	}

	//std::cout << "n:\t" << n << ", sumZi:\t" << sumZi << ", sumZiSq:\t"
	//		<< sumZiSq << ", sumXi:\t" << sumXi << ", sumXiZi:\t" << sumXiZi
	//		<< "\n";
	double detM(n * sumZiSq -sumZi * sumZi);
	double delta(1.0/detM);
	//std::cout << "detM:\t" << detM << "\n";
	double p0(delta * sumZiSq * sumXi -delta * sumZi * sumXiZi);
	double p1(-1.0 * delta * sumZi * sumXi + n * delta * sumXiZi);
	//std::cout << "\tp0:\t" << p0 << ", p1:\t" << p1 << "\n";
	std::pair<double, double> answer(p0, p1);

	return answer;
}

double MapsTrack::chiSq(const unsigned& dimension) const {
	double chiSq(0);
	std::pair<double, double> ps = fitParameters(dimension);
	for (std::map<MapsSensor*, MapsTrack::physXY>::const_iterator it =
			getGlobalHits().begin(); it != getGlobalHits().end(); ++it) {
		MapsSensor* s = (*it).first;
		//PRE-PHYSICAL XY
		std::pair<double, double> xy((*it).second);
		//POST
		//std::pair<double, double> xy;
		//s->convertHitToPhysical((*it).second, xy);
		//std::pair<double, double> c = xy + s->getAlignment();
		//NEW: Sensors now do their own alignments
		std::pair<double, double> c = xy;
		if (dimension ==0) {
			chiSq += pow(c.first - (ps.first + s->zPosition()
							* ps.second), 2) / pow(myTrackError.first, 2);
		} else {
			chiSq += pow(c.second - (ps.first + s->zPosition()
							* ps.second), 2) / pow(myTrackError.second, 2);
		}
	}
	return chiSq;
}

double MapsTrack::chiSqProb(const unsigned& dimension) const {
	Double_t theChiSq(chiSq(dimension));
	Int_t ndf(getGlobalHits().size() - 2);
	return TMath::Prob(theChiSq, ndf);
}

std::ostream& MapsTrack::printCoords(std::ostream& s) const {
	for (std::map<MapsSensor*, MapsTrack::coord>::const_iterator it =
			myHits.begin(); it != myHits.end(); it++) {
		if((*it).first->phi() < 1.5)
			s << *((*it).first) << "\t: " << (*it).second  << "\t--> [" << (*it).second << "], " << getGlobalHits().at(((*it).first)) << "\n";
		else {
			MapsTrack::coord offset(168, 168);
			s << *((*it).first) << "\t: " << (*it).second << "\t--> [" << offset - (*it).second << "], " << getGlobalHits().at(((*it).first)) << "\n";
		}
			
	}
	return s;
}

std::map<MapsSensor*, std::pair<double, double> > MapsTrack::allResiduals() {
	std::map<MapsSensor*, std::pair<double, double> > resids;
	for (std::map<MapsSensor*, physXY>::const_iterator it = getGlobalHits().begin(); it != getGlobalHits().end(); it++) {
		MapsSensor* s = (*it).first;
		resids[s] = simpleResidual(s);
	}
	return resids;
}

std::pair<double, double> MapsTrack::simpleResidual(MapsSensor* s) const
		throw(MapsException) {
	//first check a hit exists for this sensor
	if (getGlobalHits().count(s) == 0) {
		// no such hit exists
		std::string desc(__PRETTY_FUNCTION__);
		desc.append(": Sensor doesn't exist!\n");
		MapsException me(desc.c_str());
		throw me;
	}

	MapsSensor* left = leftExtrapPoint(s);
	MapsSensor* right = rightExtrapPoint(s);
	std::map<MapsSensor*, physXY> globals = getGlobalHits();
	MapsSensor::physXY diff = globals.at(right) - globals.at(left);

	//what fraction should this difference be multiplied by to get to a
	//predicted x,y position?
	double zscale = s->zPosition()/(right->zPosition() - left->zPosition());
	//std::cout << "\tZScale:\t" << zscale << "\n";
	//what's the predicted coordinate of the third hit?
	MapsSensor::physXY pred(0, 0);
	pred.first = diff.first * zscale;
	pred.second = diff.second * zscale;
	//std::cout << "\tShift" << pred << "\n";
	MapsSensor::physXY lXY = globals.at(left);
	MapsSensor::physXY sXY = globals.at(s);
	pred = pred + lXY;
	//std::cout << "\tPred:" << pred <<"\n";
	MapsSensor::physXY resid = pred - sXY;
	return resid;
}

MapsSensor* MapsTrack::leftExtrapPoint(MapsSensor* s) const {
	//is it the left sensor?
	if (s == leftSensor()) {
		//go through sensors, find the one whose distance between this sensor and itself is minimised
		double separation(pow(2, sizeof(double))-1);
		MapsSensor* chosenSensor = 0;
		for (std::map<MapsSensor*, coord>::const_iterator it = myHits.begin(); it
				!= myHits.end(); it++) {
			MapsSensor* compare = (*it).first;
			if (compare != s) {
				if (separation > compare->zPosition() - s->zPosition()) {
					separation = compare->zPosition() - s->zPosition();
					chosenSensor = compare;
				}
			}
		}
		return chosenSensor;
	} else {
		return leftSensor();
	}
}

MapsSensor* MapsTrack::rightExtrapPoint(MapsSensor* s) const {
	//is it the right sensor?
	if (s == rightSensor()) {
		//go through sensors, find the one whose distance between this sensor and itself is minimised
		double separation(pow(2, sizeof(double)));
		MapsSensor* chosenSensor = 0;
		for (std::map<MapsSensor*, coord>::const_iterator it = myHits.begin(); it
				!= myHits.end(); it++) {
			MapsSensor* compare = (*it).first;
			if (compare != s) {
				if (separation > s->zPosition() -compare->zPosition()) {
					separation = s->zPosition() -compare->zPosition();
					chosenSensor = compare;
				}
			}
		}
		return chosenSensor;
	} else {
		return rightSensor();
	}
}

void MapsTrack::make3HitTrack(MapsTrack& mt) {
	if (myHits.size() == 3) {
		mt.setHits(myHits);
	} else {
		std::map<MapsSensor*, coord> newHits(myHits);
		newHits.erase(myFourthSensor);
		mt.setHits(newHits);
	}
	
	mt.setFourthSensorThresh(myFourthThresh);
	mt.setPixelPitch(myPixelPitch);
	mt.setTrackError(myTrackError);
	mt.myT = myT;
	mt.myFourthSensor = myFourthSensor;
	mt.myToldSensorsHits = myToldSensorsHits;
	mt.myToldSensorsResids = myToldSensorsResids;
}
//aligned output
unsigned MapsTrack::getFourthHitResidual(const MapsTrack& threeHitTrack,
		std::pair<double, double>& answer) {

	//If we don't have a fourth hit for comparison, tell these goons to go away
	if (getGlobalHits().size() == 3)
		return 1;

	//aligned
	std::pair<double, double> fourthHitXY = getGlobalHits().at(myFourthSensor);
	answer = threeHitTrack.findXYPrediction(myFourthSensor->zPosition())
				- (fourthHitXY);
//	answer = threeHitTrack.findXYPrediction(myFourthSensor->zPosition())
//			- (fourthHitXY + myFourthSensor->getAlignment());
	return 0;
}

const std::map<MapsSensor*, MapsTrack::physXY >& MapsTrack::getGlobalHits() const {
	//if (myGlobalHits.size() != myHits.size()) {
		for (std::map<MapsSensor*, MapsTrack::coord>::const_iterator it =
				myHits.begin(); it != myHits.end(); ++it) {
			MapsSensor* s= (*it).first;
			physXY xy;
			s->convertHitToPhysical((*it).second, xy);
			myGlobalHits[s] = xy;
		}
	//}
	return myGlobalHits;
}

void MapsTrack::eraseGlobalHits() {
	myGlobalHits.clear();
}

//this guy's aligned
std::pair<double, double> MapsTrack::findXYPrediction(const double& zpos) const {
	std::pair<double, double> p = fitParameters(0);
	std::pair<double, double> q = fitParameters(1);

	std::pair<double, double> pred;
	pred.first = (p.first + zpos * p.second);
	pred.second = (q.first + zpos * q.second);
	return pred;
}

std::ostream& diagnose(std::ostream& s, const MapsTrack& mt) {
	s.precision(3);
	s << std::fixed;
	s.width(3);
	std::pair<double, double> phys;
	s << "Track at BX:\t" << mt.timeStamp() << ", hit pattern:\n";
	mt.printCoords(s);

	s << "\t\tchi2X:\t" << mt.chiSq(0) << "\t chi2Y:\t" << mt.chiSq(1)
			<< "\tp:\t" << mt.fitParameters(0) << "\tq:\t"
			<< mt.fitParameters(1) << "\n";

	s << "\t\tchi2ProbX:\t" << mt.chiSqProb(0) << "\t chi2ProbY:\t"
			<< mt.chiSqProb(1) << "\n";
	s << "\t\ttheta:\t" << mt.theta() << "\tmeanX:\t" << mt.meanX()
			<< "\tmeanY:\t" << mt.meanY() << "\n";
	s << "\t\tleftSensor:\t" << *(mt.leftSensor()) << "\trightSensor:\t"
			<< *(mt.rightSensor()) << "\n";
	return s;
}
std::ostream& operator<<(std::ostream& s, const MapsTrack& mt) {
	return s << "Track at BX = " << mt.timeStamp() << ", meanX: " << mt.meanX()
			<< ", meanY: " << mt.meanY() << ", fourth sens: "
			<< *(mt.fourthSensor());
}