#include "MapsTrackManager.hh"
#include "MapsTrack.hh"
#include "MapsSensor.hh"
#include "MapsException.hh"
#include <cmath>
#include <iostream>
#include <vector>
#include <algorithm>
#include <ostream>
#include <iterator>
#include <functional>
#include "TRandom2.h"
#include "TFile.h"
#include "TH1F.h"
#include "TH2F.h"

struct testArea :
	public std::binary_function<MapsSensor*, std::pair<double, double>, void> {
	void operator()(MapsSensor* s, std::pair<double, double> p) const {
		std::cout << *s << "\n";
		std::cout << "\tInput:\t" << p << "\n";
		MapsTrack::coord testCoord;
		if (s->isDeadArea(p))
			std::cout << "\tDead area? Yes\n";
		else {
			s->convertPhysicalToHit(p, testCoord);
			std::cout << "\tMaps to:\t" << testCoord << "\n";
		}
	}

};

int main() {
	std::cout << "** Testing Tracking...\n";
	double max(pow(2, 8*sizeof(double)) +1);
	double pi = 3.14159265358979323846;

	MapsTrack::coord c1(132, 137);
	MapsTrack::coord c2(30, 30);
	MapsTrack::coord c3(132, 137);
	MapsTrack::coord c4(30, 30);

	MapsSensor* s1 = new MapsSensor(1, 0, pi);
	MapsSensor* s2 = new MapsSensor(5, 10);
	MapsSensor* s3 = new MapsSensor(7, 20, pi);
	MapsSensor* s4 = new MapsSensor(9, 30);

	std::vector<MapsSensor*> sensors;
	sensors.push_back(s1);
	sensors.push_back(s2);
	sensors.push_back(s3);
	sensors.push_back(s4);

	std::cout << "** Sensors initialised. e.g.\n\t" << *s1 << "\n";
	std::map<MapsSensor*, MapsTrack::coord> theHits;
	theHits[s1] = c1;
	theHits[s2] = c2;
	theHits[s3] = c3;
	theHits[s4] = c4;
	//	std::pair<double, double> h;
	//	s1->convertHitToPhysical(c1, h);
	//	std::cout << "\t:" << h << "\n";
	//	s2->convertHitToPhysical(c2, h);
	//	std::cout << "\t:" << h << "\n";
	std::cout << "** Testing conversions...\n";
	//where are these places?
	MapsTrack::physXY p0(-2.0, 2.0);
	MapsTrack::physXY p1(-3.5, -2.8);
	MapsTrack::physXY p2(2.4, 1.0);
	std::for_each(sensors.begin(), sensors.end(), std::bind2nd(testArea(), p2));

	MapsTrack theTrack(35, theHits, s3, 100);

	std::cout << "** Track initialised:\n\t" << theTrack << "\n";
	std::cout << "Track's coordinates:\n";
	theTrack.printCoords(std::cout);

	if (theTrack.missingSensor(sensors) != 0)
		std::cout << "Missing sensor in track is "
				<< *(theTrack.missingSensor(sensors)) << "\n";

	//	std::cout << "Track's theta: \t" << theTrack.theta() << "\n";
	//
	//	std::cout << "s1's left extrap sensor: " << *(theTrack.leftExtrapPoint(s1))
	//			<< "\n";
	//	std::cout << "s1's right extrap sensor: "
	//			<< *(theTrack.rightExtrapPoint(s1)) << "\n";
	//	std::cout << "s3's left extrap sensor: " << *(theTrack.leftExtrapPoint(s3))
	//			<< "\n";
	//	std::cout << "s3's right extrap sensor: "
	//			<< *(theTrack.rightExtrapPoint(s3)) << "\n";
	//	std::cout << "s2's left extrap sensor: " << *(theTrack.leftExtrapPoint(s2))
	//			<< "\n";
	//	std::cout << "s2's right extrap sensor: "
	//			<< *(theTrack.rightExtrapPoint(s2)) << "\n";

	std::cout << "** Testing residuals...\n";
	std::pair<double, double> resid3(0, 0);

	resid3 = theTrack.simpleResidual(s3);

	MapsSensor* s9 = new MapsSensor(9, 100);
	try {
		std::pair<double, double> exception = theTrack.simpleResidual(s9);
	} catch (MapsException& me) {
		std::cout << me.what();
	}
	std::cout << "Residual for " << *s3 << ":\t" << resid3 << "\n";

	std::cout << "Getting p0, p1: X = \t" << theTrack.fitParameters(0) << "\n";
	std::cout << "Getting p0, p1: Y = \t" << theTrack.fitParameters(1) << "\n";
	std::cout << "Track's chiSq: X = \t" << theTrack.chiSq(0) << ", \tY = \t"
			<< theTrack.chiSq(1) << "\n";

	MapsTrackManager mtm(sensors);
	TFile* file = new TFile("trackTest.root", "recreate");
	//discuss this with mattttt
	std::vector<MapsTrack*> myTracks;
	MapsSensor::physXY alignS4(0.3, -0.5);
	//s4->setAlignment(alignS4);
	TRandom2 rand;
	TH2F sensorCorr("hSensorCorr", "hSensorCorr", 168, 0, 168, 1000, -5.0, 5.0);
	for (unsigned l(0); l < 10000; l++) {
		std::map<MapsSensor*, MapsTrack::coord> theseHits;
		//generate a random x, y
		double x, y, z;
		rand.Sphere(x, y, z, 5);
		MapsTrack::physXY location(x, y);
		//		MapsTrack::coord c1(129.5 + rand.Uniform() * 5, 134.5 + rand.Uniform()
		//				* 5);
		//		MapsTrack::coord c2(27.5 + rand.Uniform() * 5, 27.5 + rand.Uniform()
		//				* 5);
		//		MapsTrack::coord c3(129.5 + rand.Uniform() * 5, 134.5 + rand.Uniform()
		//				* 5);
		//		MapsTrack::coord c4(27.5 + rand.Uniform() * 5, 27.5 + rand.Uniform()
		//				* 5);
		MapsTrack::coord c1;
		MapsTrack::coord c2;
		MapsTrack::coord c3;
		MapsTrack::coord c4;

		//where would s1 have it?
		if (!s1->isDeadArea(location)) {
			s1->convertPhysicalToHit(location, c1);
			
			c1.first += rand.Uniform() * 2;
			c1.second += rand.Uniform() * 2;
			theseHits[s1] = c1;
			
		}
		if (!s2->isDeadArea(location)) {
			s2->convertPhysicalToHit(location, c2);
			c2.first += rand.Uniform() * 2;
			c2.second += rand.Uniform() * 2;
			theseHits[s2] = c2;
			sensorCorr.Fill(c2.first, location.first);
		}
		if (!s3->isDeadArea(location)) {
			s3->convertPhysicalToHit(location, c3);
			c3.first += rand.Uniform() * 2;
			c3.second += rand.Uniform() * 2;
			theseHits[s3] = c3;
		}
		if (!s4->isDeadArea(location)) {
			s4->convertPhysicalToHit(location, c4);
		}

		unsigned bx = rand.Uniform() * 8000;

		if (rand.Uniform() > 0.2 && !s4->isDeadArea(location))
			theseHits[s4] = c4;

		if (theseHits.size() > 2) {
			MapsTrack* thisTrack = new MapsTrack(bx, theseHits, s4, 100);
			myTracks.push_back(thisTrack);
			mtm.addTrack(thisTrack);
			if(c2.first < 1)
				diagnose(std::cout, *thisTrack);

			std::pair<double, double> fourthHitResid;
			MapsTrack threeHitVariant;
			thisTrack->make3HitTrack(threeHitVariant);
//			if (theseHits.size() == 4) {
//				std::cout << "** 3 hit version:\n";
//				diagnose(std::cout, threeHitVariant);
//			}
			unsigned status = thisTrack->getFourthHitResidual(threeHitVariant,
					fourthHitResid);
			if (status == 0) {
				s4->registerTrackConfirm(100, c4, bx, fourthHitResid);
				//std::cout << "\tFourth hit resid: " << fourthHitResid << "\n";
			} else {
				std::cout << __PRETTY_FUNCTION__ << __PRETTY_LINE__ << ": disabled!\n
				//s4->registerTrackMiss(100, bx);
			}
		}

	}

	sensorCorr.Write();
	TH1F* hChiSqX = new TH1F("hChiSqX", "Chi squared in X", 100, 0, 100);
	TH1F* hChiSqY = new TH1F("hChiSqY", "Chi squared in Y", 100, 0, 100);
	TH2F residS2, ineff2;
	for (std::vector<MapsTrack*>::iterator it = myTracks.begin(); it
			!= myTracks.end(); it++) {
		MapsTrack* t = *it;
		hChiSqX->Fill(t->chiSq(0));
		hChiSqY->Fill(t->chiSq(1));
		t->tellSensorsOfResiduals();
	}
	s2->getResidualXYPlot(residS2);
	s2->getInefficiencyXYPlot(ineff2, true);
	hChiSqX->Write();
	hChiSqY->Write();
	residS2.Write();
	ineff2.Write();
	TH1F efficiencyForSensor4;
	s4->getEfficiencyCurve(efficiencyForSensor4, 10, 50, 150);
	efficiencyForSensor4.Write();

	TH2F fourthHitResid;
	s4->getFourthHitResidualPlot(fourthHitResid);
	fourthHitResid.Write();

	file->Write();
	file->Close();

	std::cout << "Exporting to root file...\n";
	mtm.exportToRootFile("MapsTrackManager.root");

		std::cout << "Recreating from root file.\n";
		MapsTrackManager mtm2;
		mtm2.recreateFromRootFile("MapsTrackManager.root");
	//std::cout << "Rewriting to new root file...\n";
	//mtm2.exportToRootFile("MapsTrackManager2.root");


	std::cout << "Done.\n";
	return 0;
}