RMMTracker.h

#ifndef RMMTRACKER_H
#define RMMTRACKER_H

#include <QDomNode>
#include "Datapool.h"
#include "src/ReliabilityMerge.h"
#include "RMMHypothesisSetList.h"
#include "src/blob.h"
#include "src/TimeStamp.h"
#include "src/calibration.h"
#include "src/sharedTrackingClasses.h"
#include "src/groundtruth.h"

//#define RMTT_OUTPUT

class bestHypothesesNode;

typedef QSharedPointer<bestHypothesesNode> SpBestHypothesesNode;

class bestHypothesesNode {
  public:
    static int numVariables;
    static int variablesSum;
    static int *variablesNumFrames;

    static std::set<SpRMMMobileObject, RMMorderedByBestCoherenceOperator>::iterator *newObjectsListEnds;

    double value;
    int mobileIndex;
    int *versionIndex;
    bool added;
    std::set<SpRMMMobileObject, RMMorderedByBestCoherenceOperator>::iterator *mobileIterators;
    double *variableContribution;

    bestHypothesesNode(double i_value, int i_index);
    bestHypothesesNode(SpBestHypothesesNode toCopy, int indexToMove);
    ~bestHypothesesNode();

    void setNextHypothesisValue(int index);

};

struct orderedByBestHypothesisProbabilityCooperationOperator {
    bool operator()(SpBestHypothesesNode altNode1, SpBestHypothesesNode altNode2);
};


/*class IdBlobPair {
  public:
    long id;
    Blob *blob;

    IdBlobPair(long i_id, Blob *i_blob);
    ~IdBlobPair();
    bool operator<(IdBlobPair &P);
};*/



class RMMTracker {

public:
    Datapool *m_data;
    //Template for building the hierarchy by operator=
    ReliabilityMultiModelTemplate m_multiModelTemplate;
    ReliabilityMerge *m_RMerge;
    ReliabilityMerge *m_PreMerge;
    //ReliabilityClassification *m_rclassif;
    SceneModel* smodel;
    QImage *m_pSegmentation;
    bool activatePreMerge;
    double maxObjectSpeed;
//    std::map<ObjectType, SpModelInterface> *objectModels;

    int m_MultiModelBufferSize;
    double meanw, meanl, meanh;
    double hypothesesProbabilityThreshold;


    int m_maximumGeneratedHypothesesPerMobile;

    int m_maximumRetainedHypotheses;

    int maximumMobilePaths;

    int m_maximumHypothesesPerMobile;

    QString eliminationStrategy;
  std::deque<IdBlobPair> biggestBlobForNewMobile;
  std::set<long int> usedIdsByHypothesis;
  std::map< long int, std::set<SpRMMMobileObject, RMMorderedByBestCoherenceOperator> > mobileAlternativesMap;
  std::map< int, std::deque<long int> > mobileIdsUsingABlob;

  RMMHypothesisSetList hypothesisSets;

  std::map<long int, SpRMMMobileObject> mobilesOutList;

    bool parametersInitialised;

    QString m_MultiModelConfig;


    static int m_hypothesisNumber;
    static bool m_newOuts;
    static int acceptedPixelError;
    static int acceptedPixelDistance;
    static double accepted3DFeatureError;
    static double acceptedOrientationError;
    static double m_highVisualSupportThreshold;
    static int m_meanMillisecondsDifferenceBetweenFrames;
    int lastMilliSecondsDifference;

    //PARAMETERS:
    double CoherenceReliabilityThreshold;
    double CoherenceProbabilityThreshold;

    double IgnoreByDimensionalCoherenceThreshold;
    double IgnoreByVelocityCoherenceThreshold;

    double ImportanceRateForBestMobileHypothesisProbability;

    double m_mobile2DCoverageRateToConsiderEqual;

    bool m_internalOutputActivated;
    bool m_reducedOutputActivated;

    double m_lowVisualSupportThreshold;
    double m_blobCompletellySupportedThreshold;

    double m_InvolvedVisualEvidenceMultiplyingFactor;
    int m_InvolvedVisualEvidenceSumFactor;
    double m_InvolvedVisualEvidenceMinIntersectionRatio;

    bool m_setObjectLog;
    QString m_objectLogRootName;
    QString m_objectLogModelName;
    bool m_logModelFound;
    bool m_logFirstTime;
    std::map<unsigned long, QString> logFileNames;

    bool m_useKalman;
    int m_kalmanMaxPixelError;

    int *g_intersectionAreas;

    Blob **blobsVector;
    bool *usedBlobs;
    int *involvedHypothesisSetsCounter;
    bool **initialMergeMap;
    int *initialGroups;
    bool initialPreparation;

    bool *checkedMobilePairValidity, *validMobilePair;

    int blobsNumber;

    unsigned long mobile_id_counter;
    unsigned long hset_id_counter;
    int currentFrameNumber;
    unsigned long currentTimeMilliSeconds;
    TimeStamp lastTimeStamp;

    //Data used to generate the alternative paths for a mobile
    int g_inserted_for_hypothesis, g_NumCurrentlyLost;
    SpRMMHypothesis g_baseHypothesis;

    std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> g_newHypotheses;
    std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> g_completeHypotheses;
    std::deque<Blob *> g_includedBlobsInNewMobiles;
    std::deque<Blob *> g_allocatedBlobs;
    double g_bestGlobalP;
    int g_numberOfNewMobiles, g_NumLocallyAddedHypotheses, *g_numberOfNewMobileVersions, *g_mobileVersionIndex, g_totalNumberOfMobileVersions;
    std::set<SpRMMMobileObject, RMMorderedByBestCoherenceOperator>::iterator *g_mobileIterators, *g_setBegins;
    std::set<SpBestHypothesesNode, orderedByBestHypothesisProbabilityCooperationOperator> g_leaves;

    std::map<long int, std::set<SpRMMMobileObject, RMMorderedByBestCoherenceOperator> > *g_newObjectsList;
    double *g_variableContribution;

    double *distanceBetweenBlobs;

    bool m_activatePauseOnLost, m_pauseOnLost;
    bool m_activatePauseOnNew, m_pauseOnNew;

    //Trajectories log file
    std::set<unsigned long> chosen_object_trajectories;
    bool m_logTrajectories;
    bool m_logTrajectoriesById;
    QString m_logTrajectoryFileName;
    std::map<unsigned long, simplifiedObject> simplifiedObjects;

    RMMTracker(Datapool *i_data);
    ~RMMTracker();

    //Write chosen object trajectories
    void writeObjectTrajectories();

    //Reset pause indicators.
    void resetPauseCriterias();

    void activatePause();

    //Implements default multimodel hierarchy if failed to extract multi model  from xml configuration file
    void setDefaultMultiModelTemplate();

    //Sets the buffer sizes according to the maximal buffer defined for a submodel:
    void setBufferSize();
    void setBufferSize(int &max, std::vector<SpReliabilitySingleModelInterface> &models);

    QString initObjectLog(long object_id);
    void writeObjectLog();

    void setRMMMobileObjectTemplates();

    bool setParameters(QDomNode& config);
    bool processModelParameters(QString& filename);

    bool init();

    bool initialPrepareRun();

    bool prepareRun();

    void initStaticMobileObject();

    void getMostLikelyMobileObjects(std::deque<SpRMMMobileObject> &mobileObjectsOutput);

    void preMerge(std::vector<Blob>&);

    void update();

    void eliminateUnlikelyMobiles();

    void run(std::vector<Blob>&);

    void followExistingMobiles();

    void setInvolvedBlobs();

    void createMobilePossibilities();

    void mergeUsedHypothesisSets();

    void mergeConflictingHypothesisSets(SpRMMHypothesisSet first, SpRMMHypothesisSet second);

    void insertNewMobiles(std::deque<SpRMMHypothesisSet> &newHSets);
    void insertNewMobiles(SpRMMHypothesis hypothesis, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &newHypotheses);

    void setBlobsVector(std::vector<Blob>&);

    void freeBlobsVector();

    void presetBlobsVectorAndInitialMergeTable(std::vector<Blob>&);

    void freeBlobsVectorAndInitialMergeTable();


    int setGroups(int elementsVector[], bool **relations, int elementsNumber, int *elementsToAnalyzeVector);

    void setGroup(int elementsVector[], bool **relations, int elementsNumber, int groupId, int startPoint, int referencePoint, int *elementsToAnalyzeVector);


    int setNewGroups(int elementsVector[], int elementsNumber, std::vector<BoundingBox> &elementsToAnalyzeVector);

    void setNewGroup(int groupsVector[], int elementsNumber, int groupId, int startPoint, int referencePoint, std::vector<BoundingBox> &elementsToAnalyzeVector);

    bool boundingBoxesRelated(BoundingBox &b1, BoundingBox &b2);

    void generateNewMobiles(int bboxNumberToAnalyze, std::vector<BoundingBox> &bboxesToAnalyze, int groupsNumber, int *blobGroupVector,
                            std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hyps);
    void generateNewMobiles(int bboxNumberToAnalyze, std::vector<BoundingBox> &bboxesToAnalyze, int groupsNumber, int *blobGroupVector,
                            std::deque<SpRMMHypothesisSet> &newHSets);

    void generateHypothesesForEvidences(std::vector<BoundingBox> &bboxesToMerge, int bboxesToMergeNumber,
                                        std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hyps);

    void recursiveComboGenerator(int position, int value, int *hypothesesCombo, std::vector<BoundingBox> &bboxesToMerge, int bboxesToMergeNumber,
                                 std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hyps);

    bool validBboxMergeConfiguration(int mergeGroupNumber, int mergeLength, int bboxesToMergeNumber, std::vector<BoundingBox> &bboxesToMerge, int *hypothesesCombo);

    void processMergeVector(int *hypothesesCombo, std::vector<BoundingBox> &bboxesToMerge, int bboxesToMergeNumber,
                            std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hyps);

    void checkConnectivity(bool groupVector[], int elementsNumber, int referencePoint, int *elementsToAnalyzeVector, std::vector<BoundingBox> &bboxesToMerge);

    void insertNewMergeSolution(BoundingBox mergedBboxes[], int finalBboxesNumber, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hyps);

    void setCurrentTimeAndFrame();

    void getMergeConnections(bool *listToUpdate, int startingBlobIndex);

    void getRecursiveMergeConnections(bool *mergeList, int blobIndex);

    //Quicksort
    void orderAscending(int *array, int top, int bottom);
    int getMiddle(int *array, int top, int bottom);

    void filterUnseenMobiles();

    void separateReliableHypotheses();

    void determineInvolvedBlobsForMobileObject(SpRMMMobileObject mobile);

    bool combinationAlreadyIncluded(BoundingBox mergedBboxes[], int bboxNumberForHypothesis);

    int getMobileId();

    bool checkDataPackDifference(SpReliabilitySingleModelInterface m1, SpReliabilitySingleModelInterface m2);

    void getNewHypothesesFromBoundingBox(BoundingBox &bbox, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &newHypotheses);
    void getHypothesesForExistingMobile(SpRMMMobileObject mobile, BoundingBox &bbox, DetectionProblemType dtype, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &newHypotheses);
    void getNewHypothesesFromMobile(SpRMMMobileObject mobile, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &newHypotheses);
    void setInitialHypothesisSet(SpRMMMobileObject mobile, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &newHypotheses);
    ModelInstanceGeneralPack *iterateMultiModel(int modelIndex, SpRMMMobileObject mobile, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &hypotheses);
    void generateNewHypotheses(SpRMMMobileObject mobile, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &hypotheses);

    void regenHypotheses(std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hypotheses,
                         std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> hyps[],
                         SpRMMMobileObject mobiles[], int dsize);
    void regenHypotheses(std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hypotheses,
                         std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> hyps[], int dsize);

    ModelInstanceGeneralPack *combineHypotheses(std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hypotheses,
                           int modelIndex, SpRMMMobileObject mobile, int dsize,
                           std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> hyps[],
                           ModelInstanceGeneralPack *post[]);
    void checkRepeatedObject(std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &hyps, SpRMMMobileObject m, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hyps);

    bool equalHypotheses(unsigned long current_mobile_id, SpRMMHypothesis h1, SpRMMHypothesis h2);
    bool equalHypotheses(SpRMMHypothesis h1, SpRMMHypothesis h2);

    double segmentationSimilarity(ModelInstanceGeneralPack &m1, ModelInstanceGeneralPack &m2);
    void genROIFromFg(cv::Mat &image_roi, BoundingBox &roi);

    BoundingBox getMergedBoundingBox(int *mergeSequence, int sequenceLength, std::vector<BoundingBox> &bboxesToMerge);

    double areaOfIntersection(SpRMMMobileObject m1, SpRMMMobileObject m2);

    int getPairIndex(int mindex, int vindex);

    bool mobilePairValidityChecked(int index1, int index2);

    bool mobilePairIsValid(int index1, int index2);

    //It can be eliminated or reused
    bool checkMobilePairValidity(SpRMMMobileObject m1, int mindex1, int vindex1, SpRMMMobileObject m2, int mindex2, int vindex2);

    bool setPairValidityAndGo(int index1, int index2, bool ret_value);

    bool blobAlreadyIncludedInNewMobilesSet(Blob *blob);

    bool equalMobiles(SpRMMHypothesis alt1, SpRMMHypothesis alt2);
    bool equalMobiles(SpRMMMobileObject m1, SpRMMMobileObject m2);

    bool sameUsed(SpRMMMobileObject m1, SpRMMMobileObject m2);

    void filterContainedStartingMobiles();

    void filterEquallyConvergedMobiles();

    bool highCoverage(SpRMMMobileObject m1, SpRMMMobileObject m2);

    void filterRepeatedHypotheses();

    std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> generateBestPossiblePathsForMobiles(SpRMMHypothesis hypothesis);

    std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> generateHypothesesFromInitialBlob(
            SpRMMMobileObject currentMobile, std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> newHypotheses,
            Blob *initialBlob, int numUsed, bool *usedBlobs, double *blobSupport, Rectangle<int> &mobileBBox);

    void generateMobilePathFromInitialBlob(SpRMMMobileObject mobile, Blob *initialBlob, int numUsed, bool *usedBlobs, double *blobSupport,
                                           std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &newHypotheses);

    void addNewHypotheses(unsigned long current_mobile_id,
                          std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &current_hypotheses,
                          std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hypotheses);
    void addNewHypotheses(std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &current_hypotheses,
                          std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &new_hypotheses);

    void generateHypothesesForMobilePathFromInitialBlob(int length, int position, int value, int *hypothesesCombo,
                                                        int blobsToMergeNumber, int *blobsToMerge, SpRMMMobileObject mobile,
                                                        Blob *initialBlob, int numUsed, bool *usedBlobs,
                                                        std::set<SpRMMHypothesis, hypothesesOrderedByBestProbabilityOperator> &newHypotheses);

    bool blobCanBeIncludedForMerge(int currentMergeLength, int *mergedBlobIndexes, int currentBlobIndex);


};

typedef QSharedPointer<RMMTracker> SpRMMTracker;


#endif // RMMTRACKER_H