//_HADES_CLASS_DESCRIPTION
////////////////////////////////////////////////////////////////
//
//  HPionTrackerHitF:
//
//  Hit finder of the PionTracker
//
////////////////////////////////////////////////////////////////

#include "hpiontrackerhitf.h"
#include "hpiontrackerdef.h"
#include "hpiontrackercal.h"
#include "hpiontrackerhit.h"
#include "hpiontrackerdetector.h"
#include "hpiontrackerhitfpar.h"
#include "hpiontrackergeompar.h"
#include "../base/geometry/hgeomtransform.h"
#include "hades.h"
#include "hcategory.h"
#include "hdebug.h"
#include "hevent.h"
#include "hiterator.h"
#include "hruntimedb.h"
#include "hspectrometer.h"

#include <iomanip>
#include <iostream>
#include <algorithm>
#include <vector>
#include <cmath>
#include <stdlib.h>
#include <string.h>
using namespace std;

#define PR(x) std::cout << "++DEBUG: " << #x << " = |" << x << "| (" << __FILE__ << ", " << __LINE__ << ")\n";
#define PRh(x) std::cout << "++DEBUG: " << #x << " = hex |" << std::hex << x << std::dec << "| (" << __FILE__ << ", " << __LINE__ << ")\n";

typedef std::vector< StripData > strips_vec;

struct ClusterCand
{
	strips_vec hits; //neighbours

// 	void print(Int_t m = -1)
// 	{
// 		Int_t min_t, max_t, min_c, max_c;
// 		min_t = max_t = hits[0].first;
// 		min_c = max_c = hits[0].second.second;
// 
// 		printf("Cluster candidate (%d):\n", (int)hits.size());
// 		printf(" -c:\ts: %d,\tt: %.0f,\tc: %.0f\n", hits[0].second.first, hits[0].first, hits[0].second.second);
// 		for (size_t h = 1; h < hits.size(); ++h)
// 		{
// 			printf("  n:\ts: %d,\tt: %.0f,\tc: %.0f\n", hits[h].second.first, hits[h].first, hits[h].second.second);
// 
// 			if (hits[h].first < min_t) min_t = hits[h].first;
// 			if (hits[h].first > max_t) max_t = hits[h].first;
// 
// 			if (hits[h].second.second < min_c) min_c = hits[h].second.second;
// 			if (hits[h].second.second > max_c) max_c = hits[h].second.second;
// 		}
// 		if (hits.size() > 1)
// 			printf("CSTAT:\t%d\t%lu\t%d\t%d\n", m, hits.size(), max_t - min_t, max_c - min_c);
// 	}
};

struct ClusterData
{
	Int_t strip;			// keeps the info about local maximum strip location

	Float_t pos;			// weighted position of the cluster
	Float_t time;			// weighted time of the cluster
	Float_t charge;			// total charge of teh cluster

// 	void print()
// 	{
// 		printf("Cluster (%d):\n", (int)strip);
// 		printf(" -d:\tp: %.0f,\tt: %.0f,\tc: %.0f\n", pos, time, charge);
// 	}
};

struct HitsData
{
	Float_t posX;			// weighted position of the cluster
	Float_t timeX;			// weighted time of the cluster
	Float_t chargeX;			// total charge of teh cluster

	Float_t posY;			// weighted position of the cluster
	Float_t timeY;			// weighted time of the cluster
	Float_t chargeY;			// total charge of teh cluster

	Float_t time_avg;			// weighted time of the cluster
	Float_t charge_avg;			// total charge of teh cluster

	void calcAvgs()
	{
		Float_t qtot = chargeX + chargeY;
		time_avg = (timeX * chargeX + timeY*chargeY)/qtot;
		charge_avg = (chargeX + chargeY) / 2.0;
	}

// 	void print()
// 	{
// 		printf("Matched hit:\tx\ty\n");
// 		printf(" position:\t%.1f\t%.1f\n time\t%.0f\t%.0f\t%.0f\n charge:\t%.0f\t%.0f\t%.0f\n",
// 			   posX, posY, timeX, timeY, time_avg, chargeX, chargeY, charge_avg);
// 	}
};

// Sort hits by charge
Bool_t hit_sort_c(StripData a, StripData b)
{
	return a.second.second < b.second.second;
}

typedef std::vector< ClusterCand > clus_cand_vec;
typedef std::vector< ClusterData > clusters_vec;
typedef std::vector< HitsData > hits_vec;

Float_t hitFitFunction(Float_t timeX, Float_t chargeX, Float_t timeY, Float_t chargeY)
{
	Float_t etat = (timeX-timeY);///(timeX+timeY);
	Float_t etaq = (chargeX-chargeY);///(chargeX+chargeY);

// 	Float_t detat = etat*etat;
// 	Float_t detaq = etaq*etaq;
// 	return 1.0 / ( 1.0 + sqrt(dtsqr + dqsqr) );
// 	return 1.0 / ( 1.0 + sqrt(dtsqr + dqsqr) );
	return 1.0/(1.0 + fabs(etat)*3.3 + fabs(etaq));
}

// void printClusVec(const clusters_vec c)
// {
// 	printf("  p:");
// 	for (Uint_t i = 0; i < c.size(); ++i)
// 		printf("\t%.0f", c[i].pos);
// 	printf("\n  t:");
// 	for (UInt_t i = 0; i < c.size(); ++i)
// 		printf("\t%.0f", c[i].time);
// 	printf("\n  q:");
// 	for (UInt_t i = 0; i < c.size(); ++i)
// 		printf("\t%.0f", c[i].charge);
// 	printf("\n");
// }

// void printPerm(std::vector<int> v)
// {
// 	const size_t s = v.size();
// 	cout << "perm: ";
// 	for (UInt_t i = 0; i < s; ++i)
// 	{
// 		cout << v[i];
// 	}
// 	cout << endl;
// }

class combinatorics
{
public:
	combinatorics(Int_t n, Int_t r) : p_n(n), p_r(r)
	{
		this->c.resize(p_n);
		this->r.resize(p_r);
	}

	virtual void init() = 0;
	virtual Bool_t next() = 0;

	Int_t operator[](Int_t i)
	{
		return r[i];
	}

	void is_best() { b = r; }
	const std::vector<int> & best() { return b; }

protected:
	const Int_t p_n, p_r;
	std::vector<int> c, r, b;
};

class variation : public combinatorics
{
public:
	variation(Int_t n, Int_t r) : combinatorics(n, r) {}

	void init()
	{
		Int_t cnt = 0;
		for (Int_t i = p_n-p_r; i < p_n; ++i)
			c[i] = ++cnt;
	}

	Bool_t next()
	{
		for (Int_t i = 0; i < p_n; ++i)
		{
			if ( c[i] > 0 )
				r[c[i]-1] = i;
		}
		return std::next_permutation(c.begin(), c.end());
	}
};

class combination : public combinatorics
{
public:
	combination(Int_t n, Int_t r) : combinatorics(n, r) {}

	void init()
	{
		std::fill(c.begin() + p_n - p_r, c.end(), 1);
	}

	Bool_t next()
	{
		const size_t s = c.size();
		UInt_t j = 0;
		for (UInt_t i = 0; i < s; ++i)
		{
			if ( c[i] > 0 )
				r[j++] = i;
		}

		return std::next_permutation(c.begin(), c.end());
	}
};

hits_vec matchClusters(const clusters_vec cX, const clusters_vec cY, Float_t timeC, Float_t timeW, Float_t chargeC, Float_t chargeW)
{
	hits_vec hv;
	const size_t sX = cX.size();
	const size_t sY = cY.size();
	Float_t fitarr[sX][sY];
// const Int_t trig = 2;
// if (sX >= trig or sY >= trig)
// {
// 	printf("  = input data\n");
// 	printf("--- X\n");
// 	printClusVec(cX);
// 	printf("--- Y\n");
// 	printClusVec(cY);
// 	printf("---\n");
// }

	for (UInt_t i = 0; i < cX.size(); ++i)
		for (UInt_t j = 0; j < cY.size(); ++j)
		{
			Float_t tx = cX[i].time;
			Float_t qx = cX[i].charge;
			Float_t ty = cY[i].time;
			Float_t qy = cY[i].charge;

// 			Float_t diff_time = fabs(tx - ty - timeC);
// 			Float_t diff_charge = fabs(qx - qy - chargeC);
// PR(diff_charge);PR(chargeW);
// 			if ( (diff_time > timeW) or (diff_charge > chargeW) )  // time & charge matching?
// 				fitarr[i][j] = 0.;
// 			else
				fitarr[i][j] = hitFitFunction(tx, qx, ty, qy);
		}

// if (sX >= trig or sY >= trig)
// {
// 	printf("  - output data\n");
// 	for (UInt_t j = 0; j < cY.size(); ++j)
// 	{
// 		for (UInt_t i = 0; i < cX.size(); ++i)
// 			printf ("   %.6f", fitarr[i][j]);
// 		printf ("\n");
// 	}
// }

	const size_t ssize = sX < sY ? sX : sY;

	combination aX(sX, ssize);
	variation aY(sY, ssize);

	Bool_t rX = false;
	Bool_t rY = false;

	aX.init();
	aY.init();

	Float_t best_weight = 0.0;
// 	Int_t cmpiters = 0;
	do {
		rX = aX.next();

		do {
			rY = aY.next();
// ++cmpiters;
			Float_t weight = 0.0;
			for (UInt_t i = 0; i < ssize; ++i)
			{
// 				printf("  %f\t", fitarr[permX[i]][permY[i]]);
				weight += fitarr[aX[i]][aY[i]];
			}
			if (weight > best_weight)
			{
				best_weight = weight;
				aX.is_best();
				aY.is_best();
			}

		} while (rY);
	} while (rX);

// 	printf("!!!best: %f in %d\n", best_weight, cmpiters);
// 	printPerm(bestX);
// 	printPerm(bestY);

	for (UInt_t i = 0; i < ssize; ++i)
	{
		Int_t bestX = aX.best()[i];
		Int_t bestY = aY.best()[i];

		if (fitarr[bestX][bestY] != -1.)
		{
			HitsData hd = {
				/*.posX =*/ cX[bestX].pos, /*.timeX =*/ cX[bestX].time, /*.chargeX =*/ cX[bestX].charge,
				/*.posY =*/ cY[bestY].pos, /*.timeY =*/ cY[bestY].time, /*.chargeY =*/ cY[bestY].charge,
				/*.time_avg =*/ 0., /*.charge_avg =*/ 0.
			};
			hv.push_back(hd);
			hv[i].calcAvgs();
// 			printf("-> qavg = %f, tavg = %f\n", hv[i].charge_avg, hv[i].time_avg);
		}
	}
	return hv;
}

Float_t radius(StripData ref, StripData other, Float_t rad_s, Float_t rad_t)
{
	Float_t ds = fabs(ref.second.first - other.second.first);
	Float_t dt = fabs(ref.first - other.first);

	if ( (ds > rad_s) and (dt > rad_t) )
		return -2.; // not in the x-y radius
	if ( (ds > rad_s) or (dt > rad_t) )
		return -1; // not in the x or y radius

	return sqrt(ds*ds + dt*dt);
}

ClassImp (HPionTrackerHitF)
HPionTrackerHitF::HPionTrackerHitF (void)
{
	// default constructor
	pCalCat      = NULL;
	pHitCat      = NULL;
	iter         = NULL;
	pHitfpar     = NULL;
	pGeompar     = NULL;
}

HPionTrackerHitF::HPionTrackerHitF (const Text_t * name, const Text_t * title, Bool_t skip)
	: HReconstructor (name, title)
{
	// constructor
	// the parameter "skip" (default; kFALSE) defines, if the event is skipted in case no hit can be reconstructed
	pCalCat      = NULL;
	pHitCat      = NULL;
	iter         = NULL;
	pHitfpar     = NULL;
	pGeompar     = NULL;
}

HPionTrackerHitF::~HPionTrackerHitF (void)
{
	//destructor deletes the iterator on the raw category
	if (NULL != iter)
	{
		delete iter;
		iter = NULL;
	}
}

Bool_t HPionTrackerHitF::init (void)
{
	// gets the parameter containers
	// gets the PionTrackerCal category and creates the PionTrackerHit category
	// creates an iterator which loops over all fired strips in PionTrackerCal

	HPionTrackerDetector * det = (HPionTrackerDetector *) gHades->getSetup()->getDetector ("PionTracker");

	if (!det)
	{
		Error ("init", "No PionTracker found.");
		return kFALSE;
	}

	pHitfpar = (HPionTrackerHitFPar *) gHades->getRuntimeDb()->getContainer ("PionTrackerHitFPar");

	if (!pHitfpar) return kFALSE;

	pGeompar = (HPionTrackerGeomPar *) gHades->getRuntimeDb()->getContainer ("PionTrackerGeomPar");

	if (!pGeompar) return kFALSE;

	pCalCat = gHades->getCurrentEvent()->getCategory (catPionTrackerCal);

	if (!pCalCat)
	{
		Error ("init()", "HPionTrackerCal category not available!");
		return kFALSE;
	}

	iter = (HIterator *) pCalCat->MakeIterator();
	loccal.set (2, 0, 0);

	pHitCat = det->buildCategory (catPionTrackerHit);
	if (!pHitCat) return kFALSE;

	loc.set(2, 0, 0);

	fActive = kTRUE;

	return kTRUE;
}

Int_t HPionTrackerHitF::execute (void)
{
	// make hits
	HPionTrackerCal * pCal = NULL;
	HPionTrackerHit * pHit = NULL;

	const Int_t modules = pHitfpar->getNumModules();

	strips_vec fired_strips[modules];
	clus_cand_vec clus_cand[modules];
	clusters_vec clusters[modules];

	for (Int_t entry = 0; entry < pCalCat->getEntries(); ++entry)
	{
		if (NULL == (pCal = static_cast<HPionTrackerCal *> (pCalCat->getObject (entry))))
		{
			Error ("execute", "Pointer to HPionTrackerCal object == NULL, returning");
			return -1;
		}

		Int_t module = pCal->getModule();
		Int_t strip = pCal->getStrip();

		size_t mult = pCal->getMultiplicity();

		Float_t win_l = pHitfpar->getTimeWindowOffset(module);
		Float_t win_u = pHitfpar->getTimeWindowWidth(module) + win_l;
		Float_t q_thresh = pHitfpar->getChargeThresh(module);

		// put all fired strips info into array
		for (size_t i = 0; i < mult; ++i)
		{
			Float_t hit_t;
			Float_t hit_c;
			// get time and charge
			pCal->getTimeAndCharge(i, hit_t, hit_c);

			// check if time is inside the selectrion window
			if ( (hit_t < win_l) or (hit_t > win_u) or (hit_c < q_thresh) )
				continue;

			//---------------------------------------------------------------
			// fix warnings gcc < 4.7
			std::pair<Int_t, Float_t> pinner(strip, hit_c);
			StripData stripdat(hit_t,pinner);
			fired_strips[module].push_back(stripdat);
			//fired_strips[module].push_back( { hit_t, { strip, hit_c } } );  //  for C++11 gcc > 4.7
			//---------------------------------------------------------------
		}
	}

	// do actions per module
	for (Int_t m = 0; m < modules; ++m)
	{
		// sort by charge
		std::sort(fired_strips[m].begin(), fired_strips[m].end(), hit_sort_c);
// PR(m);
// PR(fired_strips[m].size());
		// get cluster size definitions per module
		Float_t clus_t_rad = pHitfpar->getClusterDistT(m);
		Float_t clus_s_rad = pHitfpar->getClusterDistX(m);
		Float_t clq_thresh = pHitfpar->getClusterThresh(m);

		// how many hits left
		Int_t hits_left =  fired_strips[m].size();
		while(hits_left)
		{
			// create new hit candidate
			ClusterCand hc;

			// get the maximal stored hit by charge size
			hc.hits.push_back(fired_strips[m][hits_left-1]);

			// check all fired strips for neighbours
			for (Int_t hf = hits_left-2; hf >= 0; --hf)
			{
				// check radius
				Float_t r = radius(hc.hits[0], fired_strips[m][hf], clus_s_rad, clus_t_rad);
				// if is inside radius
				if (r >= 0)
				{
					// add hit to cluster
					hc.hits.push_back(fired_strips[m][hf]);
					// remove used hit from fired strips array
					fired_strips[m].erase(fired_strips[m].begin() + hf);
				}
			}

			// fill cluster candidates array
			clus_cand[m].push_back(hc);

			// remove global maximum from the array
			fired_strips[m].pop_back();
			// allow for searching of next global maximum
			hits_left = fired_strips[m].size();
		}

		// loop over all cluster candidates
		for (size_t i = 0; i < clus_cand[m].size(); ++i)
		{
// 			clus_cand[m][i].print();

			// write data to the ouput category
			Float_t hit_t = 0;
			Float_t hit_s = 0;
			Float_t hit_c = 0;

			Int_t strip = -10000; // local maximum

			Float_t hit_c_tmp = -1; // temporal value of the charge

			// cluster size
			size_t mult = clus_cand[m][i].hits.size();

			if (!mult)
				continue;

			// local maximum strip number
			strip = clus_cand[m][i].hits[0].second.first;

			for (size_t j = 0; j < mult; ++j)
			{
				// charge of single strip, for weighting
				hit_c_tmp = clus_cand[m][i].hits[j].second.second;

				// weight time and strip by charge
				hit_t += clus_cand[m][i].hits[j].first * hit_c_tmp;
				hit_s += clus_cand[m][i].hits[j].second.first * hit_c_tmp;

				// add to total charge
				hit_c += hit_c_tmp;
			}

			if (hit_c < clq_thresh)
				continue;

			// scale position by charge
			hit_s /= hit_c;
			// scale time by charge and hits number
			hit_t /= hit_c;
			hit_t /= mult;

			// fill clusters array: local max strip number, weighted value: strip, time, charge
			//---------------------------------------------------------------
			// fix warnings gcc < 4.7
			ClusterData clusdat;
                        clusdat.strip  = strip;
                        clusdat.pos    = hit_s;
                        clusdat.time   = hit_t;
                        clusdat.charge = hit_c;
			clusters[m].push_back(clusdat);
			//clusters[m].push_back({strip, hit_s, hit_t, hit_c});  //  for C++11 gcc > 4.7
			//---------------------------------------------------------------
// printf("module = %d\n", m);
// clus_cand[m][i].print(m);
// clusters[m][i].print();
		}
// 	PR(clusters[m].size());
// if (clusters[m].size() > 5)
// {
// 	printf("!!!!!\n");
// 	for (Int_t i = 0; i < clusters[m].size(); ++i)
// 		clusters
// }

// if (clus_cand[m].size() > 6)
// {
// 	printf("!!!!!\n");
// 	for (Int_t m = 0; m < modules; ++m)
// 	{
// 		printf("==m: %d\n", m);
// 		for (Int_t i = 0; i < clus_cand[m].size(); ++i)
// 			clus_cand[m][i].print();
// 	}
// }
	}
	
	// make hit matching for each plane
	for (Int_t p = 0; p < pHitfpar->getNumPlanes(); ++p)
	{
		Int_t planeX, planeY;
		pHitfpar->getPlanePair(p, planeX,  planeY); // (0 1), (2 3)

		Float_t match_t_c = pHitfpar->getHitMatchTimeC(p);
		Float_t match_t_w = pHitfpar->getHitMatchTimeW(p);
		Float_t match_c_c = pHitfpar->getHitMatchChargeC(p);
		Float_t match_c_w = pHitfpar->getHitMatchChargeW(p);

		// avoid hit matching for huge number of cluster, for example cal data
		if (
			( clusters[planeX].size() > 5 or clusters[planeY].size() > 5 )
			and
			( clusters[planeX].size() * clusters[planeY].size() > 50 )
		)
			continue;

// 		printf("+ Hit matching: plane=%d\n", p);
		hits_vec h = matchClusters(clusters[planeX], clusters[planeY], match_t_c, match_t_w, match_c_c, match_c_w);

		HPionTrackerDetGeomPar * dgeo = pGeompar->getDetector(p);	// detector geometry
		HGeomTransform gt = dgeo->getLabTransform();				// transformation info

		Int_t plane_hit_cnt = 0;
		loc[0] = p;

		for (size_t i = 0; i < h.size(); ++i)
		{
			loc[1] = plane_hit_cnt++; //first save value to loc[1], then increase hit counter by one

			pHit = (HPionTrackerHit*)pHitCat->getObject(loc);
			if (!pHit)
			{
				pHit = (HPionTrackerHit *)pHitCat->getSlot(loc);
				if (pHit)
				{
					pHit = new(pHit) HPionTrackerHit;
					pHit->setPlaneHit(p, loc[1]); // save plane & hit number info to the object

					pHit->setTimeAndCharge(h[i].timeX, h[i].chargeX, h[i].timeY, h[i].chargeY);
					pHit->setLocalPos(h[i].posX, h[i].posY);

					// hit postion in the local frame
					HGeomVector v1(
						dgeo->getStripPos(h[i].posX),
						dgeo->getStripPos(h[i].posY),
						0);

					// hit position in the lab frame
					HGeomVector v2 = gt.getTransVector() + gt.getRotMatrix() * v1;
// 					printf("det: %d\n c: %f (%f, %f),\t t: %f (%f, %f)\n", p,
// 						diff_charge, clusters[planeX][i].charge, clusters[planeY][j].charge,
// 							diff_time, clusters[planeX][i].time, clusters[planeY][j].time);
// 					v1.print();
// 					v2.print();

					// set lab values to the object
					pHit->setLabPos(v2.getX(), v2.getY(), v2.getZ());
				}
				else
				{
					Error("execute()", "Can't get slot plane=%i, cnt=%i for hit %lu",
						  loc[0], loc[1], i+1);
					return 0;
				}
			}
			else
			{
				Error("execute()", "Slot already exists for plane=%i, cnt=%i", loc[0], loc[1]);
				return -1;
			}
		}
	}
	return 0;
}