/*

 * ex10.cpp

 *

 *

 *  Author: Daniel Rehfeldt

 *

 *  Compile: g++ ex10.cpp -o ex10 -O3 -march=native -std=c++14 -lboost_timer  -lboost_system  -pedantic -Wall
 *
 *  Compile with OpenMP:  g++ ex10.cpp -o ex10 -O3 -march=native -std=c++14 -lboost_timer  -lboost_system  -pedantic -Wall -fopenmp
 *
 *
 */

#define NDEBUG
#define SEQHEAP

#include <limits>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <cassert>
#include <memory>
#include <string>
#include <array>
#include <algorithm>
#include <boost/spirit/include/qi.hpp>
#include <boost/timer/timer.hpp>
#ifdef SEQHEAP
#include "knheap.C"
#else
#include "binheap.h"
#endif
#ifdef _OPENMP
#include <omp.h>
#endif

using Weight = long long;

struct IntInt
{
      int index;
      int prededge;
};

class Graph{

    public:
        int* deg = nullptr;
        int* term = nullptr;
        int* head = nullptr;
        int* tail = nullptr;
        int* headcprs = nullptr;
        int* tailcprs = nullptr;
        int* startcprs = nullptr;
        int* weightcprs = nullptr;
        int* orgedgecprs = nullptr;
        Weight* edgeweight = nullptr;
        Weight* edgeweightcprs = nullptr;

        int nterms = -1;
        int nnodes = -1;
        int nedges = -1;
        int nmaxedges = -1;

        Graph(int nodes, int maxedges)
        {
            assert(nodes >= 1);
            assert(maxedges >= 0);
            nedges = 0;
            nterms = 0;
            nnodes = nodes;
            nmaxedges = maxedges;

            if( maxedges > 0 )
            {
               // initialize with 0
               deg = new int[nnodes]();
               term = new int[nnodes]();

               head = new int[maxedges];
               tail = new int[maxedges];
               edgeweight = new Weight[maxedges];
            }
        }

        bool isTerm(int i) const;

        void makeTerm(int i);

        void addEdge(int etail, int ehead, Weight eweight);

        // initialize compressed data structures
        void initCompressed();

        // check whether compression was successful
        bool compressedValid() const;

        bool isConnected() const;

        ~Graph()
        {
           delete[] orgedgecprs;
           delete[] weightcprs;
           delete[] edgeweightcprs;
           delete[] headcprs;
           delete[] tailcprs;
           delete[] startcprs;
           delete[] edgeweight;
           delete[] deg;
           delete[] head;
           delete[] tail;
        }
};

bool Graph::isConnected() const
{
     assert(startcprs);
     assert(headcprs);

     bool* visited = new bool[nnodes];
     int* stack = new int[nnodes];

     int size = 0;

     std::fill_n(visited, nnodes, false);

     stack[size++] = 0;
     visited[0] = true;

     int nodecount = 0;

     // DFS
     while( size )
     {
        const int k = stack[--size];
        nodecount++;

        //traverse adjacent edges of k
        const int end = startcprs[k + 1];
        for( int e = startcprs[k]; e != end; e++ )
        {
            const int head = headcprs[e];
            if( !visited[head] )
            {
               visited[head] = true;
               stack[size++] = head;
            }
        }
     }

     delete[] visited;
     delete[] stack;

     if( nodecount != nnodes )
         return false;

     return true;
}

inline bool Graph::isTerm(
     int i
) const
{
   assert(i >= 0 && i < nnodes);
   assert(term);

   return (term[i] != 0);
}

inline void Graph::makeTerm(
    int i
)
{
   assert(i >= 0 && i < nnodes);
   assert(term);

   if( !term[i] )
   {
      nterms++;
      term[i] = 1;
   }
}

inline void Graph::addEdge(
    int etail,
    int ehead,
    Weight eweight
)
{
    assert(ehead >= 0 && ehead < nnodes );
    assert(etail >= 0 && etail < nnodes );
    assert(nedges < nmaxedges);
    assert(eweight >= Weight(0));

    head[nedges] = ehead;
    tail[nedges] = etail;
    edgeweight[nedges++] = eweight;
    deg[ehead]++;
    deg[etail]++;
}

void Graph::initCompressed()
{
    tailcprs = new int[2 * nedges];
    headcprs = new int[2 * nedges];
    orgedgecprs = new int[2 * nedges];
    startcprs = new int[nnodes + 1];
    edgeweightcprs = new Weight[2 * nedges];

    startcprs[0] = deg[0];

    for( int i = 1; i < nnodes; i++ )
        startcprs[i] = startcprs[i - 1] + deg[i];

    assert(startcprs[nnodes - 1] == 2 * nedges);

    startcprs[nnodes] = 2 * nedges;

    auto update_cprs = [this](int node, int adjnode, int edge)
    {
        const int e = --startcprs[node];
        assert(e >= 0);
        assert(node == 0 || e >= startcprs[node - 1]);
        edgeweightcprs[e] = edgeweight[edge];
        tailcprs[e] = node;
        headcprs[e] = adjnode;
        orgedgecprs[e] = edge;
    };

    for( int e = 0; e < nedges; e++ )
    {
        update_cprs(tail[e], head[e], e);
        update_cprs(head[e], tail[e], e);
    }

    assert(startcprs[nnodes - 1] + deg[nnodes - 1] == 2 * nedges);
}

bool Graph::compressedValid() const
{
   if( !startcprs || !edgeweightcprs || !tailcprs || !headcprs || !orgedgecprs )
      return false;

   auto is_adjacent = [this](int node, int nodeadj)
   {
      for( int e = startcprs[node], end = startcprs[node + 1]; e != end; e++ )
         if( tailcprs[e] != node )
            return false;

      for( int e = startcprs[node], end = startcprs[node + 1]; e != end; e++ )
         if( headcprs[e] == nodeadj )
            return true;

      return false;
   };

   for( int e = 0; e < nedges; e++ )
       if( !is_adjacent(tail[e], head[e]) || !is_adjacent(head[e], tail[e]) )
          return false;

   return true;
}

// fills array that marks whether edge i is part of the tree (steineredge[i] = true) or not (steineredge[i] = false)
static std::unique_ptr<bool[]> computeSteinerTree(
    const Graph& graph,
    const int root
)
{
    const int nnodes = graph.nnodes;
    const int nedges = graph.nedges;
    const int nterms = graph.nterms;

    assert(nnodes >= 1);
    assert(nedges >= 1);
    assert(nterms >= 1);
    assert(root >= 0 && root < nnodes);

    auto steineredge = std::make_unique<bool[]>(nedges);

    if( nterms == 1 && graph.isTerm(root) )
       return move(steineredge);

    const int* orgarr = graph.orgedgecprs;
    const int* tailarr = graph.tailcprs;
    const int* headarr = graph.headcprs;
    const int* startarr = graph.startcprs;
    const Weight* weightarr = graph.edgeweightcprs;

    assert(orgarr);
    assert(tailarr);
    assert(startarr);
    assert(headarr);
    assert(weightarr);

    // predecessor and distance array to store incoming edge and distance for each node todo unique pointer
    bool* steinernode = new bool[nnodes];
    int* predarr = new int[nnodes];
    Weight* distarr = new Weight[nnodes];

    // initialize array for Dijkstra calculations
    std::fill_n(distarr, nnodes, std::numeric_limits<Weight>::max());
    std::fill_n(predarr, nnodes, -1);
    std::fill_n(steinernode, nnodes, false);

#ifdef SEQHEAP
    KNHeap<Weight, IntInt> heap(std::numeric_limits<Weight>::max(), std::numeric_limits<Weight>::min());
#else
    BinHeap<Weight, IntInt> heap(2 * nedges);
#endif

    assert(heap.getSize() == 0);

    IntInt dummy;
    dummy.index = root;
    dummy.prededge  = -1;
    heap.insert(Weight(0), dummy);

    distarr[root] = Weight(0);
    steinernode[root] = true;

    int termcount = 0;
    if( graph.isTerm(root) )
       termcount++;

    while( heap.getSize() != 0 )
    {
        Weight distk;

#ifdef SEQHEAP
        heap.deleteMin(&distk, &dummy);
#else
        heap.deleteMin(distk, dummy);
#endif

        assert(distk >= Weight(0));

        const int node = dummy.index;

        assert(node >= 0);

        if( distarr[node] >= distk )
        {
           distarr[node] = distk;
           predarr[node] = dummy.prededge;

           if( graph.isTerm(node) && !steinernode[node] )
           {
              int k = node;

              // add path to current tree
              do
              {
                 int pred;
                 steinernode[k] = true;
                 pred = predarr[k];

                 assert(pred >= 0);
                 assert(headarr[pred] == k);

                 steineredge[orgarr[pred]] = true;
                 dummy.index = k;
                 dummy.prededge = pred;

                 distarr[k] = Weight(0);

                 heap.insert(Weight(0), dummy);

                 k = tailarr[pred];

              }
              while( !steinernode[k] );

              // all terminals connected?
              if( ++termcount == nterms )
                 break;
           }

           // traverse adjacent edges of node
           const int end = startarr[node + 1];
           for( int e = startarr[node]; e != end; e++ )
           {
               const int head = headarr[e];

               if( distarr[head] > distk + weightarr[e] )
               {
                  // update
                  distarr[head] = distk + weightarr[e];
                  dummy.index = head;
                  dummy.prededge = e;
                  heap.insert(distarr[head], dummy);
                  //heap.insert(distarr[head], e);
               }
           }
        }
    }

    delete[] steinernode;
    delete[] distarr;
    delete[] predarr;

    return move(steineredge);
}

static bool isTreeValid(
      const Graph& graph,
      bool* steinertree,
      const int root
)
{
   assert(steinertree);

   const int nnodes = graph.nnodes;
   const int nedges = graph.nedges;
   const int nterms = graph.nterms;
   const int* orgarr = graph.orgedgecprs;
   const int* headarr = graph.headcprs;
   const int* startarr = graph.startcprs;

   assert(orgarr);
   assert(startarr);
   assert(headarr);

   bool* visited = new bool[nnodes];
   int* stack = new int[nnodes];

   int size = 0;
   int nsolnodes = 0;
   int termcount = 0;

   std::fill_n(visited, nnodes, false);

   stack[size++] = root;
   visited[root] = true;

   // DFS
   while( size )
   {
      const int k = stack[--size];

      nsolnodes++;

      if( graph.isTerm(k) )
         termcount++;

      //traverse adjacent edges of k
      const int end = startarr[k + 1];
      for( int e = startarr[k]; e != end; e++ )
      {
          const int head = headarr[e];
          if( !visited[head] && steinertree[orgarr[e]] )
          {
             visited[head] = true;
             stack[size++] = head;
          }
      }
   }

   delete[] visited;
   delete[] stack;

   int nsoledges = 0;

   for( int e = 0; e < nedges; e++ )
      if( steinertree[e] )
         nsoledges++;

   if( nsoledges != nsolnodes - 1 )
   {
       std::cout << "not a tree "  << std::endl;
       return false;
   }

   if( termcount != nterms )
   {
       std::cout << "not all terminals reached " << termcount << " < " << nterms <<  std::endl;
       return false;
   }

   return true;
}

// adds prime vertices as terminals
static void addTermsPrime(
      Graph& graph
)
{
   const int nnodes = graph.nnodes;

   // Sieve's prime algorithm

   bool* primecands = new bool[nnodes + 1];

   std::fill_n(primecands, nnodes + 1, true);

   const int sqrn = sqrt(nnodes);

   for( int k = 4; k <= nnodes; k += 2 )
      primecands[k] = false;

   for( int i = 3; i <= sqrn; i += 2 )
      if( primecands[i] )
      {
         const int currprime = i;

         for( int k = currprime * currprime; k <= nnodes; k += 2 * currprime )
            primecands[k] = false;
      }

   if( nnodes >= 2 )
      graph.makeTerm(1);

   for( int i = 3; i <= nnodes; i += 2 )
      if( primecands[i] )
         graph.makeTerm(i - 1);

   delete[] primecands;

}

static Graph loadGraph(
   const char* filename
)
{
    std::ifstream file (filename, std::ifstream::in);

    if( file.fail() )
    {
       std::cerr << "file could not be opened" << std::endl;
       exit(EXIT_FAILURE);
    }

    int nnodes;
    int nedges;

    if( !(file >> nnodes >> nedges) || nedges <= 0 || nnodes <= 1 )
    {
        std::cerr << "file syntax error" << std::endl;
        exit(EXIT_FAILURE);
    }

    Graph graph(nnodes, nedges);

    std::string strline;

    using namespace boost::spirit;
    using qi::int_;
    using qi::phrase_parse;
    using ascii::space;

    int etail, ehead;
    Weight eweight;

    while( std::getline(file, strline) )
    {
        auto it = strline.begin();

        bool success = phrase_parse(it, strline.end(),
                 int_[([&etail](int i){ etail = i; })] >> int_[([&ehead](int i){ ehead = i; })] >> int_[([&eweight](Weight i){ eweight = Weight(i); })]
                          , space);

        if( success && it == strline.end() )
           graph.addEdge(etail - 1, ehead - 1, eweight);
    }

    file.close();

    std::cout << "loaded graph;" << " nodes: " << nnodes << ", edges: " << nedges  << " ... ";

    return graph;
}

int main(
    int argc,
    char* argv[]
)
{
    // check number of parameters
    if( argc != 3 )
    {
       // tell the user how to run the program
       std::cerr << "Usage: " << argv[0] << " filename " << "number_of_starting_nodes"  << std::endl;
       exit(EXIT_FAILURE);
    }

    const int nstarts = std::stoi(argv[2]);

    if( nstarts <= 0 )
    {
       std::cout << "number of starting nodes should be >= 1" << std::endl;
       exit(EXIT_FAILURE);
    }

    Graph graph = loadGraph(argv[1]);
    addTermsPrime(graph);

    std::cout << "terminals: " << graph.nterms << std::endl;

    graph.initCompressed();
    assert(graph.compressedValid());

    if( !graph.isConnected() )
    {
       std::cout << "graph unconnected" << std::endl;
       exit(EXIT_FAILURE);
    }

    const int nedges = graph.nedges;
    const int nruns = std::min(nstarts, graph.nterms);

    bool* incumbent(new bool[nedges]);
    int* startterms(new int[graph.nnodes]);

    for( int i = 0, t = 0; t < nruns; i++ )
       if( graph.isTerm(i) )
          startterms[t++] = i;

    int bestroot = -1;
    Weight bestobj = std::numeric_limits<Weight>::max();

    using namespace boost::timer;
    cpu_timer timer;

    // run heuristic from first nruns many terminals (prime nodes)
    #pragma omp parallel for
    for( int r = 0; r < nruns; r++ )
    {
       const int root = startterms[r];

       assert(graph.isTerm(root));

       std::unique_ptr<bool[]> steinertree = computeSteinerTree(graph, root);

       assert(isTreeValid(graph, steinertree, root));

       Weight obj = Weight(0);
       for( int e = 0; e < nedges; e++ )
          if( steinertree[e] )
             obj += graph.edgeweight[e];

#ifdef _OPENMP
#pragma omp critical
       {
          int t = omp_get_thread_num();
          int tcount = omp_get_num_threads();
          std::cout << "thread " << t  << " of " << tcount << " finished" << std::endl;
       }
#endif

       #pragma omp critical
       if( obj < bestobj )
       {
          bestobj = obj;
          bestroot = root;
          for( int i = 0; i < nedges; i++ )
             incumbent[i] = steinertree[i];
       }
    }

    cpu_times times = timer.elapsed();

    delete[] startterms;

    assert(bestroot >= 0);

    if( !isTreeValid(graph, incumbent, bestroot) )
       std::cout << "FAILED WITH FINAL TREE" << std::endl;
    else
       std::cout << "FINAL TREE OK!" << std::endl;

    delete[] incumbent;
    assert(bestobj < std::numeric_limits<Weight>::max());

    std::cout << "TLEN: " << bestobj << std::endl;
    std::cout << "TIME: "  << times.user / 1e9 << " s" << std::endl;
    std::cout << "WALL: "  << times.wall / 1e9 << " s" << std::endl;

    return EXIT_SUCCESS;
}


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file suffix_array.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-08-29
 */
#ifndef PAAL_SUFFIX_ARRAY_HPP
#define PAAL_SUFFIX_ARRAY_HPP

#include "paal/utils/irange.hpp"

#include <boost/range/algorithm/fill.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/numeric.hpp>

/*
 * algorithm from:
 *
 * http://www.cs.cmu.edu/~guyb/realworld/papersS04/KaSa03.pdf
 *
 */

namespace paal {

namespace detail {
/**
 * @param a1
 * @param a2
 * @param b1
 * @param b2
 * @brief return true if pair (a1,a2) is smaller than pair (b1,b2) in
 * lexicographic order
 * and false otherwise
 * @tparam Letter
 */
// class suffix_array{
template <typename Letter>
inline bool leq(Letter a1, int a2, Letter b1,
                int b2) { // lexic. order for pairs
    return (a1 < b1 || (a1 == b1 && a2 <= b2));
}
/**
 * @param a1
 * @param a2
 * @param a3
 * @param b1
 * @param b2
 * @param b3
 * @brief return true if triple (a1,a2,a3) is smaller than triple (b1,b2,b3) in
 * lexicographic order
 * and false otherwise
 * @tparam Letter
 */
template <typename Letter>
inline bool leq(Letter a1, Letter a2, int a3, Letter b1, Letter b2, int b3) {
    return (a1 < b1 || (a1 == b1 && leq(a2, a3, b2, b3)));
}

struct radix_pass {

    template <typename Letter>
    radix_pass(int max_letter, const std::vector<Letter> & text) {
        if (max_letter == 0) {
            max_letter = *boost::max_element(text);
        }
        c.resize(max_letter + 2);
    }

    // stably sort sortFrom[0..n-1] to sortTo[0..n-1] with keys in 0..K from r
    template <typename Iterator>
    void operator()(std::vector<int> const &sortFrom,
            std::vector<int> &sortTo, Iterator r, int n) { // count occurrences
        boost::fill(c, 0);
        for (auto i : irange(n)) {
            ++c[r[sortFrom[i]] + 1]; // count occurrences
        }

        boost::partial_sum(c, c.begin());

        for (auto i : irange(n)) {
            sortTo[c[r[sortFrom[i]]]++] = sortFrom[i]; // sort
        }
    }
private:
    std::vector<int> c;
};

/**
 *
 * @brief
 * require text[n]=text[n+1]=text[n+2]=0, n>=2
 * fill suffix_array
 * suffix_array[i] contains the starting position of the i-1'th smallest suffix
* in Word
 * @tparam Letter
 * @param text - text
 * @param SA place for suffix_array
 * @param max_letter optional parameter max_letter in alphabet
 */
// find the suffix array SA of text[0..n-1] in {1..max_letter}^n
template <typename Letter>
void suffix_array(std::vector<Letter> &text, std::vector<int> &SA,
                   Letter max_letter) {
    int n = text.size() - 3;
    assert(n >= 0);
    int n0 = (n + 2) / 3, n1 = (n + 1) / 3, n2 = n / 3, n02 = n0 + n2;
    text.resize(text.size() + 3);
    std::vector<int> text12;
    std::vector<int> SA12;
    std::vector<int> text0;
    std::vector<int> SA0;
    radix_pass radix{max_letter, text};
    // generate positions of mod 1 and mod  2 suffixes
    // the "+(n0-n1)" adds a dummy mod 1 suffix if n%3 == 1
    for (auto i : irange(n + (n0 - n1))) {
        if (i % 3 != 0) {
            text12.push_back(i);
        }
    }
    SA0.resize(n0);
    SA12.resize(n02 + 3);
    text12.resize(n02 + 3);
    // lsb radix sort the mod 1 and mod 2 triples
    radix(text12, SA12, text.begin() + 2, n02);
    radix(SA12, text12, text.begin() + 1, n02);
    radix(text12, SA12, text.begin(), n02);

    // find lexicographic names of triples
    int name = 0;
    Letter c0 = Letter{}, c1 = Letter{}, c2 = Letter{};
    for (auto i : irange(n02)) {
        if (text[SA12[i]] != c0 || text[SA12[i] + 1] != c1 ||
            text[SA12[i] + 2] != c2 || name == 0) {
            name++;
            c0 = text[SA12[i]];
            c1 = text[SA12[i] + 1];
            c2 = text[SA12[i] + 2];
        }
        if (SA12[i] % 3 == 1) {
            text12[SA12[i] / 3] = name; // left half
        } else {
            text12[SA12[i] / 3 + n0] = name; // right half
        }
    }

    // recurse if names are not yet unique
    if (name < n02) {
        suffix_array<int>(text12, SA12, name); // parametrized by int intentionally
        // store unique names in s12 using the suffix array
        for (auto i : irange(n02)) {
            text12[SA12[i]] = i + 1;
        }
    } else { // generate the suffix array of s12 directly
        for (auto i : irange(n02)) {
            SA12[text12[i] - 1] = i;
        }
    }

    // stably sort the mod 0 suffixes from SA12 by their first character
    for (auto i : irange(n02)) {
        if (SA12[i] < n0) {
            text0.push_back(3 * SA12[i]);
        }
    }
    radix(text0, SA0, text.begin(), n0);
    auto GetI = [&](int t)->int {
        return SA12[t] < n0 ? SA12[t] * 3 + 1 : (SA12[t] - n0) * 3 + 2;
    };

    // merge sorted SA0 suffixes and sorted SA12 suffixes
    auto p = SA0.begin();
    int t = n0 - n1;
    for (auto k = SA.begin(); k < SA.begin() + n; k++) {
        int i = GetI(t); // pos of current offset 12 suffix
        int j = (*p);    // pos of current offset 0  suffix
        if (SA12[t] < n0
                ? leq(text[i], text12[SA12[t] + n0], text[j], text12[j / 3])
                : leq(text[i], text[i + 1], text12[SA12[t] - n0 + 1], text[j],
                      text[j + 1], text12[j / 3 + n0])) { // suffix from SA12 is
                                                          // smaller
            (*k) = i;
            t++;
            if (t == n02) { // done --- only SA0 suffixes left
                k++;
                if (p < SA0.end()) {
                    k = std::copy(p, SA0.end(), k);
                    p = SA0.end();
                }
            }
        } else {
            (*k) = j;
            p++;
            if (p == SA0.end()) { // done --- only SA12 suffixes left
                for (k++; t < n02; t++, k++) {
                    (*k) = GetI(t);
                }
            }
        }
    }
}

}//!detail

/**
 *
 * @brief
 * require text.size()>=2
 * fill suffix_array
 * suffix_array[i] contains the starting position of the i-1'th smallest suffix
* in Word
 * @tparam Letter
 * @param text - text
 * @param SA place for suffix_array
 * @param max_letter optional parameter max_letter in alphabet
 */
// find the suffix array SA of text[0..n-1] in {1..max_letter}^n
template <typename Letter>
void suffix_array(std::vector<Letter> &text, std::vector<int> &SA,
                  Letter max_letter = 0) {
    text.resize(text.size() + 3);
    detail::suffix_array(text, SA, max_letter);
    text.resize(text.size() - 3);
}

} //!paal
#endif // PAAL_SUFFIX_ARRAY_HPP


// Compares two outfiles from phase2, adds appropriate labels
//#include <boost/algorithm/string/replace.hpp>

#include <iostream>
#include <cstdlib>
#include <vector>
#include <algorithm>

#include <TMath.h>
#include <TH1.h>
#include <TH2.h>
#include <TFile.h>
#include <TString.h>
#include <TRegexp.h>
#include <TGraph.h>
#include <TGraphErrors.h>
#include <TMultiGraph.h>
#include <TProfile.h>
#include <TCanvas.h>
#include <TLegend.h>
#include <TSystem.h>
#include <TSystemDirectory.h>
#include <TColor.h>
#include <TStyle.h>
#include <TKey.h>

#include "analysis_params.h"

int nTriggerPtBins = nJetPtBins;
std::vector<double> fTriggerPtBins = jetPtBins;

void set_plot_style() {
    const Int_t NRGBs = 5;
    const Int_t NCont = 255;

    Double_t stops[NRGBs] = { 0.00, 0.34, 0.61, 0.84, 1.00 };
    Double_t red[NRGBs]   = { 0.00, 0.00, 0.87, 1.00, 0.51 };
    Double_t green[NRGBs] = { 0.00, 0.81, 1.00, 0.20, 0.00 };
    Double_t blue[NRGBs]  = { 0.51, 1.00, 0.12, 0.00, 0.00 };
    TColor::CreateGradientColorTable(NRGBs, stops, red, green, blue, NCont);
    gStyle->SetNumberContours(NCont);
}

using namespace std;

struct fileLabel {
  fileLabel(TString _filepath, TString _label): filepath(_filepath),label(_label) {}
  TString filepath;
  TString label;
};

/**
  * Calculates the systematic fractional uncertainty in the tgraphs in the input matrices by using k =1,2, nVersions
  * trials.  Saves them to the parameter output file
  */
void  CalculateSystematicUncertaintyN(vector <vector<TGraphErrors * >> tGraphs, TFile * f){
  // tGraphs expected to be indexed by k = 0,1, .., nVersions, then j = 0,1, .. nJetPtBins
	int nModes = tGraphs.size();

  for (int i = 0; i < nTriggerPtBins; i++) {
    int n;
    TGraph * SysErr;
    n = tGraphs[0][i]->GetN();
    SysErr = new TGraph(n);
    SysErr->SetName(Form("%s_SysUncert",tGraphs[0][i]->GetName()));
    for (int j = 0; j < n; j++) {
      double sumOfSquares = 0;
      double sum = 0;
      double x = 0;
      double y;
      for (int k = 0; k < nModes; k++) {
        y=0;
        tGraphs[k][i]->GetPoint(j,x,y);
        sumOfSquares += y*y;
        sum += y;
      }
			//y = sum / nModes;// New Fix
			tGraphs[0][i]->GetPoint(j,x,y); // Newer Fix, use the first value as the primary value to be reported.
      if (y != 0) SysErr->SetPoint(j,x,(sqrt((sumOfSquares - sum*sum/(nModes))/(nModes-1))/abs(y)));
    }
    f->Add(SysErr);
  }

}

void cleanName(TString &str) {
		str.ReplaceAll(" ","_");
		str.ReplaceAll("%","_");
		str.ReplaceAll("#","_");
		str.ReplaceAll(",","_");
		str.ReplaceAll("-","_");
		str.ReplaceAll("=","_");
}

vector <vector<TGraphErrors * >> extractTGraphArray(TString name, vector<TFile *> * files) {

	int nModes = files->size();
	vector <vector<TGraphErrors * >> tGraphArray;// = new vector <vector<TGraphErrors * >>;

	for (int k = 0; k < nModes; k++) {
		TFile * f1 = files->at(k);

		vector<TGraphErrors *> tGraphSubArray;
		for (int i = 0; i < nTriggerPtBins; i++) {
			TGraphErrors * tGraph = (TGraphErrors *) f1->Get(Form(name,fTriggerPtBins.at(i), fTriggerPtBins.at(i+1)));
			if (!tGraph) {
				fprintf(stderr,"Error: Could not find %s\n",Form(name,fTriggerPtBins.at(i),fTriggerPtBins.at(i+1)));
				exit(1);
			}
//			printf("Found %s in file %s\n",tGraph->GetName(),f1->GetTitle());
			tGraphSubArray.push_back(tGraph);
		}
		tGraphArray.push_back(tGraphSubArray);
	}

	return tGraphArray;
}

const Int_t nTypeList = 9;

void calcSystUncert(vector<fileLabel> *fileLabels, char * fSystematicUncertOutFilePath)  {

  Int_t colorList[nTypeList] = {kBlack,kRed,kOrange-3,kGreen-3,kBlue,kViolet,kMagenta-5,kRed-10,kGray};
  Int_t markerList[nTypeList] = {kFullSquare,kFullCircle,kFullDiamond,kOpenSquare,kOpenCircle,kOpenDiamond,kFullStar,23,34};

  double c_width = 600;
  double c_height = 600;

	TFile * fSystematicUncertOutFile = TFile::Open(fSystematicUncertOutFilePath,"RECREATE");
	if (!fSystematicUncertOutFile) {
		fprintf(stderr,"Error: Unable to open systematic error output file!\n");
		exit(1);
	}

  vector<TFile *> *files = new vector<TFile *>;

  for (int i = 0 ; i < fileLabels->size(); i++ ) {
    fileLabel fl = fileLabels->at(i);
    printf("Opening File %s [%s] ... \n",fl.filepath.Data(),fl.label.Data());
    TFile *f = TFile::Open(fl.filepath.Data(),"READ");
    if (!f) {
      fprintf(stderr,"Error: file %s not found!\nExiting.\n",fl.filepath.Data());
      exit(1);
    }
		TString nameNoSpace  = fl.label;
		cleanName(nameNoSpace);
//		printf("MHO nameNoSpace = %s\n",nameNoSpace.Data());
    f->SetTitle(fl.label.Data());
		f->SetName(nameNoSpace);
    files->push_back(f);

    printf("file %s, title %s\n",f->GetName(),f->GetTitle());

  }

  if ( !files->size()) exit(0);
  TFile *f0 = files->at(0);

  TCanvas *canvas = new TCanvas("canvas","canvas",c_width,c_height);
  gStyle->SetOptStat(0);

	// Need to construct arrays of objects, with an arbitrary first index,
  // second index is jetpt

	int iTrigger = 0; // 0 for jet, 1 for pi0Pt, 2 for pi0Zt

  TString jetClass = "jetPt_%.0f_%.0f";

	TString triggerClass = jetClass;

	if (iTrigger > 0) {
		triggerClass = "pi0Pt_%.0f_%.0f";
		nTriggerPtBins = nPi0PtBins;
		fTriggerPtBins = pi0PtBins;
	}

	vector <vector<TGraphErrors * >> ptBinASIntegralsGraphsEP_0 = extractTGraphArray(triggerClass + "_EP_0_AS_Integrals", files);
	vector <vector<TGraphErrors * >> ptBinASIntegralsGraphsEP_1 = extractTGraphArray(triggerClass + "_EP_1_AS_Integrals", files);
	vector <vector<TGraphErrors * >> ptBinASIntegralsGraphsEP_2 = extractTGraphArray(triggerClass + "_EP_2_AS_Integrals", files);
	vector <vector<TGraphErrors * >> ptBinASIntegralsGraphsEP_3 = extractTGraphArray(triggerClass + "_EP_3_AS_Integrals", files);

	CalculateSystematicUncertaintyN(ptBinASIntegralsGraphsEP_0,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASIntegralsGraphsEP_1,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASIntegralsGraphsEP_2,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASIntegralsGraphsEP_3,fSystematicUncertOutFile);

	vector <vector<TGraphErrors * >> ptBinNSIntegralsGraphsEP_0 = extractTGraphArray(triggerClass + "_EP_0_NS_Integrals", files);
	vector <vector<TGraphErrors * >> ptBinNSIntegralsGraphsEP_1 = extractTGraphArray(triggerClass + "_EP_1_NS_Integrals", files);
	vector <vector<TGraphErrors * >> ptBinNSIntegralsGraphsEP_2 = extractTGraphArray(triggerClass + "_EP_2_NS_Integrals", files);
	vector <vector<TGraphErrors * >> ptBinNSIntegralsGraphsEP_3 = extractTGraphArray(triggerClass + "_EP_3_NS_Integrals", files);

	CalculateSystematicUncertaintyN(ptBinNSIntegralsGraphsEP_0,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinNSIntegralsGraphsEP_1,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinNSIntegralsGraphsEP_2,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinNSIntegralsGraphsEP_3,fSystematicUncertOutFile);

	vector <vector<TGraphErrors * >> ptBinASYieldsGraphsEP_0 = extractTGraphArray(triggerClass + "_EP_0_AS_Yields", files);
	vector <vector<TGraphErrors * >> ptBinASYieldsGraphsEP_1 = extractTGraphArray(triggerClass + "_EP_1_AS_Yields", files);
	vector <vector<TGraphErrors * >> ptBinASYieldsGraphsEP_2 = extractTGraphArray(triggerClass + "_EP_2_AS_Yields", files);
	vector <vector<TGraphErrors * >> ptBinASYieldsGraphsEP_3 = extractTGraphArray(triggerClass + "_EP_3_AS_Yields", files);

	CalculateSystematicUncertaintyN(ptBinASYieldsGraphsEP_0,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASYieldsGraphsEP_1,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASYieldsGraphsEP_2,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASYieldsGraphsEP_3,fSystematicUncertOutFile);

	vector <vector<TGraphErrors * >> ptBinASRmsGraphsEP_0 = extractTGraphArray(triggerClass + "_EP_0_AS_Rms", files);
	vector <vector<TGraphErrors * >> ptBinASRmsGraphsEP_1 = extractTGraphArray(triggerClass + "_EP_1_AS_Rms", files);
	vector <vector<TGraphErrors * >> ptBinASRmsGraphsEP_2 = extractTGraphArray(triggerClass + "_EP_2_AS_Rms", files);
	vector <vector<TGraphErrors * >> ptBinASRmsGraphsEP_3 = extractTGraphArray(triggerClass + "_EP_3_AS_Rms", files);

	CalculateSystematicUncertaintyN(ptBinASRmsGraphsEP_0,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASRmsGraphsEP_1,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASRmsGraphsEP_2,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASRmsGraphsEP_3,fSystematicUncertOutFile);

	vector <vector<TGraphErrors * >> ptBinNSRmsGraphsEP_0 = extractTGraphArray(triggerClass + "_EP_0_NS_Rms", files);
	vector <vector<TGraphErrors * >> ptBinNSRmsGraphsEP_1 = extractTGraphArray(triggerClass + "_EP_1_NS_Rms", files);
	vector <vector<TGraphErrors * >> ptBinNSRmsGraphsEP_2 = extractTGraphArray(triggerClass + "_EP_2_NS_Rms", files);
	vector <vector<TGraphErrors * >> ptBinNSRmsGraphsEP_3 = extractTGraphArray(triggerClass + "_EP_3_NS_Rms", files);

	CalculateSystematicUncertaintyN(ptBinNSRmsGraphsEP_0,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinNSRmsGraphsEP_1,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinNSRmsGraphsEP_2,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinNSRmsGraphsEP_3,fSystematicUncertOutFile);

	vector <vector<TGraphErrors * >> ptBinASWidthsGraphsEP_0 = extractTGraphArray(triggerClass + "_EP_0_AS_Widths", files);
	vector <vector<TGraphErrors * >> ptBinASWidthsGraphsEP_1 = extractTGraphArray(triggerClass + "_EP_1_AS_Widths", files);
	vector <vector<TGraphErrors * >> ptBinASWidthsGraphsEP_2 = extractTGraphArray(triggerClass + "_EP_2_AS_Widths", files);
	vector <vector<TGraphErrors * >> ptBinASWidthsGraphsEP_3 = extractTGraphArray(triggerClass + "_EP_3_AS_Widths", files);

	CalculateSystematicUncertaintyN(ptBinASWidthsGraphsEP_0,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASWidthsGraphsEP_1,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASWidthsGraphsEP_2,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(ptBinASWidthsGraphsEP_3,fSystematicUncertOutFile);

	// Now doing the Ratios
	vector <vector<TGraphErrors * >> OutOverIn_AS = extractTGraphArray("OutOverIn_AS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> MidOverIn_AS = extractTGraphArray("MidOverIn_AS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> OutOverIn_NS = extractTGraphArray("OutOverIn_NS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> MidOverIn_NS = extractTGraphArray("MidOverIn_NS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> OutOverInPar_AS = extractTGraphArray("OutOverInPar_AS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> MidOverInPar_AS = extractTGraphArray("MidOverInPar_AS_" + triggerClass, files);

	CalculateSystematicUncertaintyN(OutOverIn_AS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(MidOverIn_AS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(OutOverIn_NS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(MidOverIn_NS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(OutOverInPar_AS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(MidOverInPar_AS,fSystematicUncertOutFile);

	vector <vector<TGraphErrors * >> OutMinusIn_AS = extractTGraphArray("OutMinusIn_AS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> MidMinusIn_AS = extractTGraphArray("MidMinusIn_AS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> OutMinusIn_NS = extractTGraphArray("OutMinusIn_NS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> MidMinusIn_NS = extractTGraphArray("MidMinusIn_NS_" + triggerClass, files);

	CalculateSystematicUncertaintyN(OutMinusIn_AS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(MidMinusIn_AS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(OutMinusIn_NS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(MidMinusIn_NS,fSystematicUncertOutFile);

	vector <vector<TGraphErrors * >> RMSOutOverIn_AS = extractTGraphArray("RMSOutOverIn_AS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> RMSMidOverIn_AS = extractTGraphArray("RMSMidOverIn_AS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> RMSOutOverIn_NS = extractTGraphArray("RMSOutOverIn_NS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> RMSMidOverIn_NS = extractTGraphArray("RMSMidOverIn_NS_" + triggerClass, files);

	CalculateSystematicUncertaintyN(RMSOutOverIn_AS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(RMSMidOverIn_AS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(RMSOutOverIn_NS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(RMSMidOverIn_NS,fSystematicUncertOutFile);

	// FIXME add width ratios?
	vector <vector<TGraphErrors * >> WidthsOutOverIn_AS = extractTGraphArray("WidthsOutOverIn_AS_" + triggerClass, files);
	vector <vector<TGraphErrors * >> WidthsMidOverIn_AS = extractTGraphArray("WidthsMidOverIn_AS_" + triggerClass, files);

	CalculateSystematicUncertaintyN(WidthsOutOverIn_AS,fSystematicUncertOutFile);
	CalculateSystematicUncertaintyN(WidthsMidOverIn_AS,fSystematicUncertOutFile);

// MidOverIn_AS_pi0Pt

  /*
  vector <vector<TGraphErrors *> > ptBinASWidthsGraphsEP;
  vector <vector<TGraphErrors *> > ptBinASRmsGraphsEP;
  vector <vector<TGraphErrors *> > ptBinASIntegralsGraphsEP;
  vector <vector<TGraphErrors *> > ptBinASFwhmGraphsEP;
  vector <vector<TGraphErrors *> > ptBinASFwhmRescaledGraphsEP;
  vector <vector<TGraphErrors *> > ptBinASYieldsGraphsEP;
*/

	fSystematicUncertOutFile->Write();

	printf("Finished calculating systematic uncertainties for integrals,yields,rms,width\n");

}

int main(int argc, char * argv[]) {

  if (argc < 3) {
    printf("Usage: %s <SystematicUncertaintyRootFile> [Filepath_1] [Label_1] [Filepath_2] [Label_2] ... \n",argv[0]);
//    printf("    Or %s -p [PATTERN] [Filepath_1] [Label_1] [Filepath_2] [Label_2] ... \n",argv[0]);
    exit(0);
  }

  int num = ((argc) / 2) ;
  vector<fileLabel> *fileLabels = new vector<fileLabel>;
  for (int i = 1; i < num; i++) {
    fileLabels->push_back(fileLabel(TString(argv[2*i]),TString(argv[2*i+1])));
  }
	printf("Starting calculation of systematic uncertainties using files:");
	for (int i = 0; i < num - 1 ; i++) {
		printf("%s ",fileLabels->at(i).filepath.Data());
	}
	printf("\n");
  calcSystUncert(fileLabels,argv[1]);
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct mark_planar_edge
{
    template <typename Graph, typename Vertex>
    void visit_vertex_pair(Vertex u, Vertex v, Graph& g)
    {
        if (!is_adjacent(u, v, g))
            add_edge(u, v, g);
    }
};

struct do_maximal_planar
{
    template <class Graph, class VertexIndex, class EdgeIndex>
    void operator()(Graph& g, VertexIndex vertex_index, EdgeIndex edge_index) const
    {

        unchecked_vector_property_map
            <vector<typename graph_traits<Graph>::edge_descriptor>, VertexIndex>
            embedding(vertex_index, num_vertices(g));
        bool is_planar = boyer_myrvold_planarity_test
            (boyer_myrvold_params::graph = g,
             boyer_myrvold_params::edge_index_map = edge_index,
             boyer_myrvold_params::embedding = embedding);

        if (!is_planar)
            throw GraphException("Graph is not planar!");

        mark_planar_edge vis;
        make_biconnected_planar(g, embedding, edge_index, vis);
        boyer_myrvold_planarity_test
            (boyer_myrvold_params::graph = g,
             boyer_myrvold_params::edge_index_map = edge_index,
             boyer_myrvold_params::embedding = embedding);
        make_maximal_planar(g, embedding, vertex_index, edge_index, vis);
    }

};

void maximal_planar(GraphInterface& gi)
{
    run_action<graph_tool::detail::never_directed, mpl::true_>()
        (gi, std::bind(do_maximal_planar(), placeholders::_1, gi.GetVertexIndex(),
                       gi.GetEdgeIndex()))();
}


///
/// \file
///  \author Hao Li hao.li@live.co.uk
///  \version 1.0
///  \date 2017
///  \pre OpenCV and Boost libraries
///

#include "Detection.h"
#include <iostream>
#include <vector>
#include <math.h>
#include <deque>
#include <numeric>
#include <algorithm>
#include <functional>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include "Utils/Exception.h"
#include "Serialisation/json.h"

using namespace std;
using namespace cv;

namespace {
	class LessRegion : public unary_function<vector<Point>, bool>
    {
    public:
        LessRegion(int minWidth, int minHeight): m_area(minWidth*minHeight) {}
        bool operator()(const vector<Point>& val) const
        {
            return static_cast<int>(val.size()) < m_area;
        }

    private:
        int m_area;
    };
}

namespace FG {
    /// Struct definition of GMM based background model
    class CGmmModelParam
    {
    public:
        // Tuning parameters
        int     m_windowSize;                ///< reciprocal of update rate (300-600)
        int     m_numberGmm;                 ///< number of Gaussians (3-5)
        float   m_stdThreshold;              ///< define in normal Gaussian distribution (mu-std_thre < x < mu+std_thre), m_stdThreshold = 2.5 means 99% included
        float   m_bgThreshold;               ///< threshold of weight sum (0.8-0.9)
        float   m_gradientThreshold;         ///< threshold for pixel gradient similarity measurement (0.7-0.9)
        float   m_shadowBrightThreshold;     ///< threshold for shadow brightness distortion (0.7-0.9)
        float   m_highlightThreshold;        ///< threshold for sudden lighting change (1.1-1.3)
        int     m_globalUpdateRate;          ///< update rate for non-moving regions (5-10)
        int     m_minRegionWidth;            ///< minimum foreground width (5-10) for removing small regions
        int     m_minRegionHeight;           ///< minimum foreground height (10-20)

        // Fixed parameters
        float   m_initSigma2;                ///< Square of standard deviation for each cluster (10)
        float   m_supportRate;               ///< percentage of samples that supports an existed cluster (0.05)
        float   m_shadowChromaThreshold;     ///< threshold for shadow chroma distortion (3.0)
        int     m_gridSize;                  ///< grid size for coarse FG mask generation
        int     m_medfiltSize;               ///< size of 2D median filter
    };

    /// CGmmBgModelParam implementation details
    class CGmmBgModelParam::Impl
    {
    public:
        /// helper to set default BgModelParam
        void BgModelParamDefault()
        {
            m_param.m_windowSize = 300;
            m_param.m_numberGmm = 3;
            m_param.m_stdThreshold = 3;
            m_param.m_bgThreshold = 0.90f;
            m_param.m_gradientThreshold = 0.80f;
            m_param.m_shadowBrightThreshold = 0.70f;
            m_param.m_highlightThreshold = 1.20f;
            m_param.m_globalUpdateRate = 10;
            m_param.m_minRegionWidth = 5;
            m_param.m_minRegionHeight = 10;
            m_param.m_initSigma2 = 10;
            m_param.m_supportRate = 0.05f;
            m_param.m_shadowChromaThreshold = 3.0f;
            m_param.m_gridSize = 4;
            m_param.m_medfiltSize = 7;
        }

		void BgModelParamFromConfig(const string& configFile)
		{
			CREQUIRE(!configFile.empty(), "No configuration file specified");

			Json::Reader reader;
			Json::Value root;
			std::ifstream ifs(configFile.c_str());
			CREQUIRE(ifs.is_open(), "Error opening configuration file: " + configFile);
			CREQUIRE(reader.parse(ifs, root), "Error parsing configuration file: " + reader.getFormattedErrorMessages());

			const auto& config = root["ForegroundDetection"];
			const auto& preGmm = config["Preprocessing"];
			const auto& gmm = config["GMM"];
			const auto& postGmm = config["Postprocessing"];
			m_param.m_gradientThreshold = preGmm["GradientThreshold"].asFloat();
			m_param.m_globalUpdateRate = preGmm["GlobalUpdateRate"].asInt();
			m_param.m_gridSize = preGmm["GridSize"].asInt();
			m_param.m_windowSize = gmm["WindowSize"].asInt();
			m_param.m_numberGmm = gmm["NumberGmms"].asInt();
			m_param.m_stdThreshold = gmm["StdThreshold"].asFloat();
			m_param.m_initSigma2 = gmm["InitSigma2"].asFloat();
			m_param.m_bgThreshold = gmm["BackgroundThreshold"].asFloat();
			m_param.m_supportRate = gmm["SupportRate"].asFloat();
			m_param.m_shadowBrightThreshold = postGmm["ShadowBrightThreshold"].asFloat();
			m_param.m_shadowChromaThreshold = postGmm["ShadowChromaThreshold"].asFloat();
			m_param.m_highlightThreshold = postGmm["HighlightThreshold"].asFloat();
			m_param.m_minRegionWidth = postGmm["MinRegionWidth"].asInt();
			m_param.m_minRegionHeight = postGmm["MinRegionHeight"].asInt();
			m_param.m_medfiltSize = postGmm["MedianFilterSize"].asInt();
		}

        CGmmModelParam m_param;
    };

    CGmmBgModelParam::CGmmBgModelParam() : m_impl(new CGmmBgModelParam::Impl())
    {
        m_impl->BgModelParamDefault ();
    }

    CGmmBgModelParam::CGmmBgModelParam (const std::string& configFile) : m_impl(new CGmmBgModelParam::Impl())
    {
		m_impl->BgModelParamFromConfig(configFile);
    }

    // Get all background model parameters
    int     CGmmBgModelParam::WindowSize() const                 {return m_impl->m_param.m_windowSize;}
    int     CGmmBgModelParam::NumberGmm() const                  {return m_impl->m_param.m_numberGmm;}
    float   CGmmBgModelParam::StdThreshold() const               {return m_impl->m_param.m_stdThreshold;}
    float   CGmmBgModelParam::BgThreshold() const                {return m_impl->m_param.m_bgThreshold;}
    float   CGmmBgModelParam::GradientThreshold() const          {return m_impl->m_param.m_gradientThreshold;}
    float   CGmmBgModelParam::ShadowBrightThreshold() const      {return m_impl->m_param.m_shadowBrightThreshold;}
    float   CGmmBgModelParam::HighlightThreshold() const         {return m_impl->m_param.m_highlightThreshold;}
    int     CGmmBgModelParam::GlobalUpdateRate() const           {return m_impl->m_param.m_globalUpdateRate;}
    int     CGmmBgModelParam::MinRegionWidth() const             {return m_impl->m_param.m_minRegionWidth;}
    int     CGmmBgModelParam::MinRegionHeight() const            {return m_impl->m_param.m_minRegionHeight;}
    float   CGmmBgModelParam::InitSigma2() const                 {return m_impl->m_param.m_initSigma2;}
    float   CGmmBgModelParam::SupportRate() const                {return m_impl->m_param.m_supportRate;}
    float   CGmmBgModelParam::ShadowChromaThreshold() const      {return m_impl->m_param.m_shadowChromaThreshold;}
    int     CGmmBgModelParam::GridSize() const                   {return m_impl->m_param.m_gridSize;}
    int     CGmmBgModelParam::MedfiltSize() const                {return m_impl->m_param.m_medfiltSize;}

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    /// CGmmBgModel implementation details
    class CGmmBgModel::Impl
    {
    public:

        /// Store GMM information at each pixel position
        class CGmmInfo
        {
        public:
            long m_matchCout;				///< number of match times, used before window size reaches predefined value
            float m_updateRate;		///< update rate of current pixel location, can be changed through process
            float m_weight;					///<weight of each Gaussian cluster
            float m_mu[CGmmBgModel::GMM_DIMENSION];		///<store mu of each color channel
            float m_sigma2[CGmmBgModel::GMM_DIMENSION];	///<store Sigma of each color channel
        };

        /// Store GMM information of the model
        class CGmmPixelInfo
        {
        public:
            int M;      ///< number of Gaussian used in current pixel position
            CGmmInfo* m_info;
        };

        /// Hold grid based coarse foreground detection results
        class CGmmTempInfo
        {
        public:
            shared_ptr<Mat> m_frameGray;
			shared_ptr<Mat> m_backgroundGray;
			shared_ptr<Mat> m_frameGradientX;
			shared_ptr<Mat> m_frameGradientY;
			shared_ptr<Mat> m_backgroundGradientX;
			shared_ptr<Mat> m_backgroundGradientY;
			shared_ptr<Mat> m_frameGradientMag;
			shared_ptr<Mat> m_frameGradientMag2;
			shared_ptr<Mat> m_backgroundGradientMag;
			shared_ptr<Mat> m_backgroundGradientMag2;
			shared_ptr<Mat> m_gradientDotProduct;
			shared_ptr<Mat> m_temp1;
			shared_ptr<Mat> m_temp2;

        };

        /// Calculate similarity gradient mask
        void SgMaskCalculation(const Mat& frame)
        {
            int gradSize = m_param.GridSize();
            int channels = frame.channels();
            if (channels == 3)  // covert to gray scale
            {
                cvtColor (frame, *mpTempInfo->m_frameGray, CV_BGR2GRAY);
                cvtColor (*mpBackground, *mpTempInfo->m_backgroundGray, CV_BGR2GRAY);
            }
            else    // store to temporal place
            {
                frame.copyTo(*mpTempInfo->m_frameGray);
                mpBackground->copyTo(*mpTempInfo->m_backgroundGray);
            }

            // calculate the image derivatives using sobel filter
            // TODO gpu::GpuMat to accrelate speed
            Sobel (*mpTempInfo->m_frameGray, *mpTempInfo->m_frameGradientX, CV_32F, 1, 0, 3, 1.0/8);    //scaling
            Sobel (*mpTempInfo->m_frameGray, *mpTempInfo->m_frameGradientY, CV_32F, 0, 1, 3, 1.0/8);
            Sobel (*mpTempInfo->m_backgroundGray, *mpTempInfo->m_backgroundGradientX, CV_32F, 1, 0, 3, 1.0/8);
            Sobel (*mpTempInfo->m_backgroundGray, *mpTempInfo->m_backgroundGradientY, CV_32F, 0, 1, 3, 1.0/8);

            // find the square of magnitude of gradient
            multiply(*mpTempInfo->m_frameGradientX, *mpTempInfo->m_frameGradientX, *mpTempInfo->m_temp1);
            multiply(*mpTempInfo->m_frameGradientY, *mpTempInfo->m_frameGradientY, *mpTempInfo->m_temp2);
            add (*mpTempInfo->m_temp1, *mpTempInfo->m_temp2, *mpTempInfo->m_frameGradientMag2);
            multiply(*mpTempInfo->m_backgroundGradientX, *mpTempInfo->m_backgroundGradientX, *mpTempInfo->m_temp1);
            multiply(*mpTempInfo->m_backgroundGradientY, *mpTempInfo->m_backgroundGradientY, *mpTempInfo->m_temp2);
            add (*mpTempInfo->m_temp1, *mpTempInfo->m_temp2, *mpTempInfo->m_backgroundGradientMag2);

            Size fsize = frame.size();
            // calculate the dot product and magnitude of gradient;
            float* pGradientDotProduct = mpTempInfo->m_gradientDotProduct->ptr<float>();
            float* pBackgroundGradientX =  mpTempInfo->m_backgroundGradientX->ptr<float>();
            float* pBackgroundGradientY = mpTempInfo->m_backgroundGradientY->ptr<float>();
            float* pFrameGradientMag = mpTempInfo->m_frameGradientMag->ptr<float>();
            float* pFrameGradientMag2 = mpTempInfo->m_frameGradientMag2->ptr<float>();
            float* pBackgroundGradientMag = mpTempInfo->m_backgroundGradientMag->ptr<float>();
            float* pBackgroundGradientMag2 = mpTempInfo->m_backgroundGradientMag2->ptr<float>();
            for (int i = 0, n = 0; i < fsize.height; ++i)
            {
                for (int j = 0; j < fsize.width; ++j, ++n)
                {
                    // dot product
                    pGradientDotProduct[n] = pBackgroundGradientX[n]*pBackgroundGradientX[n] + pBackgroundGradientY[n]*pBackgroundGradientY[n];
                    // gradient
                    pFrameGradientMag[n] = sqrt(pFrameGradientMag2[n]);
                    pBackgroundGradientMag[n] = sqrt(pBackgroundGradientMag2[n]);
                }
            }

            //Scalar_<float> frameGradientMean = mean(*mpTempInfo->m_frameGradientMag);
            //Scalar_<float> backgroundGradientMean = mean(*mpTempInfo->m_backgroundGradientMag);
            //cout << "\"" << frameGradientMean(0) << " " << backgroundGradientMean(0) << "\"";
            // calculate similarity mask SgMask == 1 for gradient dissimilar regions, 0 for similar regions and gradient homogeneous regions
            int gridSize = m_param.GridSize ();
            //int nElems = gridSize*gridSize;
            uchar* pSgMask = mpSgMask->ptr<uchar>();
            for (int i = 0, n = 0; i < fsize.height/gridSize; ++i)
            {
                for (int j = 0; j < fsize.width/gridSize; ++j, ++n)
                {
                    float sum1 = 0.0f;
                    float sum2 = 0.0f;
                    //float sum3 = 0.0f;  /// mean of magnitude of each grid in frame
                    //float sum4 = 0.0f;  /// mean of magnitude of each grid in background
                    for (int x = 0; x < gridSize; ++x)
                    {
                        for (int y = 0; y < gridSize; ++y)
                        {
                            int index = (i*gridSize+x)*fsize.width+j*gridSize+y;
                            //sum1 += 2*mpTempInfo->m_gradientDotProduct->at<float>(i*gridSize+x, j*gridSize+y);
                            //sum2 += mpTempInfo->m_frameGradientMag2->at<float>(i*gridSize+x, j*gridSize+y) + mpTempInfo->m_backgroundGradientMag2->at<float>(i*gridSize+x, j*gridSize+y);
                            sum1 += 2*pGradientDotProduct[index];
                            sum2 += pFrameGradientMag2[index]+pBackgroundGradientMag2[index];
                            //sum3 += pFrameGradientMag[n];
                            //sum4 += pBackgroundGradientMag[n];
                        }
                    }

                    //sum3 /= nElems;
                    //sum4 /= nElems;

                    //float thres = 0.5f; /// to remove homogeneous regions
                    if (/*(sum3 > thres || sum4 > thres) && */sum1/sum2 < m_param.GradientThreshold ())
                    {
                        pSgMask[n] = static_cast<uchar>(255);
                    } else
                    {
                        pSgMask[n] = static_cast<uchar>(0);	//TODO
                    }

                }
            }

        }

        /// Coarse foreground detection based on single Gaussian model
        void CoarseForegroundDetection(const Mat& frame)
        {
            Size fsize = frame.size();
            int channels = frame.channels ();
            int gridSize = m_param.GridSize ();
            int nElems = gridSize*gridSize;
            float stdThres = m_param.StdThreshold ();
            const uchar* pdata = frame.ptr<uchar>();
            uchar* pCFMaskS = mpCFMaskS->ptr<uchar>();
            float* pCBMu = mpCBMu->ptr<float>();
            float* pCBSigma2 = mpCBSigma2->ptr<float>();
            float* pCFMu = mpCFMu->ptr<float>();
            // Gaussian based detection
            for (int i = 0, n = 0; i < fsize.height/gridSize; ++i)
            {
                for (int j = 0; j < fsize.width/gridSize; ++j, ++n)
                {
                    float sumCBMu[CGmmBgModel::GMM_DIMENSION] = {0.0f};
                    float sumCBSigma2[CGmmBgModel::GMM_DIMENSION] = {0.0f};
                    float sumCFMu[CGmmBgModel::GMM_DIMENSION] = {0.0f};

                    // sum in each grid, take the 1st Gaussian in the GMM as the current background
                    for (int x = 0; x < gridSize; ++x)
                    {
                        for (int y = 0; y < gridSize; ++y)
                        {
                            int index = (i*gridSize+x)*fsize.width + j*gridSize+y;
                            int p = index*channels;

                            for (int d = 0; d < channels; ++d)
                            {
                                sumCBMu[d] += mpPixelInfo[index].m_info[0].m_mu[d];
                                sumCBSigma2[d] += mpPixelInfo[index].m_info[0].m_sigma2[d];
                                sumCFMu[d] += static_cast<float>(pdata[p+d]);
                            }
                        }
                    }

                    float sumDelta2 = 0.0f;
                    float sum_sigma2 = 0.0f;
                    // average each grid and do single Gaussian detection
                    for (int d = 0; d < channels; ++d)
                    {
                        pCBMu[n+d] = sumCBMu[d]/nElems;
                        pCBSigma2[n+d] = sumCBSigma2[d]/nElems;
                        pCFMu[n+d] = sumCFMu[d]/nElems;

                        float delta = sumCFMu[d] - sumCBMu[d];
                        sumDelta2 += delta*delta;
                        sum_sigma2 += sumCBSigma2[d];
                    }
                    sum_sigma2 = stdThres*stdThres*sum_sigma2;

                    // generate coarse foreground mask
                    if (sumDelta2 >= sum_sigma2)
                    {
                        pCFMaskS[n] = static_cast<uchar>(255);
                    } else
                    {
                        pCFMaskS[n] = static_cast<uchar>(0);
                    }
                }
            }

            // extend CFMaskS to full size CFMaskF and generate a updateMask based on CFMaskF and SgMask
            Size sMaskSize = mpCFMaskS->size();
            uchar* pCFMaskF = mpCFMaskF->ptr<uchar>();
            uchar* pUpdateMask = mpUpdateMask->ptr<uchar>();
            uchar* pSgMask = mpSgMask->ptr<uchar>();
            for (int i = 0, n = 0; i < fsize.height; ++i)
            {
                for (int j = 0; j < fsize.width; ++j, ++n)
                {
                    int index = i/gridSize*sMaskSize.width + j/gridSize;
                    pCFMaskF[n] = pCFMaskS[index];
                    pUpdateMask[n] = pSgMask[index];
                }
            }

            Mat key = Mat::ones(3, 3, CV_8U);

            dilate(*mpCFMaskF, *mpCFMaskF, key, Point(-1, -1), 1);
            bitwise_or(*mpCFMaskF, *mpUpdateMask, *mpUpdateMask);

        }

        /// Calculate probability of pixel follows a Gaussian distribution
        /** @param[in] pixel pixel value in a \b d dimensional vector
            ** @param[in] pInfo reference to CGmmInfo
            ** @param[in] channels number of dimensions of the pixel
            ** \return Gaussian probability
            */
        float GaussianPDF(const float* pixel, const CGmmInfo& pInfo, int channels = 3)
        {
            static const float pi = 3.14159f;
            float invSigma2 = 0.0f;
            float invDetSigma2 = 1.0;
            float delta = 0.0f;
            float sumVec = 0.0f;

            for (int d = 0; d < channels; ++d)
            {
                delta = pixel[d] - pInfo.m_mu[d];
                invSigma2 = 1.0f/pInfo.m_sigma2[d];
                invDetSigma2 *= invSigma2;
                sumVec += delta*delta*invSigma2;
            }

            return static_cast<float>(sqrt(pow(2.0*pi, -channels)*invDetSigma2)*exp(-0.5*sumVec));
        }

        /// Test if current pixel matches any Gaussian in the model
        /** @param[in] pixel pixel value
            ** @param[in] pPixelInfo pointer to CGmmPixelInfo
            ** @param[in] channels number of dimensions of the pixel
            ** @param[out] matchIndex a <b>1 x M</b> array (\b M is the number of Gaussians in GMM). The
            ** corresponding match position is set to 1, otherwise all 0
            ** \return \b true if has match, otherwise \b false
            ***/
        bool GmmMatchTest(const float* pixel, const CGmmPixelInfo* pPixelInfo, int* matchIndex, int channels = 3)
        {
            bool isMatch = false;
            const int M = pPixelInfo->M;
            float stdThres = m_param.StdThreshold ();
            // each Gaussian is already sorted in descending order
            for (int k = 0; k < M; ++k)
            {
                float sumDelta2 = 0.0f;
                float sum_sigma2 = 0.0f;
                for (int d = 0; d < channels; ++d)
                {
                    float delta = pixel[d] - pPixelInfo->m_info[k].m_mu[d];
                    sumDelta2 += delta*delta;
                    sum_sigma2 += pPixelInfo->m_info[k].m_sigma2[d];
                }
                sum_sigma2 = stdThres*stdThres*sum_sigma2;

                // match measure under Euclidean distance and extract the number of first matched Gaussian
                if (sumDelta2 < sum_sigma2)
                {
                    matchIndex [k] = 1;
                    isMatch = true;
                    break;
                }
            }

            return isMatch;

        }

        /// Update the GMM model at each pixel position if has matched input
        void Gm_mupdateMatch(const float* pixel, CGmmPixelInfo* pPixelInfo, const int* matchIndex, bool isGlobalUpdate, uchar updateMask, long frameCount, int channels = 3)
        {

            int windowSize = m_param.WindowSize();
            float supportRate = m_param.SupportRate ();
            float learningRate;
            if (isGlobalUpdate && !updateMask)
            {
                learningRate = frameCount < windowSize ? 1.0f/frameCount : m_param.GlobalUpdateRate()*1.0f/windowSize;
            }
            else
            {
                learningRate = frameCount < windowSize ? 1.0f/frameCount : 1.0f/windowSize;
            }
            float weightSum = 0.0f;
            float delta = 0.0f;
            const int M = pPixelInfo->M;
            float updateRate = 0.0f;
            float probGaussian = 0.0f;
            float eat = 0.0f;

            for (int k = 0; k < M; ++k)
            {
                pPixelInfo->m_info[k].m_matchCout += static_cast<long>(matchIndex[k]);

                // gmm initialisation stage
                if (frameCount < windowSize)
                {
                    pPixelInfo->m_info[k].m_weight += learningRate*(static_cast<float>(matchIndex[k]) - pPixelInfo->m_info[k].m_weight);
                }
                else  // gmm enters stable stage
                {
                    pPixelInfo->m_info[k].m_weight += learningRate*(static_cast<float>(matchIndex[k]) - pPixelInfo->m_info[k].m_weight) - learningRate*supportRate;
                    // delete negative weighted clusters
                    if ( pPixelInfo->m_info[k].m_weight < 0)
                    {
                        pPixelInfo->M--;
                        pPixelInfo->m_info[k].m_matchCout = 0;
                        pPixelInfo->m_info[k].m_weight = 0;

                        for (int d = 0; d < channels; ++d)
                        {
                            pPixelInfo->m_info[k].m_mu[d] = 0;
                            pPixelInfo->m_info[k].m_sigma2[d] = 0;
                        }
                    }
                }

                weightSum += pPixelInfo->m_info[k].m_weight;

                if (matchIndex[k])
                {
                    updateRate = frameCount < windowSize ? static_cast<float>(matchIndex[k])/static_cast<float>(pPixelInfo->m_info[k].m_matchCout) : learningRate*GaussianPDF (pixel, pPixelInfo->m_info[k], channels);

                    for (int d = 0; d < channels; ++d)
                    {
                        delta = pixel[d] - pPixelInfo->m_info[k].m_mu[d];
                        pPixelInfo->m_info[k].m_mu[d] += updateRate*delta;
                        pPixelInfo->m_info[k].m_sigma2[d] += updateRate*(delta*delta - pPixelInfo->m_info[k].m_sigma2[d]);

                        // avoid divergence? TODO?
                    }
                }
            }

            // normalise the weight of each Gaussian
            for (int k = 0; k < M; ++k)
            {
                pPixelInfo->m_info[k].m_weight /= weightSum;
            }

        }

        /// Update GMM information at each pixel position if no matched input found
        void Gm_mupdateNoMatch(const float* pixel, CGmmPixelInfo* pPixelInfo, long frameCount, int channels = 3)
        {
            int windowSize = m_param.WindowSize();
            float learningRate = frameCount < windowSize ? 1.0f/frameCount : 1.0f/windowSize;
            int M = pPixelInfo->M;
            float initSigma2 = m_param.InitSigma2 ();
            float weightTemp = 0.0f;

            if (M < m_param.NumberGmm ())
            {
                pPixelInfo->M++;
                M++;
            }
            else
            {
                weightTemp = pPixelInfo->m_info[M-1].m_weight;
            }

            pPixelInfo->m_info[M-1].m_matchCout = 1;
            pPixelInfo->m_info[M-1].m_weight = learningRate;

            for (int d = 0; d < channels; ++d)
            {
                pPixelInfo->m_info[M-1].m_mu[d] = pixel[d];
                pPixelInfo->m_info[M-1].m_sigma2[d] = initSigma2;
            }

            for (int k = 0; k < M; ++k)
            {
                pPixelInfo->m_info[k].m_weight /= 1.0f - weightTemp + learningRate;
            }

        }

        /// Sort the GMM at each pixel position in descending order according to each Gaussian's weight/Sigma2
        void GmmSort(CGmmPixelInfo* pPixelInfo, int* matchIndex, int channels = 3)
        {
            const int M = pPixelInfo->M;
            float sortIndex[CGmmBgModel::GMM_MAX_NUMBER] = {0.0f};
            for (int k = 0; k < M; ++k)
            {
                // avoid divided by zero in initial step and after delete some clusters
                if (pPixelInfo->m_info[k].m_matchCout > 0)
                {
                    float sum_sigma2 = 0.0f;
                    for (int d = 0; d < channels; ++d)
                    {
                        sum_sigma2 += pPixelInfo->m_info[k].m_sigma2[d];
                    }

                    sortIndex[k] = pPixelInfo->m_info[k].m_weight/sum_sigma2;
                }

            }

            // reorder Gaussians according to the index
            for (int k = 1; k < M; ++k)
            {
                for (int i = k; i > 0 && (sortIndex[i-1] < sortIndex[k]); --i)
                {
                    float sortIndexTemp = sortIndex[i];
                    sortIndex[i] = sortIndex[i-1];
                    sortIndex[i-1] = sortIndexTemp;

                    int matchIndexTemp = matchIndex[i];
                    matchIndex[i] = matchIndex[i-1];
                    matchIndex[i-1] = matchIndexTemp;

                    // using pointer change to speed up?
                    CGmmInfo infoTemp = pPixelInfo->m_info[i];
                    pPixelInfo->m_info[i] = pPixelInfo->m_info[i-1];
                    pPixelInfo->m_info[i-1] = infoTemp;
                }
            }

        }

        /// Generate current background image from GMM model
        void GmmBackground()
        {
            Size fsize = mpBackground->size();
            int channels = mpBackground->channels ();
            uchar* pBackground = mpBackground->ptr<uchar>();
            for (int i = 0, n = 0; i < fsize.height; ++i)
            {
                for (int j = 0; j < fsize.width; ++j, ++n)
                {
                    int p = n*channels;
                    for (int d = 0; d < channels; ++d)
                    {
                        pBackground[p+d] = static_cast<uchar>(mpPixelInfo[n].m_info[0].m_mu[d]+0.5);
                    }
                }
            }
        }

        /// Calculate if current pixel position belongs to foreground and set the corresponding mask in mpForeground
        void GmmForegourndPixel(const CGmmPixelInfo* pPixelInfo, const int* matchIndex, int n, int regionMask)
        {
            uchar foregroundPixel = static_cast<uchar>(255);
            float weightSum = 0.0f;
            int M = pPixelInfo->M;
            float bgThres = m_param.BgThreshold ();
            for (int k = 0; k < M; ++k)
            {
                if (matchIndex[k])
                {
                    foregroundPixel = 0;
                }

                weightSum += pPixelInfo->m_info[k].m_weight;
                if (weightSum > bgThres)
                    break;
            }

            if (mIsCoarseDetection)
                mpForeground->at<uchar>(n) = foregroundPixel & mpCFMaskF->at<uchar>(n) & regionMask;
            else
                mpForeground->at<uchar>(n) = foregroundPixel & regionMask;

        }

        /// Find connected regions from foreground
        void Neighbours(const cv::Mat& foreground, std::vector<cv::Point>& points, int previndex, int index, int nNeighbours = 8)
        {
            Size fsize = foreground.size();
            const uchar* pdata = foreground.ptr<uchar>();
            uchar* ptemp = mpTempMat->ptr<uchar>(); // to record if the current foreground pixel has been looped

            if (index < 0 || index >= fsize.height*fsize.width)
            {
                return;
            }
            else
            {
                if (pdata[index] > 0 && ptemp[index] == 0)  // good to go
                {
                    if (index%fsize.width == 0 && previndex%fsize.width == fsize.width-1) // central pixel at right boundary
                        return;
                    if (index%fsize.width == fsize.width-1 && previndex%fsize.width == 0) // left boundary
                        return;

                    ptemp[index] = 1;
                    Point point;
                    point.x = index%fsize.width;
                    point.y = index/fsize.width;
                    points.push_back (point);

                    if (nNeighbours == 4)
                    {
                        Neighbours (foreground, points, index, index-1, 4);
                        Neighbours (foreground, points, index, index+1, 4);
                        Neighbours (foreground, points, index, index-fsize.width, 4);
                        Neighbours (foreground, points, index, index+fsize.width, 4);
                    }
                    else if (nNeighbours == 8)
                    {
                        Neighbours (foreground, points, index, index-1, 8);
                        Neighbours (foreground, points, index, index+1, 8);
                        Neighbours (foreground, points, index, index-fsize.width, 8);
                        Neighbours (foreground, points, index, index+fsize.width, 8);
                        Neighbours (foreground, points, index, index-fsize.width-1, 8);
                        Neighbours (foreground, points, index, index-fsize.width+1, 8);
                        Neighbours (foreground, points, index, index+fsize.width-1, 8);
                        Neighbours (foreground, points, index, index+fsize.width+1, 8);
                    }
                }
                return;
            }
        }

        CGmmBgModelParam        m_param;                 ///< store parameters
        long                    mFrameCount;            ///< number of frames that has been processed
        bool                    mIsCoarseDetection;     ///< if ture, do coarse pre-detection, otherwise do simple GMM
        CGmmPixelInfo*          mpPixelInfo;            ///< pointer to array of CGmmPixelInfo (equal to frame size)
        shared_ptr<Mat>                    mpBackground;           ///< store background of the model (channel = channel of frame)
        shared_ptr<Mat>                    mpForeground;           ///< detected foreground (single channel)
        shared_ptr<Mat>                    mpUpdateMask;           ///< mask to indicate which pixel to be updated
        shared_ptr<Mat>                    mpSgMask;               ///< similar gradient mask, smaller sized image, depend on param.gridsize
        shared_ptr<Mat>                    mpCFMaskS;              ///< coarse foreground mask smaller sized
        shared_ptr<Mat>                    mpCFMaskF;              ///< coarse foreground mask full sized
        shared_ptr<Mat>                    mpCBMu;                 ///< coarse background mu
        shared_ptr<Mat>                    mpCBSigma2;             ///< coarse background Sigma2
        shared_ptr<Mat>                    mpCFMu;                 ///< coarse frame mu
        shared_ptr<Mat>                    mpTempMat;              ///< same size as frame for any temp useage
        shared_ptr<CGmmTempInfo>           mpTempInfo;             ///< store intermediate results
        vector<vector<Point> >  mFgContours;            ///< store each individual foreground contours, default initialisation equals to 0
        vector<vector<Point> >  mFgRegions;               ///< store all foreground pixels in each individual foreground regions
        vector<Rect>            mFgBoxes;                 ///< foreground bounding boxes
    };

    CGmmBgModel::CGmmBgModel(const cv::Mat& frame, const CGmmBgModelParam& param, bool isCoarseDetection)
        : m_impl(new CGmmBgModel::Impl())
    {
        // allocate corresponding memories for foreground detection
        int channels = frame.channels (); // number of channel of frame
        Size fsize = frame.size();    // height and width of frame

        m_impl->m_param = param;
        m_impl->mFrameCount = 0;
        m_impl->mIsCoarseDetection = isCoarseDetection;
        m_impl->mpPixelInfo = new Impl::CGmmPixelInfo[fsize.height*fsize.width];
        //const int sizeND1[] = {fsize.height, fsize.width, channels};
        //m_impl->mpBackground = new Mat(channels, sizeND1, CV_8U);
        //frame.copyTo(*m_impl->mpBackground);
        if (1 == channels)
        {
            m_impl->mpBackground.reset(new Mat(fsize.height, fsize.width, CV_8UC1));
        }
        else if (3 == channels)
        {
            m_impl->mpBackground.reset(new Mat(fsize.height, fsize.width, CV_8UC3));
        }

        m_impl->mpForeground.reset(new Mat(fsize.height, fsize.width, CV_8UC1));
        m_impl->mpTempMat.reset(new Mat(fsize.height, fsize.width, CV_8UC1));
        m_impl->mpUpdateMask.reset(new Mat(fsize.height, fsize.width, CV_8UC1));

        if (isCoarseDetection)   // do my method, otherwise pure GMM
        {
            m_impl->mpSgMask.reset(new Mat(fsize.height/param.GridSize (), fsize.width/param.GridSize (), CV_8UC1));
            m_impl->mpCFMaskS.reset(new Mat(fsize.height/param.GridSize (), fsize.width/param.GridSize (), CV_8UC1));
            m_impl->mpCFMaskF.reset(new Mat(fsize.height, fsize.width, CV_8UC1));
            if (1 == channels)
            {
                m_impl->mpCBMu.reset(new Mat(fsize.height/param.GridSize (), fsize.width/param.GridSize (), CV_32FC1));
                m_impl->mpCBSigma2.reset(new Mat(fsize.height/param.GridSize (), fsize.width/param.GridSize (), CV_32FC1));
                m_impl->mpCFMu.reset(new Mat(fsize.height/param.GridSize (), fsize.width/param.GridSize (), CV_32FC1));
            } else if (3 == channels)
            {
                m_impl->mpCBMu.reset(new Mat(fsize.height/param.GridSize (), fsize.width/param.GridSize (), CV_32FC3));
                m_impl->mpCBSigma2.reset(new Mat(fsize.height/param.GridSize (), fsize.width/param.GridSize (), CV_32FC3));
                m_impl->mpCFMu.reset(new Mat(fsize.height/param.GridSize (), fsize.width/param.GridSize (), CV_32FC3));
            }

        }

        // allocate memories for CGmmInfo class in each pixel position
        //for (int i = 0; i < fsize.height; ++i)
        //{
        //    for (int j = 0; j < fsize.width; ++j)
        //    {
        //        m_impl->mspPixelInfo[i*fsize.width+j].m_info = new Impl::CGmmInfo[param.NumberGmm()];
        //    }
        //}

        // assign memories for CGmmInfo as a chunk to reduce assign and delete time
        m_impl->mpPixelInfo[0].m_info = new Impl::CGmmInfo[param.NumberGmm ()*fsize.height*fsize.width];

        const uchar* pdata = frame.ptr();   // pointer to image
        uchar* pbackground = m_impl->mpBackground->ptr ();
        // assign initial value to the model. Foreground, UpdateMask, SgMask, CFMask can be kept as default
        for (int i = 0, n=0; i < fsize.height; ++i)
        {
            for (int j = 0; j < fsize.width; ++j, ++n)
            {
                int p = n*channels;
                m_impl->mpPixelInfo[n].M = 1;
                // pointer offset assigned to m_info in each mpPixelInfo
                m_impl->mpPixelInfo[n].m_info = m_impl->mpPixelInfo[0].m_info + n*param.NumberGmm ();
                m_impl->mpPixelInfo[n].m_info[0].m_matchCout = 1;
                m_impl->mpPixelInfo[n].m_info[0].m_weight = 1;
                m_impl->mpPixelInfo[n].m_info[0].m_updateRate = 1.0f/param.WindowSize();
                for (int d = 0; d < channels; ++d)
                {
                    m_impl->mpPixelInfo[n].m_info[0].m_mu[d] = pdata[p+d];
                    m_impl->mpPixelInfo[n].m_info[0].m_sigma2[d] = param.InitSigma2();
                    pbackground[p+d] = pdata[p+d];
                }

                for (int m = 1; m < param.NumberGmm(); ++m)
                {
                    m_impl->mpPixelInfo[n].m_info[m].m_weight = 0;
                    m_impl->mpPixelInfo[n].m_info[m].m_matchCout = 0;
                    for (int d = 0; d < channels; ++d)
                    {
                        m_impl->mpPixelInfo[n].m_info[m].m_mu[d] = 0.0f;
                        m_impl->mpPixelInfo[n].m_info[m].m_sigma2[d] = 0.0f;
                    }
                }
            }
        }

        if (isCoarseDetection)
        {

            int gridSzie = param.GridSize();
            int nElems = gridSzie*gridSzie;
            float* pCBMu = m_impl->mpCBMu->ptr<float>();
            float* pCBSigma2 = m_impl->mpCBSigma2->ptr<float>();
            float* pCFMu = m_impl->mpCFMu->ptr<float>();
            for (int i = 0, n = 0; i < fsize.height/gridSzie; ++i)
            {
                for (int j = 0; j < fsize.width/gridSzie; ++j, ++n)
                {
                    int p = n*channels;
                    float sumCBMu[CGmmBgModel::GMM_DIMENSION] = {0.0f};
                    float sumCBSigma2[CGmmBgModel::GMM_DIMENSION] = {0.0f};
                    float sumCFMu[CGmmBgModel::GMM_DIMENSION] = {0.0f};

                    for (int x = 0; x < gridSzie; ++x)
                    {
                        for (int y = 0; y < gridSzie; ++y)
                        {
                            int n = (i*gridSzie+x)*fsize.width+(j*gridSzie+y);
                            int p = n*channels;
                            for (int d = 0; d < channels; ++d)
                            {
                                sumCBMu[d] +=  m_impl->mpPixelInfo[n].m_info[0].m_mu[d];
                                sumCBSigma2[d] +=  m_impl->mpPixelInfo[n].m_info[0].m_sigma2[d];
                                sumCFMu[d] += pdata[p+d];
                            }
                        }
                    }

                    // average value within each grid for coarse single Gaussian detection
                    for (int d = 0; d < channels; ++d)
                    {
                        sumCBMu[d] /= nElems;
                        sumCBSigma2[d] /= nElems;
                        sumCFMu[d] /= nElems;

                        pCBMu[p+d] = sumCBMu[d];
                        pCBSigma2[p+d] = sumCBSigma2[d];
                        pCFMu[p+d] = sumCFMu[d];

                    }
                }
            }
        }

        if (isCoarseDetection)
        {
            m_impl->mpTempInfo.reset(new Impl::CGmmTempInfo());
			m_impl->mpTempInfo->m_frameGray.reset(new Mat(fsize.height, fsize.width, CV_8UC1));
            m_impl->mpTempInfo->m_backgroundGray.reset(new Mat(fsize.height, fsize.width, CV_8UC1));
            m_impl->mpTempInfo->m_frameGradientX.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_frameGradientY.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_backgroundGradientX.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_backgroundGradientY.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_frameGradientMag.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_frameGradientMag2.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_backgroundGradientMag.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_backgroundGradientMag2.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_gradientDotProduct.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_temp1.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
            m_impl->mpTempInfo->m_temp2.reset(new Mat(fsize.height, fsize.width, CV_32FC1));
        }

        m_impl->mFrameCount++;
        //cout << "CGmmBgModel constructor called" << endl;
    }

    CGmmBgModel::~CGmmBgModel()
    {

        //Size fsize = m_impl->mpBackground->size();
        //for (int i = 0; i < fsize.height; ++i)
        //{
        //    for (int j = 0; j < fsize.width; ++j)
        //    {
        //        delete[] m_impl->mpPixelInfo[i*fsize.width+j].m_info;
        //    }
        //}
        delete[] m_impl->mpPixelInfo[0].m_info;
        delete[] m_impl->mpPixelInfo;

    }

    void CGmmBgModel::BgModelUpdate (const cv::Mat& frame, const cv::Mat* regionMask)
    {
        m_impl->mpForeground->setTo(0);   // reset foreground
        m_impl->mpTempMat->setTo (0); // to record if the current foreground pixel has been looped in Neighbours()

        bool isGlobalUpdate;
        if (m_impl->mIsCoarseDetection)
        {
            m_impl->SgMaskCalculation (frame);

            m_impl->CoarseForegroundDetection (frame);

            isGlobalUpdate = !(m_impl->mFrameCount%m_impl->m_param.GlobalUpdateRate ());    // if true, do global update
        }
        else
        {
            isGlobalUpdate = true;
        }

        Size fsize = frame.size();
        int channels = frame.channels ();
        int windowSize = m_impl->m_param.WindowSize ();
        //int gridSize = m_impl->m_param.GridSize ();
        const uchar* pdata = frame.ptr<uchar>();
		const uchar* pregionMask = regionMask == nullptr ? nullptr : regionMask->ptr<uchar>();
        uchar* pUpdateMask = m_impl->mpUpdateMask->ptr<uchar>();

        for (int i = 0, n = 0; i < fsize.height; ++i)
        {
            for (int j = 0; j < fsize.width; ++j, ++n)
            {
                //int m = i/gridSize*fsize.width/gridSize + j/gridSize;   /// corresponding position in the coarse image

                // selective update pixel information  updateMask regions + global update rate + if in initialisation stage update all
                if (m_impl->mFrameCount < windowSize || isGlobalUpdate || pUpdateMask[n])
                {
                    int p = n*channels;
                    Impl::CGmmPixelInfo* pPixelInfo = &m_impl->mpPixelInfo[n];

                    float pixel[CGmmBgModel::GMM_DIMENSION] = {0.0f};
                    for (int d = 0; d < channels; ++d)
                    {
                        pixel[d] = static_cast<float>(pdata[p+d]);      /// extract pixel value
                    }

                    int matchIndex[CGmmBgModel::GMM_MAX_NUMBER] = {0};
                    // test if current pixel matches any Gaussian in the model, if has match isMatch == true and matchIndex[] set the corresponding match position to 1
                    bool isMatch = m_impl->GmmMatchTest (pixel, pPixelInfo, matchIndex, channels);

                    if (isMatch)
                        m_impl->Gm_mupdateMatch (pixel, pPixelInfo, matchIndex, isGlobalUpdate, pUpdateMask[n], m_impl->mFrameCount, channels);
                    else
                        m_impl->Gm_mupdateNoMatch (pixel, pPixelInfo, m_impl->mFrameCount, channels);

                    // sorting index for sorting each gaussian after update (in descending order according to weight/Sigma2)
                    m_impl->GmmSort (pPixelInfo, matchIndex, channels);

                    // generate foreground mask at each pixel position
                    m_impl->GmmForegourndPixel(pPixelInfo, matchIndex, n, pregionMask == nullptr ? 255 : pregionMask[n]);

                }
            }
        }

        m_impl->GmmBackground ();

        m_impl->mFrameCount++;

    }

    // Remove shadows and highlights
    void CGmmBgModel::ShadowHighlightRemoval(const Mat& frame)
    {
        Size fsize = frame.size();
        int channels = frame.channels ();
        int gridSize = m_impl->m_param.GridSize ();
        float shadowBrightThres = m_impl->m_param.ShadowBrightThreshold ();
        float shadowChromaThres = m_impl->m_param.ShadowChromaThreshold ();
        float highightThreshold = m_impl->m_param.HighlightThreshold ();
        const uchar* pdata = frame.ptr<uchar>();
        uchar* pForeground = m_impl->mpForeground->ptr<uchar> ();
        uchar* pSgMask = NULL;
        if (m_impl->mIsCoarseDetection)
            pSgMask = m_impl->mpSgMask->ptr<uchar>();

        for (int i = 0, n = 0; i < fsize.height; ++i)
        {
            for (int j = 0; j < fsize.width; ++j, ++n)
            {
                int p = n*channels;
                int m = i/gridSize*fsize.width/gridSize + j/gridSize;

                // per pixel based shadow detection
                if (pForeground[n] != 0)
                {
                    float sumTemp1 = 0.0f;
                    float sumTemp2 = 0.0f;
                    float sum_sigma2 = 0.0f;

                    for (int d = 0; d < channels; ++d)
                    {
                        sumTemp1 += pdata[p+d]*m_impl->mpPixelInfo[n].m_info[0].m_mu[d]/m_impl->mpPixelInfo[n].m_info[0].m_sigma2[d];
                        sumTemp2 += m_impl->mpPixelInfo[n].m_info[0].m_mu[d]*m_impl->mpPixelInfo[n].m_info[0].m_mu[d]/m_impl->mpPixelInfo[n].m_info[0].m_sigma2[d];
                        sum_sigma2 += m_impl->mpPixelInfo[n].m_info[0].m_sigma2[d];
                    }

                    float brightDistortion = sumTemp1/sumTemp2;
                    float chromaDistrotion = 0.0f;
                    for (int d = 0; d < channels; ++d)
                    {
                        chromaDistrotion += (pdata[p+d] - brightDistortion*m_impl->mpPixelInfo[n].m_info[0].m_mu[d])*(pdata[p+d] - brightDistortion*m_impl->mpPixelInfo[n].m_info[0].m_mu[d]);
                    }

                    // removal shadow and highlight
                    if (m_impl->mIsCoarseDetection)
                    {
                        if (brightDistortion > shadowBrightThres && brightDistortion < highightThreshold && chromaDistrotion < shadowChromaThres*shadowChromaThres*sum_sigma2)
                        {
                            pForeground[n] = 0;
                        } else if (brightDistortion > highightThreshold && chromaDistrotion < shadowChromaThres*shadowChromaThres*sum_sigma2 && pSgMask[m] == 0)
                        {
                            pForeground[n] = 0;
                        }
                    }
                    else
                    {
                        if (brightDistortion > shadowBrightThres && brightDistortion < highightThreshold && chromaDistrotion < shadowChromaThres*shadowChromaThres*sum_sigma2)
                        {
                            pForeground[n] = 0;
                        }
                    }

                }
            }
        }
    }

    // Remove small regions and fill holes
    void CGmmBgModel::ForegroundNoiseRemoval()
    {
        // foreground median filtering
        medianBlur(*m_impl->mpForeground, *m_impl->mpForeground, m_impl->m_param.MedfiltSize ());

        //// dilate foreground to include more boundary
        //dilate(*m_impl->mpForeground, *m_impl->mpForeground, Mat());

        // find each separated foreground region using findContours
        //findContours (*m_impl->mpForeground, m_impl->mFgContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
        findContours (*m_impl->mpForeground, m_impl->mFgContours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

        m_impl->mpTempMat->setTo (0); // to record if the current foreground pixel has been looped in Neighbours()
        //FindForegroundRegions(*m_impl->mpForeground, m_impl->mFgRegions);

		// approximate contours to polygons + get bounding rects
		vector<vector<Point> > contoursPoly(m_impl->mFgContours.size());
		//vector<Rect> boundRect(m_impl->mFgContours.size());
		vector<Rect>().swap(m_impl->mFgBoxes);
		m_impl->mFgBoxes.reserve(m_impl->mFgContours.size());
		for (size_t i = 0; i < m_impl->mFgContours.size(); ++i)
		{
			approxPolyDP(Mat(m_impl->mFgContours[i]), contoursPoly[i], 3, true);
			m_impl->mFgBoxes.push_back(boundingRect(Mat(contoursPoly[i])));
		}

        // delete small regions
        //m_impl->mFgRegions.erase (remove_if(m_impl->mFgRegions.begin (), m_impl->mFgRegions.end(), LessRegion(m_impl->m_param.MinRegionWidth (), m_impl->m_param.MinRegionHeight ())), m_impl->mFgRegions.end());

        //vector<vector<Point> >::iterator it = m_impl->mFgRegions.begin ();
        //while (it != m_impl->mFgRegions.end())
        //{
        //    if (static_cast<int>((*it).size()) < m_impl->m_param.MinArea())
        //        it = m_impl->mFgRegions.erase(it);
        //    else
        //        ++it;
        //}

        //FindForegroundBoxes(m_impl->mFgRegions, m_impl->mFgBoxes);

        // delete small regions
        //vector<vector<Point> >::iterator itP = m_impl->mFgRegions.begin ();
        vector<Rect>::iterator itR = m_impl->mFgBoxes.begin ();
        while (itR != m_impl->mFgBoxes.end())
        {
            if (static_cast<int>((*itR).width) < m_impl->m_param.MinRegionWidth () || static_cast<int>((*itR).height) < m_impl->m_param.MinRegionHeight ())
            {
                itR = m_impl->mFgBoxes.erase(itR);
                //itP = m_impl->mFgRegions.erase (itP);
            }
            else
            {
                ++itR;
                //++itP;
            }
        }

		// find FgRegions
		//BOOST_FOREACH(const Rect& rect, m_impl->mFgBoxes)
		//{
		//	for (int x = rect.tl().x; x <= rect.br().x; ++x)
		//		for (int y = rect.tl().y; y <= rect.br().y; ++y)
		//			if (m_impl->mpForeground->
		//}
    }

    void CGmmBgModel::ForegroundFillHoles ()
    {
        m_impl->mpForeground->setTo(0);   // reset foreground

        // fill holes within each foreground region
        drawContours (*m_impl->mpForeground, m_impl->mFgContours, -1, Scalar(255, 255, 255), CV_FILLED);

        Mat foreground;
        m_impl->mpForeground->copyTo (foreground);
        //// find the filled regions  ????? IT IS CONTOURS NOT WHOLE PIXELS WITHIN REGIONS
        //findContours (foreground, m_impl->mFgRegions, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

    }

    void CGmmBgModel::BgModelPostUpdate (const Mat& frame)
    {
		//if(FrameCount() > 50)
  //      {
			ShadowHighlightRemoval (frame);
			ForegroundNoiseRemoval ();
			ForegroundFillHoles ();
		//}
    }

    void CGmmBgModel::FindForegroundRegions (const cv::Mat& foreground, std::vector<std::vector<cv::Point> >& regions)
    {
        Size fsize = foreground.size();
        regions.clear ();
        for (int i = 0; i < fsize.width*fsize.height; ++i)
        {
            vector<Point> points;
            m_impl->Neighbours (foreground, points, i, i);

            if (points.size() > 0)
            {
                regions.push_back (points);
            }
        }
        vector<vector<Point> >(regions).swap(regions);  // crop capacity
    }

    void CGmmBgModel::FindForegroundBoxes(std::vector<std::vector<cv::Point> >& regions, std::vector<cv::Rect>& boxes)
    {
        vector<Rect>().swap(boxes);    // clear and reduce boxes' capacity
        boxes.reserve(regions.size());
        for (size_t i = 0; i < regions.size(); ++i)
        {
            Point tl(100000, 100000);
            Point br(0,0);

            for (size_t j = 0; j < regions[i].size(); ++j)
            {
                tl.x = tl.x < regions[i][j].x ? tl.x : regions[i][j].x;
                tl.y = tl.y < regions[i][j].y ? tl.y : regions[i][j].y;
                br.x = br.x > regions[i][j].x ? br.x : regions[i][j].x;
                br.y = br.y > regions[i][j].y ? br.y : regions[i][j].y;
            }
            Rect temp(tl, br);

            boxes.push_back(temp);
        }
    }

    void CGmmBgModel::GetForegroundPoints(const std::vector<std::vector<cv::Point> >& regions, std::vector<cv::Point2f>& points)
    {
        vector<Point2f>().swap (points);
        for (size_t i = 0; i < regions.size(); ++i)
        {
            /*for (cv::vector<cv::Point>::const_iterator it = regions[i].begin(); it != regions[i].end(); ++it)
            {
                points.push_back (*it);
            }*/

            vector<Point>::size_type exsize = regions[i].size();
            size_t psize = points.size();
            points.resize(psize+exsize);
            copy(regions[i].begin(), regions[i].end(), points.begin()+psize);
        }

    }

	bool CGmmBgModel::IsForgroundOverCrowded() const
	{
		return static_cast<float>(cv::sum(*m_impl->mpForeground)[0]) / (m_impl->mpForeground->size().height * m_impl->mpForeground->size().width * 255) > 0.25;
	}

    const long CGmmBgModel::FrameCount() const
    {
        return m_impl->mFrameCount;
    }

    const Mat& CGmmBgModel::Background () const
    {
        return *(m_impl->mpBackground);
    }
    Mat& CGmmBgModel::Background ()
    {
        return *(m_impl->mpBackground);
    }

    const Mat& CGmmBgModel::Foreground () const
    {
        return *(m_impl->mpForeground);
    }
    Mat& CGmmBgModel::Foreground ()
    {
        return *(m_impl->mpForeground);
    }

    const Mat* CGmmBgModel::SgMask () const
    {
		if (m_impl->mpSgMask)
			return m_impl->mpSgMask.get();
		else
			return NULL;
    }

    const Mat* CGmmBgModel::FrameGray() const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpTempInfo->m_frameGray.get();
        else
            return NULL;
    }

    const Mat* CGmmBgModel::BackgroundGray () const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpTempInfo->m_backgroundGray.get();
        else
            return NULL;
    }

    const cv::Mat* CGmmBgModel::FrameGradientX() const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpTempInfo->m_frameGradientX.get();
        else
            return NULL;
    }

    const cv::Mat* CGmmBgModel::FrameGradientY() const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpTempInfo->m_frameGradientY.get();
        else
            return NULL;
    }

    const cv::Mat* CGmmBgModel::FrameGradientMag() const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpTempInfo->m_frameGradientMag.get();
        else
            return NULL;
    }

    const cv::Mat* CGmmBgModel::BackgroundGradientX() const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpTempInfo->m_backgroundGradientX.get();
        else
            return NULL;
    }

    const cv::Mat* CGmmBgModel::BackgroundGradientY() const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpTempInfo->m_backgroundGradientY.get();
        else
            return NULL;
    }

    const cv::Mat* CGmmBgModel::BackgroundGradientMag() const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpTempInfo->m_backgroundGradientMag.get();
        else
            return NULL;
    }

    const Mat* CGmmBgModel::CFMaskF() const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpCFMaskF.get();
        else
            return NULL;
    }
    const Mat* CGmmBgModel::UpdateMask() const
    {
        if (m_impl->mpTempInfo)
            return m_impl->mpUpdateMask.get();
        else
            return NULL;
    }

    //const vector<vector<Point> >& CGmmBgModel::ForegroundRegions() const
    //{
    //    return m_impl->mFgRegions;
    //}

    const vector<Rect>& CGmmBgModel::ForegroundBoxes () const
    {
        return m_impl->mFgBoxes;
    }

}