 // -*- LSST-C++ -*-
/* jonathan myers */


#include <cmath>
#include <sstream>

#include <gsl/gsl_fit.h>

#include <unistd.h>
#include <getopt.h>

#include "collapseTracklets.h"
#include "removeSubsets.h"
#include "rmsLineFit.h"

// these just need to be greater or less than any date we'll ever see...
#define IMPOSSIBLY_EARLY_MJD -1.0E16
#define IMPOSSIBLY_LATE_MJD 1.0E16

// hard to guess an ideal number for this - could use some benchmarks.
#define MAX_LEAF_SIZE 50 


namespace collapseTracklets {

    // declare some helper functions - these don't need to be exposed.

    void populateTrackletsForTreeVector(const std::vector<Detection> *detections,
                                        const std::vector<Tracklet> *tracklets,
                                        std::vector<KDTree::PointAndValue <unsigned int> >
                                        &trackletsForTree);



    bool detectionsHaveGT2UniqueTimes(const std::vector<Detection> * detections);



    /* returns true iff two tracklets are 'compatible', i.e. the set of their
       combined unique detections contains no detections with the same
       observation time. NB: if t1 is detections AB, and t2 is detections AC,
       then they ARE compatible if B, C have different times (because their
       combined detections are ABC). */

    bool trackletsAreCompatible(const std::vector<Detection> * detections,
                                Tracklet t1, Tracklet t2);



    /* returns the average of the squared distances of each detection in queryTracklet from
     * the line described by slopeAndOffsetRA, slopeAndOffsetDec. */ 
    double getAverageSqDist(std::vector<double> slopeAndOffsetRA, std::vector<double> slopeAndOffsetDec,
                            const std::vector<Detection>* allDets, const Tracklet* queryTracklet, 
                            double timeOffset=0.0);



    void printTracklet(Tracklet t) {
        std::cout << "collapsed: " << t.isCollapsed << std::endl;
        for (std::set<unsigned int>::iterator iter = t.indices.begin();
             iter != t.indices.end(); iter++) {
            std::cout << *iter << " ";
        }
        std::cout << std::endl;
    }












    // definitions of our functions



    bool TrackletCollapser::isSane(unsigned int detsSize, const std::vector<Tracklet> *pairs) {
        std::vector<Tracklet>::const_iterator tIter;
        std::set<unsigned int>::const_iterator iIter;
        for (tIter = pairs->begin(); tIter != pairs->end(); tIter++) {
            for (iIter = tIter->indices.begin(); iIter != tIter->indices.end(); 
                 iIter++) {
                if (*iIter >= detsSize) {
                    return false;
                }
            }
        }
        return true;
    }




    
    void printTrackletVector(std::vector<Tracklet> vt) {
        for (std::vector<Tracklet>::iterator iter = vt.begin();
             iter != vt.end(); iter++) {
            printTracklet(*iter);
        }
    }





    /* returns true iff smaller is a subset of larger*/
    bool isASubset(const std::set<unsigned int>* smaller, 
                    const std::set<unsigned int>* larger) {
        if (smaller->size() > larger->size()) {
            return false;
        }

        std::set<unsigned int> uniqueIndices;

        std::set_union(smaller->begin(), smaller->end(), 
                       larger->begin(), larger->end(),
                       std::inserter(uniqueIndices, uniqueIndices.begin()));
        if (uniqueIndices.size() == larger->size()) {
            return true;
        }
        return false;
        
    }



    double getSqDist(std::vector<double> slopeAndOffsetRA, std::vector<double> slopeAndOffsetDec,
                     const Detection* det, double timeOffset) {
        double mjd = det->getEpochMJD();
        double projectedRA = slopeAndOffsetRA[0]*(mjd - timeOffset) + slopeAndOffsetRA[1];
        double projectedDec = slopeAndOffsetDec[0]*(mjd - timeOffset) + slopeAndOffsetDec[1];
        double obsRA = det->getRA();
        double obsDec = det->getDec();
        double distRA = obsRA - projectedRA;
        double distDec = obsDec - projectedDec;
        return (distRA)*(distRA) + (distDec)*(distDec);
        
    }




    double getAverageSqDist(std::vector<double> slopeAndOffsetRA, std::vector<double> slopeAndOffsetDec,
                            const std::vector<Detection>* allDets, const Tracklet* queryTracklet, 
                            double timeOffset) {
        std::set<unsigned int>::const_iterator iter;
        double totalSqDist = 0.0;
        if (queryTracklet->indices.size() == 0) {
            std::cerr <<  "EE: call error: getAverageSqDist called with tracklet of length 0\n";
            exit(1);
        }
        for (iter = queryTracklet->indices.begin(); iter != queryTracklet->indices.end(); iter++) {
            totalSqDist += getSqDist(slopeAndOffsetRA, slopeAndOffsetDec, &((*allDets)[*iter]), timeOffset);
        }
        return totalSqDist / queryTracklet->indices.size();
    }





    bool trackletsAreCompatible(const std::vector<Detection> *detections, Tracklet t1, Tracklet t2) {
        std::set<unsigned int> uniqueIndices; 
        std::set<unsigned int>::iterator indexIter;
        std::set<unsigned int>::iterator uniqueIndicesIter;
        /* get a list of all detections in either tracklet, then get a list of
         * observation times for each tracklet, making sure there are no repeats
         * in the *observation* times.*/
        std::set_union(t1.indices.begin(), t1.indices.end(), t2.indices.begin(), t2.indices.end(),
                       std::inserter(uniqueIndices, uniqueIndices.begin()));

        std::set<double> representedMJDs;

        for (uniqueIndicesIter = uniqueIndices.begin(); uniqueIndicesIter != uniqueIndices.end(); 
             uniqueIndicesIter++) {
            double thisMJD = (*detections)[*uniqueIndicesIter].getEpochMJD();
            if (representedMJDs.insert(thisMJD).second == false) {
                /* insert failed because MJD is already there.*/
                return false;
            };
        }
        /* if we got this far without returning false, then all unique
         * detections came from separate times */
        return true;
    }




    void TrackletCollapser::collapse(Tracklet &t1, Tracklet& t2) {
        t1.isCollapsed = true;
        t2.isCollapsed = true;
        std::set<unsigned int> uniqueIndices; 
        std::set_union(t1.indices.begin(), t1.indices.end(), t2.indices.begin(), t2.indices.end(),
                       std::inserter(uniqueIndices, uniqueIndices.begin()));
        t2.indices = uniqueIndices;
    }




    Tracklet unionTracklets(Tracklet t1, Tracklet t2) {
        Tracklet toRet;
        std::set<unsigned int> uniqueIndices; 
        std::set_union(t1.indices.begin(), t1.indices.end(), t2.indices.begin(), t2.indices.end(),
                       std::inserter(uniqueIndices, uniqueIndices.begin()));
        toRet.indices = uniqueIndices;
        toRet.isCollapsed = false;
        return toRet;
    }
    



  
    /*
     * given vector detections and pairs, a vector of vectors of indices into
     * detections, "collapse" together highly similar tracklets (where each
     * element of "pairs" describes a tracklet, a collection of detections)
     * put the resulting tracklets into collapsedPairs.
     */
    void TrackletCollapser::doCollapsingPopulateOutputVector(
        const std::vector<Detection> * detections, 
        std::vector<Tracklet> &pairs,
        std::vector<double> tolerances, 
        std::vector<Tracklet> &collapsedPairs,       
        bool useMinimumRMS, bool useBestFit, 
        bool useRMSFilt,
        double maxRMSm, double maxRMSb, bool beVerbose) {

        /* each t in trackletsForTree maps tracklet physical parameters (RA0,
         * Dec0, angle, vel.) to an index into pairs. */
        std::vector<KDTree::PointAndValue <unsigned int> >
            trackletsForTree;
        std::vector<KDTree::PointAndValue<unsigned int> >::iterator trackletIter;
        std::vector<KDTree::PointAndValue<unsigned int> >::iterator similarTrackletIter;

        std::vector<KDTree::Common::GeometryType> geometryTypes(4);
        /* RA0, Dec0, and angle are all degree measures along [0,360).
	   Velocity is purely euclidean.*/
        geometryTypes[0] = KDTree::Common::CIRCULAR_DEGREES;
        geometryTypes[1] = KDTree::Common::CIRCULAR_DEGREES;
        geometryTypes[2] = KDTree::Common::CIRCULAR_DEGREES;
        geometryTypes[3] = KDTree::Common::EUCLIDEAN;

        if (beVerbose) {
            std::cout << "Extrapolating linear movement functions for each tracklet." << std::endl;
        }
        populateTrackletsForTreeVector(detections, &pairs, trackletsForTree);
        if (beVerbose) {
            std::cout << "done." << std::endl;
            
            std::cout << "Building KDTree of all tracklets.." << std::endl;
        }
        KDTree::KDTree<unsigned int> searchTree(trackletsForTree, 4, MAX_LEAF_SIZE);       
        if (beVerbose) {
            std::cout << "done." << std::endl;
            std::cout << "Doing many, many tree queries and collapses..." << std::endl;
        }
        unsigned int trackletCount = 0;
        for (trackletIter = trackletsForTree.begin(); 
             trackletIter != trackletsForTree.end();
             trackletIter++) {
            trackletCount++;
            /* don't collapse a given tracklet twice */
            if (pairs[trackletIter->getValue()].isCollapsed == false) {
                
                /* create a new tracklet which will be output. it is marked as
                   collapsed already, and so will be the current tracklet from
                   pairs.  This way we won't bother trying to collapse this tracklet again - 
                   if we don't get it now, it won't happen later, either. */
                Tracklet newTracklet;
                collapse(pairs[trackletIter->getValue()], newTracklet);

                /* find all similar tracklets */
                std::vector<KDTree::PointAndValue<unsigned int> > queryResults = 
                    searchTree.hyperRectangleSearch((*trackletIter).getPoint(), 
                                                    tolerances, 
                                                    geometryTypes);
                


                if (useMinimumRMS) {
                    bool done = false;
                    /* until no work is done: check the RMS of the current
                     * tracklet combined with the each similar tracklet.  Choose
                     * the "best" option and collapse that into the current
                     * tracklet. repeat.
                     */
                    while (done == false) {
                        bool foundOne = false;
                        double bestMatchRMS = 1337;
                        unsigned int bestMatchID = 1337;
                        for (similarTrackletIter = queryResults.begin();
                             similarTrackletIter != queryResults.end();
                             similarTrackletIter++) {
                            /* try combining the current tracklet with each
                               similar tracklet and getting an RMS value. */
                            unsigned int similarTrackletID = similarTrackletIter->getValue();			    
			    if ((pairs[similarTrackletID].isCollapsed == false) && 
                                (trackletsAreCompatible(detections, pairs[similarTrackletID], newTracklet))) {
                                Tracklet tmp = unionTracklets(newTracklet, pairs[similarTrackletID]);
                                double tmpRMS = rmsLineFit::rmsForTracklet(tmp, detections);
                                double trackletAvMag = rmsLineFit::getAverageMagnitude(tmp, detections);  
                               if ((useRMSFilt == false) || 
                                    (tmpRMS <= trackletAvMag * maxRMSm + maxRMSb)) {
                                    if ((foundOne == false) || (bestMatchRMS > tmpRMS)) {
                                        foundOne = true;
                                        bestMatchRMS = tmpRMS;
                                        bestMatchID = similarTrackletID;
                                    }
                                }
                            }
                        }
                        if (foundOne == true) {
                            collapse(pairs[bestMatchID], newTracklet);
                        }
                        else {
                            /* found no allowable matches*/
                            done = true;
                        }
                    }
                }
                else if (useBestFit) {
                    bool done = false;
                    /* until no work is done: get the best-fit equation for the
                     * current concept of the tracklet.  Find the legal similar
                     * tracklet whose detections are closest to the current
                     * conception of the line (if one exists).  Collapse that
                     * tracklet into the current one.  Repeat.
                     */
                    while (done == false) {
                        bool foundOne = false;
                        double bestMatchAvSqDist = 1337;
                        unsigned int bestMatchID = 1337;
                        std::vector <double> currentRAFunc;
                        std::vector <double> currentDecFunc;
                        std::vector <Detection> trackletDets;
                        std::set<unsigned int>::iterator detIter;
                        for (detIter = newTracklet.indices.begin(); 
                             detIter != newTracklet.indices.end(); 
                             detIter++) {
                            trackletDets.push_back((*detections)[*detIter]);
                        }
                        double t0 = (*detections)[*newTracklet.indices.begin()].getEpochMJD();
                        leastSquaresSolveForRADecLinear(&trackletDets, currentRAFunc, currentDecFunc, t0);

                        for (similarTrackletIter = queryResults.begin();
                             similarTrackletIter != queryResults.end();
                             similarTrackletIter++) {
                            /* find the similar tracklet closest to the current line. */
                            unsigned int similarTrackletID = similarTrackletIter->getValue();
                            const Tracklet* similarTracklet = &pairs[similarTrackletID];
                            std::map<unsigned int, double> detIDToSqDist;
                            if ((pairs[similarTrackletID].isCollapsed == false) && 
                                (trackletsAreCompatible(detections, *similarTracklet, newTracklet))) {
                                double netSqDist = 0;
                                unsigned int newDets = 1;
                                for (std::set<unsigned int>::const_iterator sIter=similarTracklet->indices.begin();
                                     sIter != similarTracklet->indices.end(); sIter++) {
                                    if (newTracklet.indices.find(*sIter) == newTracklet.indices.end()) {
                                        // this detection is not already in our current tracklet
                                        newDets++;
                                        if (detIDToSqDist.find(*sIter) != detIDToSqDist.end()) {
                                            // look it up
                                            netSqDist += detIDToSqDist[*sIter];
                                        }
                                        else {
                                            //calculate it and save it
                                            double newSqDist = getSqDist(currentRAFunc, currentDecFunc, 
                                                                         &((*detections)[*sIter]), t0);
                                            detIDToSqDist.insert(std::make_pair(*sIter, newSqDist));
                                            netSqDist += newSqDist;
                                        }
                                    }
                                }
                                double avSqDist = netSqDist / newDets;
                                if ((foundOne == false) || (avSqDist < bestMatchAvSqDist)) {
                                    foundOne = true;
                                    bestMatchAvSqDist = avSqDist;
                                    bestMatchID = similarTrackletID;
                                }
                            }
                        }
                        if (foundOne == true) {
                            // if newTracklet + best match has higher RMS than filter allows, we're done!
                            Tracklet tmp = unionTracklets(newTracklet, pairs[bestMatchID]);
                            double tmpRMS = rmsLineFit::rmsForTracklet(tmp, detections);
                            double trackletAvMag = rmsLineFit::getAverageMagnitude(tmp, detections); 
                            if ((useRMSFilt == true) && 
                                (tmpRMS > trackletAvMag * maxRMSm + maxRMSb)) {
                                done = true;
                            }
                            else { /* we got a result, and it was legal */
                                collapse(pairs[bestMatchID], newTracklet);
                            }
                        }
                        else {
                            /* found no allowable matches */
                            done = true;
                        }
                    }
                }
                else {
                    /* be greedy - try tracklets without much discriminiation */
                    for (similarTrackletIter = queryResults.begin();
                         similarTrackletIter != queryResults.end();
                         similarTrackletIter++) {
                        /* if tracklet is similar, has not already been collapsed,
                           not == query tracklet, and compatible with this tracklet
                           so far, then go ahead and collapse them together
                           greedily. */
                        Tracklet similarTracklet = pairs[similarTrackletIter->getValue()];
                        if ((similarTrackletIter->getValue() != trackletIter->getValue()) 
                            &&
                            (similarTracklet.isCollapsed == false)
                            && 
                            (trackletsAreCompatible(detections, similarTracklet, newTracklet))) {
                            bool collapseIsLegal = true;
                            if (useRMSFilt) {
                                /* check that this is a 'good enough' fit to use. */
                                Tracklet tmp = newTracklet;
                                /* subtly abuse the 'collapse' function as a union operation */
                                collapse(pairs[similarTrackletIter->getValue()], newTracklet);
                                if (rmsLineFit::rmsForTracklet(tmp, detections) >
                                    rmsLineFit::getAverageMagnitude(tmp, detections) * maxRMSm + maxRMSb) {
                                    collapseIsLegal = false;
                                }
                            }
                            if (collapseIsLegal) {
                                collapse(pairs[similarTrackletIter->getValue()], newTracklet);
                            }
                        }
                    }                        
                }

                /* this tracklet is valid output. */
                collapsedPairs.push_back(newTracklet);
            } /* end 'if (pairs[trackletIter->getValue().isCollapsed == false) */
        
        } /* end 'for (trackletIter in trackletsForTree... ) */

        /* temporary sanity check */

        for (std::vector<Tracklet>::iterator tIter = pairs.begin();
             tIter != pairs.end();
             tIter++) {
            if (tIter->isCollapsed == false) {
                std::cerr << "got a tracklet which was not collapsed!" << std::endl;
            }
        }
    }
    




    void TrackletCollapser::writeTrackletsToOutFile(const std::vector<Tracklet> * tracklets, std::ofstream &outFile){
        std::vector<Tracklet>::const_iterator trackletIter;
        std::set<unsigned int>::iterator indicesIter;
        for (trackletIter = tracklets->begin(); trackletIter != tracklets->end(); trackletIter++) {
            for (indicesIter = trackletIter->indices.begin(); 
                 indicesIter != trackletIter->indices.end();
                 indicesIter++) {
                outFile << *indicesIter;
                outFile << " ";
            }
            outFile << std::endl;
        }
    }




    bool detectionsHaveGT2UniqueTimes(const std::vector<Detection> *detections) {
        std::vector<double> observedMJDs;
        double curMJD = -1;
        std::vector<double>::iterator fIter;
        for (unsigned int i = 0; i < detections->size(); i++) {

            curMJD = (*detections)[i].getEpochMJD();
            bool isInList = false;

            for (fIter = observedMJDs.begin(); 
                 (fIter != observedMJDs.end()) && (isInList == false);
                 fIter++) {
                if (KDTree::Common::areEqual(*fIter, curMJD)) {
                    isInList = true;
                }
            }

            if (isInList == false) {
                observedMJDs.push_back(curMJD);
            }
            if (observedMJDs.size() > 2) {
                return true;
            }
        }
        /* we never got observedMJDs > 2, return false */
        return false;        
    }



    void make180To360Negative(std::vector<double> &v) {
        std::vector<double>::iterator iter;
        for (iter = v.begin(); iter != v.end(); iter++){
            if (*iter > 180) {
                *iter = *iter - 360;
            }
        }
    }



    void TrackletCollapser::leastSquaresSolveForRADecLinear(const std::vector <Detection> *trackletDets,
                                                            std::vector<double> &RASlopeAndOffsetOut,
                                                            std::vector<double> &DecSlopeAndOffsetOut,
                                                            double timeOffset) {
        unsigned int numDets = (*trackletDets).size();
        
        std::vector<double> MJDs(numDets);
        std::vector<double> RAs(numDets);
        std::vector<double> Decs(numDets);

        if ((RASlopeAndOffsetOut.size() != 0) || (DecSlopeAndOffsetOut.size() != 0)) {
            //TBD real LSST exceptions 
            std::cerr <<  "EE:  leastSquaresSolveForRADecLinear: output vectors were not empty.\n";
            exit(1);
        }

        for (unsigned int i = 0; i < numDets; i++) {
            RAs[i] = (*trackletDets)[i].getRA();
            Decs[i] = (*trackletDets)[i].getDec();
            MJDs[i] = (*trackletDets)[i].getEpochMJD() - timeOffset;
        }

        /* here we make the assumption that the data is not very far apart in RA
        * or Dec degrees.  Since we are currently only concerned with a single
        * night of observation, this should be very safe.  Note that under this
        * assumption, our data could only span > 180 deg if the object crosses
        * the 0/360 line.*/

        if (*std::max_element(RAs.begin(), RAs.end()) - *std::min_element(RAs.begin(), RAs.end()) > 180.) {
            make180To360Negative(RAs);
        }
        if (*std::max_element(Decs.begin(), Decs.end()) - *std::min_element(Decs.begin(), Decs.end()) > 180.) {
            make180To360Negative(Decs);            
        }
        if ((*std::max_element(RAs.begin(), RAs.end()) - *std::min_element(RAs.begin(), RAs.end()) > 180.) ||
            (*std::max_element(Decs.begin(), Decs.end()) - *std::min_element(Decs.begin(), Decs.end()) > 180.)) {
            //TBD: use a real exception here. 
            std::cerr <<  "EE: Unexpected coding error: could not move data into a contiguous < 180 degree range.\n";
            exit(1);
        }

        /* use linear least squares to get MJD->RA, MJD->Dec funs */
        /* need to get C-style arrays for gsl. Do this The Right Way, rather
         * than with the famous hack: &(RA[0]) */
        double *arrayRAs = (double*)malloc(sizeof(double) * numDets);
        double *arrayDecs = (double*)malloc(sizeof(double) * numDets);
        double *arrayMJDs = (double*)malloc(sizeof(double) * numDets);

        if ((arrayRAs == NULL) || (arrayDecs == NULL) || (arrayMJDs == NULL)) {
            // TBD: use real exceptions
            std::cerr << "EE: Malloc returned NULL on a very small malloc. System out of memory or something very odd.\n";
            exit(2);
        }
        
        for (unsigned int i = 0; i < numDets; i++) {
            arrayRAs[i] = RAs[i];
            arrayDecs[i] = Decs[i];
            arrayMJDs[i] = MJDs[i];
        }
        
        int gslRV = 0;
        double offset, slope;
        double cov00, cov01, cov11, sumsq;

        // arrayMJDs is independent, arrayRAs is dependent.
        gslRV += gsl_fit_linear(arrayMJDs, 1, arrayRAs, 1, numDets, &offset, &slope, 
                                &cov00,&cov01,&cov11,&sumsq);
        RASlopeAndOffsetOut.push_back(slope);
        RASlopeAndOffsetOut.push_back(offset);
        
        gslRV += gsl_fit_linear(arrayMJDs, 1, arrayDecs, 1, numDets, &offset, &slope, 
                                &cov00,&cov01,&cov11,&sumsq);
        DecSlopeAndOffsetOut.push_back(slope);
        DecSlopeAndOffsetOut.push_back(offset);

        if (gslRV != 0) {
            // use real exceptions
            std::cerr << "EE: gsl_fit_linear unexpectedly returned error.\n";
            exit(10);
        }

    }



  /*
   * set define t = 0 as normalTime, so all times in the detections
   * are offsets thereof.  Then find best-fit function for mapping
   * time to RA and Dec (in deg).  Extrapolate RA, Dec at t=0; also
   * find the angle above RA = 0, Dec=t and the velocity in deg/day of
   * the tracklet.  These three will be our "point" in space.
   * 
   * ASSUME that physicalParams was allocated with 4 slots.
   */
    void TrackletCollapser::setPhysicalParamsVector(const std::vector<Detection> *trackletDets,
                                                    std::vector<double> &physicalParams,
                                                    double normalTime) {
        std::vector<double> RASlopeAndOffset;
        std::vector<double> DecSlopeAndOffset;
        

        // physical params 0, 1 are initial RA, initial Dec at normalTime.
        leastSquaresSolveForRADecLinear(trackletDets, RASlopeAndOffset, DecSlopeAndOffset,
                                        normalTime);
        physicalParams[0] = KDTree::Common::convertToStandardDegrees(RASlopeAndOffset[1]);
        physicalParams[1] = KDTree::Common::convertToStandardDegrees(DecSlopeAndOffset[1]);
        double RAv = RASlopeAndOffset[0];
        double Decv = DecSlopeAndOffset[0];
        double velocity = sqrt(pow(RAv, 2) + pow(Decv, 2));
        physicalParams[3] = velocity;
        if (velocity > 0) {
            double sineOfTheta = Decv/velocity;
            double theta = asin(sineOfTheta) * (360./(2*M_PI));
            physicalParams[2] = KDTree::Common::convertToStandardDegrees(theta);
        }
        else {
            physicalParams[2] = 0;     // velocity == 0, so make something up
        }
    }




    /* returns average of first obs time + last obs time */
    double getMidPointTime(const std::vector<Detection> *detections) {
        // these are reasonable extremes.
        double firstTime = IMPOSSIBLY_LATE_MJD;
        double lastTime = IMPOSSIBLY_EARLY_MJD;
        double midPointTime = 0.0;
        for (unsigned int i = 0; i < (*detections).size(); i++) {
            double curMJD = (*detections)[i].getEpochMJD();
            if (curMJD > lastTime) {
                lastTime = curMJD;
            }
            if (curMJD < firstTime) {
                firstTime = curMJD;
            }
        }
	midPointTime = (firstTime + lastTime) / 2.0;

        return midPointTime;
        
    }




    void TrackletCollapser::populateTrackletsForTreeVector(const std::vector<Detection> *detections,
                                                           const std::vector<Tracklet> * tracklets,
                                                           std::vector<KDTree::PointAndValue <unsigned int> >
                                                           &trackletsForTree) {

        double midPointTime = getMidPointTime(detections);
        std::vector<Tracklet>::const_iterator trackletIter;
        std::set<unsigned int>::iterator indicesIter;

        unsigned int i = 0;
        for (trackletIter = (*tracklets).begin(); trackletIter != (*tracklets).end(); trackletIter++) {
            KDTree::PointAndValue <unsigned int> curTracklet;
            std::vector<double> physicalParams(4); /* will hold RA0, Dec0, angle, velocity */
            std::vector<Detection> trackletDets;

            for (indicesIter = trackletIter->indices.begin(); 
                 indicesIter != trackletIter->indices.end();
                 indicesIter++) {
                trackletDets.push_back((*detections)[*indicesIter]);
            }

            setPhysicalParamsVector(&trackletDets, physicalParams, midPointTime);
	    /* for the KDTree, use physical params as the spatial
	       'point' and make sure we get a reference to the current
	       index into pairs as the associated value */
	    curTracklet.setPoint(physicalParams);
            /*each PointAndValue for the tree has Value which is index into
             * tracklets vector of corresponding tracklet*/
	    curTracklet.setValue(i);
	    trackletsForTree.push_back(curTracklet);
            i++;
        }
    }





    void TrackletCollapser::populateDetVectorFromFile(std::ifstream &detsFile, std::vector <Detection> &myDets) {
        std::string line;
        Detection tmpDet;
        line.clear();
        std::getline(detsFile, line);
        while ((detsFile.eof() == false) && (detsFile.fail() == false)) {
            tmpDet.fromMITIString(line);
            myDets.push_back(tmpDet);
            line.clear();
            std::getline(detsFile, line);
        }
    }






    void TrackletCollapser::populatePairsVectorFromFile(std::ifstream &pairsFile, 
                                                        std::vector<Tracklet> &pairsVector) {

        Tracklet tmpPair;
        std::string line;
        int tmpInt = -1;
        tmpPair.indices.clear();
        tmpPair.isCollapsed = false;
        line.clear();
        std::getline(pairsFile, line);
        while ((pairsFile.eof() == false) && (pairsFile.fail() == false)) {
            std::istringstream ss(line);
            ss.exceptions(std::ifstream::failbit | std::ifstream::badbit);    
            while (!ss.eof()) {
                try {
                    ss >> tmpInt;
                    ss >> std::ws;
                }
                catch (...) {
                    //TBD: real exceptions...                 
                    std::cerr << "EE: Improperly-formatted pairs file." << std::endl;
                    exit(2);
                }
                tmpPair.indices.insert(tmpInt);
            }
            if (tmpPair.indices.size() < 2) {
                // TBD: use real exceptions...
                std::cerr <<  "EE: CollapseTracklets: pairs in pairs file must be length >= 2!\n";
                exit(1);
            }
            pairsVector.push_back(tmpPair);
            tmpPair.indices.clear();
            tmpPair.isCollapsed = false;
            line.clear();
            std::getline(pairsFile, line);   
        }
    }





    int collapseTrackletsMain(int argc, char** argv)
    {        
        std::string USAGE("USAGE: collapseTracklets [options] <detsFile> <pairsFile> <RA Tolerance> <Dec Tolerance> <angular tolerance> <velocity tolerance> <outFile>\n\
options:  \n\
=================================================\n\
--method=<greedy|minimumRMS|bestFit>    \n\
-------------------------------------------------\n\
if greedy, then we choose as many compatible tracklets as possible, as returned by the tree search.  if minimumRMS, we take the results of the tree search and repeatedly find the tracklet which would have the lowest resulting RMS value if added, then add it. If bestFit, we repeatedly choose the tracklet which is closest to the current approximate line first, rather than re-calculating best-fit line for each possible tracklet. (NB: Greedy is default.)\n\
\n\
--useRMSFilt=<true/false>  \n\
--------------------------------------------------\n\
enforce a maximum RMS distance for any tracklet which is the product of collapsing.  default is false.\n\
\n\
--maxRMSm=<double>,\n\
--maxRMSb=<double> \n\
--------------------------------------------------\n\
Only used if useRMSfilt == true.  Describes the function for RMS filtering.  Tracklets will not be collapsed unless the resulting tracklet would have RMS <= maxRMSm * average magnitude + maxRMSb.   Defaults are 0. and .001.\n\
");

        std::ifstream detsFile;
        std::ifstream pairsFile;
        std::vector <double> tolerances;
        std::ofstream outFile;
        std::string line;
        std::vector <Detection> detections;
        /* pairs holds indices into detections. */
        std::vector <Tracklet> pairs;
        std::vector<Tracklet> collapsedPairs;        

        /* read parameters from command-line */

        /* read long options, then get required options */

        if (argc < 8) {
            std::cerr << USAGE << std::endl;
            exit(1);
        }

        
        bool useMinimumRMS = false;
	bool useGreedy = true;
        bool useBestFit = false;
        bool useRMSFilt = false;
        double maxRMSm = 0.;
        double maxRMSb = .001;
        
        static const struct option longOpts[] = {
            { "method", required_argument, NULL, 'e' },
            { "useRMSFilt", required_argument, NULL, 'u' },
            { "maxRMSm", required_argument, NULL, 'm' },
            { "maxRMSb", required_argument, NULL, 'b' },
            { "help", no_argument, NULL, 'h' },
            { NULL, no_argument, NULL, 0 }
        };

        const char* optString = "e:u:m:b:h:v";
        int longIndex = -1;
        int opt = getopt_long( argc, argv, optString, longOpts, &longIndex );        
        std::stringstream ss;
	bool beVerbose = false;
        while( opt != -1 ) {
            switch( opt ) {
	    case 'v':
	      beVerbose = true;
              break;
            case 'e':
                if (std::string(optarg) == std::string("greedy")) {
                    useMinimumRMS = false;                  
		    useBestFit = false;
                }
                else if (std::string(optarg) == std::string("minimumRMS")) {
                    useMinimumRMS = true;
		    useGreedy = false;
                }
                else if (std::string(optarg) == std::string("bestFit")) {
                    useBestFit = true;
		    useMinimumRMS = false;
		    useGreedy = false;
                }
                else {
                    std::cerr << "ERROR: options for method are: greedy, minimumRMS" << std::endl;
                    std::cerr << USAGE << std:: endl;
                    return 1;
                }
                break;

            case 'u':
                useRMSFilt = removeSubsets::guessBoolFromStringOrGiveErr(optarg, USAGE);
                break;
                
            case 'm':
                ss.clear();
                ss << optarg;
                ss >> maxRMSm;
                break;

            case 'b':
                ss.clear();
                ss << optarg;
                ss >> maxRMSb;
                break;
                
            case 'h':   /* fall-through is intentional */
            case '?':
                std::cout << "got request for help " << std::endl;
                std::cout<<USAGE<<std::endl;
                return 0;
                break;
            default:
                std::cerr <<  "EE: Unexpected programmer error in options parsing\n";
                break;
            }        
            opt = getopt_long( argc, argv, optString, longOpts, &longIndex );
        }

        std::string detsFileName;
        std::string pairsFileName;
        std::string outFileName;

        try {
            detsFileName = argv[optind];
            detsFile.open(detsFileName.c_str());
            if (!detsFile.is_open()) {
                std::cerr << "EE: Failed to open dets file " << detsFileName << " - does this file exist?" 
                          << std::endl;
                return 1;
            }
            optind++;
            pairsFileName = argv[optind];
            pairsFile.open(pairsFileName.c_str());
            if (!pairsFile.is_open()) {
                std::cerr << "EE: Failed to open pairs file " << pairsFileName << " - does this file exist?" 
                          << std::endl;
                return 1;
            }
            optind++;
            for (int i = optind; i < optind + 4; i++) {
                double tmp = 0.;
                std::stringstream ss(argv[i]);
                ss >> tmp;
                tolerances.push_back(tmp);
            }
            optind += 4;
            outFileName = argv[optind];
            outFile.open(outFileName.c_str());
            if (!outFile.is_open()) {
                std::cerr << "EE: Failed to open output file " << outFileName << " - do you have write permissions?" 
                          << std::endl;
                return 1;
            }
        }
        catch (...) {
            std::cerr << "ERROR: failed to open one of the specified files." << std::endl;
            std::cerr << USAGE << std::endl;
            exit(1);
        }



        std::cout << "collapseTracklets: " << std::endl << "--------------------------------" << std::endl;
        std::cout << "Method:            ";
	if (useMinimumRMS) {
	  std::cout << "minimum RMS";
	}
	if (useGreedy) { 
	  std::cout << "greedy" ;
	}
	if (useBestFit) {
	  std::cout << "best fit";
	}
	std::cout << std::endl;
        if (useRMSFilt == true) {
            std::cout << "enforce min. RMS:  true" << std::endl;
            std::cout << "maximum RMS:       " << maxRMSm << "* tracklet av. mag + " << maxRMSb 
                      << std::endl;
        }
        std::cout << "Dets File:         " << detsFileName << std::endl;
        std::cout << "Pairs File         " << pairsFileName << std::endl;
        std::cout << "Output File:       " << outFileName << std::endl;
        std::cout << "Tolerances: " << std::endl;
        std::cout << "RA0: " << tolerances[0] << ", Dec0: " << tolerances[1] << ", angle: " << 
            tolerances[2] << ", velocity: " << tolerances[3] << std::endl;


        /* read data from files, do collapsing if data supports it, write
         * output */

        try {
            TrackletCollapser myTC;
            myTC.populateDetVectorFromFile(detsFile, detections);
            myTC.populatePairsVectorFromFile(pairsFile, pairs);
	  // TBD: use proper LSST logs, too...
	  std::cout << "Found " << detections.size() << " detections\n";
	  std::cout << "Found " << pairs.size() << " pairs.\n";
          
          if (!myTC.isSane(detections.size(), &pairs)) {
              std::cerr << "EE: Pairs file does not seem to correspond with detections file.  Exiting." 
                        << std::endl;
              return 3;
          }
          

	  if ((detectionsHaveGT2UniqueTimes(&detections)) && (pairs.size() > 1)) {
	    std::cout << "Doing the collapsing..." << std::endl;
	    myTC.doCollapsingPopulateOutputVector(&detections, pairs, tolerances, collapsedPairs, 
                                                  useMinimumRMS, useBestFit, useRMSFilt, maxRMSm, maxRMSb, beVerbose);
	    std::cout << std::endl << "Done!" << std::endl;

	  }
          else {
              /* no need to do any collapsing, there isn't enough data! */
              collapsedPairs = pairs;
          }
	  std::cout << "Writing the output to file..." << std::endl;
          /* write the output */
          /*collapsedPairs = removeSubsets(&collapsedPairs);  for now, do this in a separate program.*/
          myTC.writeTrackletsToOutFile(&collapsedPairs, outFile);
          /* close all files. */
	  std::cout << "Done!" << std::endl;
          detsFile.close();
          pairsFile.close();
          outFile.close();
	}
        catch (char* e) {
	  // TBD: use real exceptions... or don't catch at all, punt back to framework.
	  std::cerr << e << std::endl;
	  return -1;
        }
        return 0;
    }
  

}


