#include "darknet.hpp"

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/video.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/tracking.hpp>
#include <opencv2/cudabgsegm.hpp>
//#include <opencv2/cudaimgproc.hpp>
//#include <opencv2/cudawarping.hpp>

#include <iostream>

using namespace cv;
using namespace std;
using namespace cv::cuda;

#include <time.h>
#include <stdlib.h>
#include <stdio.h>

#define MAX_OBJ	(512)
#define MAX_POINT	(640)

#define MAX_CAR_SHOW	(32)
#define CAR_WIN_SIZE	(80)

#define m_minObjectSize_X	(64)
#define m_minObjectSize_Y	(64)

#define FRAME_to_CONFIRM	(6)
#define FRAME_to_DEL_OBJ	(10)
//#define FRAME_to_DEL_OBJ	(15)
//#define FRAME_to_DEL_OBJ	(64)
#define MAX_SHIFT	(40)	// sensitive !!!
#define NUM_COLOR	(24)
#define AREA_RATIO_TO_REMOVE_OBJECT	(0.4)
#define BIG_IOU_OVERLAP	(0.60)

typedef struct	{
	int x, y, xMax, yMax;
	int type;	// 0 - people;	1 - car or truck;  2- bus
	int prob;
}OBJECT,*POBJECT;

typedef struct	{
	int Used, Confirmed;
	int Idx;	// index starting from 1 !!!!!!!!!
	int Area;
	int Area_Max;
	int vx, vy;
	int x, y, width, height, xMax, yMax;
	int xMin_cmp, xMax_cmp, yMin_cmp, yMax_cmp;
	int FirstFrame, LastFrame;
	int nFounded;	// the number of times detected
	int nNotFounded;	// the number of times NOT detected
	int traj_Pos;
//	CvPoint trace[MAX_POINT];
	Rect2d boundingBox;
//	Ptr<Tracker> tracker;
	Mat Img;
}TRACKER, *PTRACKER;

void Init_ObjList(void);
int Find_Available_Space(void);
int Query_Object(int x, int y, int width, int height);
int Add_an_Object(int x, int y, int width, int height);
void Remove_an_Object(int idx);
void Update_an_Object(int idx, int x, int y, int width, int height);
void Update_All_Trackers(void);
void Get_RGB_Color(int idx, int *R, int *G, int *B);
void Show_Obj_Index(void);
void Gen_FG_Mask_from_Diff(void);
double Get_White_Pixle_Ratio(Mat *pMat, int xMin, int yMin, int width, int height);	// to determine whether this object is in FG or not
void Match_ObjList_and_Yolo_List(void);
void do_Img_Templet_Match(void);

int nObj, nObj_Confirmed=0;
TRACKER ObjList[MAX_OBJ];

int nObj_Yolo=0;
OBJECT Yolo_ObjList[MAX_OBJ];
int Assigned_Yolo_Obj[MAX_OBJ];

int NotFoundObj;
int FoundObj[MAX_OBJ];
int IdxAvailable;
int Color_List[]={0xd8b2d8, 0x008aa0, 0xac8141, 0x1e90ff, 0x838da6, 0x8b0000, 0xffcccc, 0xe5f2e5, 0xcce5cc, 0xfbe6fb, 
0xffffcc, 0xe5e5ff, 0xffc0cb, 0xf37736, 0xf8d59b, 0x0d6032, 0xc6540d, 0xeb2348, 0x368059, 0x306a9f, 0x67f9f5, 0x8a2be2, 0x66cccc, 0x6dc066};

image im;

Mat img_all;	// the frame combined with car gallery
Mat frame; //current frame
Mat clean_bg;
Mat diff;	// FG mask generated by frame - clean_bg
Mat fgMaskMOG2; //fg mask fg mask generated by MOG2 method

int nCarMax;
Mat car_gallery[MAX_CAR_SHOW], car_List;


GpuMat d_frame;
GpuMat d_fgmask;

Ptr<cuda::BackgroundSubtractorMOG2> pMOG2; //MOG2 Background subtractor
double Learning_Rate=0.01;


double fps, fps_inv, t_video;
int width, height;
int nFrame=0;
char tracker_algorithm[]="KCF";

void Mat_into_image(Mat src, image im);
void Read_Yolo_Detection(FILE *fIn);
void Check_Yolo_Ojbects_Overlap(void);
void Draw_Car_Gallery(void);

int main(int argc, char **argv)
{
    float thresh=0.24;
    float hier_thresh=0.50;
    list *options = read_data_cfg("cfg/coco.data");
    char *name_list = option_find_str(options, "names", "data/names.list");
    char **names = get_labels(name_list);
    clock_t time;
    char buff[256], szTxt[128];
    int i,j;
    float nms=.4;
    int keyboard; //input from keyboard
    int ObjIdx;
    int nReadItem, Frame;
    double white_ratio;
    FILE *fOut=NULL, *fIn=NULL;


    image **alphabet = load_alphabet();
    network net = parse_network_cfg("cfg/yolo.cfg");
    load_weights(&net, "/work/00410/huang/maverick/yolo/yolo.weights");
    
    set_batch_network(&net, 1);
    layer l = net.layers[net.n-1];
    srand(2222222);
	
    gpu_index = 0;
    cuda_set_device(gpu_index);


    if(argc > 2)	{
		    clean_bg = imread(argv[2], CV_LOAD_IMAGE_UNCHANGED);
    	  if( clean_bg.empty() )	{
			      printf("Fail to open file: %s\n", argv[2]);
		    }
        diff.create(clean_bg.size(), CV_8UC1);	// gray
	  }

    VideoCapture capture(argv[1]);
    if(!capture.isOpened()){
        //error in opening the video input
        cerr << "Unable to open video file: " << argv[1] << endl;
        exit(EXIT_FAILURE);
    }
    width = capture.get(CV_CAP_PROP_FRAME_WIDTH);
    height = capture.get(CV_CAP_PROP_FRAME_HEIGHT);
    fps = capture.get(CV_CAP_PROP_FPS);
    fps_inv = 1.0/fps;
    
    nCarMax = (int)(width/CAR_WIN_SIZE);
    img_all = Mat::zeros(Size(width, height+CAR_WIN_SIZE), CV_8UC3);
    car_List = Mat::zeros(Size(width, CAR_WIN_SIZE), CV_8UC3);
    
    im = make_image(width, height, 3);	// color image. three channels
    
	  pMOG2 = cuda::createBackgroundSubtractorMOG2(2400);
    
    fOut = fopen("yolo.log", "w");
//    fIn = fopen("yolo.log", "r");
//    if(fIn == NULL)	{
//    	printf("Fail to open file: %s for Yolo detection records.\nExit\n", "yolo.log");
//    }

//		VideoWriter video("out.mov", CV_FOURCC('H','2','6','4'), fps, Size(width,height), true);
//		VideoWriter video("out.mov", CV_FOURCC('H','2','6','4'), fps, Size(width,height+CAR_WIN_SIZE), true);
		
    while( (char)keyboard != 'q' && (char)keyboard != 27 ){
        //read the current frame
        if(!capture.read(frame)) {
            cerr << "Unable to read next frame." << endl;
            cerr << "Exiting..." << endl;
//            fclose(fOut);
            if(fIn)	fclose(fIn);
            exit(EXIT_FAILURE);
        }
        nFrame++;
//   			sprintf(szTxt, "%5d", nFrame);
//        putText(frame, szTxt, cv::Point( 48, 48), FONT_HERSHEY_SIMPLEX, 2.0 , cv::Scalar(0,255,0));;
        
        t_video += fps_inv;
//        ss << capture.get(CAP_PROP_POS_FRAMES);
//        string frameNumberString = ss.str();
//        putText(frame, frameNumberString.c_str(), cv::Point(15, 15),
//                FONT_HERSHEY_SIMPLEX, 0.5 , cv::Scalar(0,0,0));

        time=clock();
/*        
        if(nFrame == 1)	{
        	d_frame.create(frame.size(), frame.type());
        }
        d_frame.upload(frame);
        pMOG2->apply(d_frame, d_fgmask, Learning_Rate);
		    d_fgmask.download(fgMaskMOG2);
*/                

        Mat_into_image(frame, im);
        image sized = letterbox_image(im, net.w, net.h);
    
        box *boxes = (box *)calloc(l.w*l.h*l.n, sizeof(box));
        float **probs = (float **)calloc(l.w*l.h*l.n, sizeof(float *));
        for(j = 0; j < l.w*l.h*l.n; ++j) probs[j] = (float *)calloc(l.classes + 1, sizeof(float *));
    
        float *X = sized.data;
        network_predict(net, X);
        get_region_boxes(l, im.w, im.h, net.w, net.h, thresh, probs, boxes, 0, 0, hier_thresh, 1);
        if (nms) do_nms_obj(boxes, probs, l.w*l.h*l.n, l.classes, nms);
        //else if (nms) do_nms_sort(boxes, probs, l.w*l.h*l.n, l.classes, nms);
    
        draw_detections_Ex(im, l.w*l.h*l.n, thresh, boxes, probs, names, alphabet, l.classes, &nObj_Yolo, (int*)Yolo_ObjList);

//        Read_Yolo_Detection(fIn);
//        if(nFrame < 3)	continue;
//        Check_Yolo_Ojbects_Overlap();
        
        fprintf(fOut, "%5d %3d\n", nFrame, nObj_Yolo);
        for(i=0; i<nObj_Yolo; i++) {
           fprintf(fOut, "%d %d %d %d %d %d\n", Yolo_ObjList[i].type, Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax, Yolo_ObjList[i].yMax, Yolo_ObjList[i].prob);
        }

/*
        memset(FoundObj, 0, sizeof(int)*MAX_OBJ);
        memset(Assigned_Yolo_Obj, 0, sizeof(int)*nObj_Yolo);
        for(i=0; i<nObj_Yolo; i++) {
           if(Yolo_ObjList[i].type > 0)	{	// car, truck or bus. Person is not included.
           	  white_ratio = Get_White_Pixle_Ratio(&fgMaskMOG2, Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
//        			sprintf(szTxt, "%4.2lf", white_ratio);
//			        putText(frame, szTxt, cv::Point( (Yolo_ObjList[i].x+Yolo_ObjList[i].xMax)/2-8, (Yolo_ObjList[i].y+Yolo_ObjList[i].yMax)/2-8), FONT_HERSHEY_SIMPLEX, 1.0 , cv::Scalar(0,255,0));;
//           	  if( (white_ratio > 0.15) && (Yolo_ObjList[i].prob > 0.55*100) && ( (Yolo_ObjList[i].xMax-Yolo_ObjList[i].x) > m_minObjectSize_X) && ( (Yolo_ObjList[i].yMax-Yolo_ObjList[i].y) > m_minObjectSize_Y) ) {
//           	  if( (white_ratio > 0.15) && (Yolo_ObjList[i].prob > 0.60*100) && ( (Yolo_ObjList[i].xMax-Yolo_ObjList[i].x) > m_minObjectSize_X) && ( (Yolo_ObjList[i].yMax-Yolo_ObjList[i].y) > m_minObjectSize_Y) ) {	// handle high quality object first. Possible new objects?
           	  if( (white_ratio > 0.15) && (Yolo_ObjList[i].prob > 0.60*100) && ( (Yolo_ObjList[i].xMax-Yolo_ObjList[i].x) > m_minObjectSize_X) && ( (Yolo_ObjList[i].yMax-Yolo_ObjList[i].y) > m_minObjectSize_Y) ) {	// handle high quality object first. Possible new objects?
			    			ObjIdx = Query_Object(Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
	
				    		if(ObjIdx < 0)	{	// Not exist
						     	printf("Add an object (%d, %d, %d, %d)\n", Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
						     	ObjIdx = Add_an_Object(Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
					     	}
						    else	{
						    	Update_an_Object(ObjIdx, Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
						    }
						    FoundObj[ObjIdx] = 1;
						    Assigned_Yolo_Obj[i] = 1;
           	  	
//           	  	rectangle(frame, Point(Yolo_ObjList[i].x, Yolo_ObjList[i].y), Point( Yolo_ObjList[i].xMax, Yolo_ObjList[i].yMax), Scalar(255, 0, 0), 2, 8, 0 );
//        			  sprintf(szTxt, "%d", Yolo_ObjList[i].prob);
//			          putText(frame, szTxt, cv::Point( (Yolo_ObjList[i].x+Yolo_ObjList[i].xMax)/2-8, (Yolo_ObjList[i].y+Yolo_ObjList[i].yMax)/2-8), FONT_HERSHEY_SIMPLEX, 1.0 , cv::Scalar(0,255,0));;
           	  }
           }
        }
        
        NotFoundObj=0;
        Match_ObjList_and_Yolo_List();
        do_Img_Templet_Match();
        Show_Obj_Index();

/*
        if(nFrame == 1)	{	// add all objects
	        for(i=0; i<nObj_Yolo; i++)	{
            if(Yolo_ObjList[i].type > 0) {
           	  rectangle(frame, Point(Yolo_ObjList[i].x, Yolo_ObjList[i].y), Point( Yolo_ObjList[i].xMax, Yolo_ObjList[i].yMax), Scalar(255, 0, 0), 2, 8, 0 );
 			     	  ObjIdx = Add_an_Object(Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
				      FoundObj[ObjIdx] = 1;
            }
	        }
        }
*/        
//        else	{
//					Match_ObjList_and_Yolo_List();
//        }


//        if(nFrame > 1)	Update_All_Trackers(); 
/*        	
        Gen_FG_Mask_from_Diff();
    
        memset(FoundObj, 0, sizeof(int)*MAX_OBJ);
        for(i=0; i<nObj_Yolo; i++) {
//           printf("%d: class %d (%d,%d,%d,%d) prob: %d%%\n", i+1, Yolo_ObjList[i].type, Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax, Yolo_ObjList[i].yMax, Yolo_ObjList[i].prob);
//           rectangle(frame, Point(Yolo_ObjList[i].x, Yolo_ObjList[i].y), Point( Yolo_ObjList[i].xMax, Yolo_ObjList[i].yMax), Scalar(255, 0, 0), 2, 8, 0 );
           if(Yolo_ObjList[i].type > 0)	{	// car, truck or bus. Person is not included.
           	  white_ratio = Get_White_Pixle_Ratio(&diff, Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
//        			sprintf(szTxt, "%4.2lf", white_ratio);
//			        putText(frame, szTxt, cv::Point( (Yolo_ObjList[i].x+Yolo_ObjList[i].xMax)/2-6, (Yolo_ObjList[i].y+Yolo_ObjList[i].yMax)/2-6), FONT_HERSHEY_SIMPLEX, 1.5 , cv::Scalar(0,255,0));;
           	  
           	  if(white_ratio > 0.03)	{ // in FG
			    			ObjIdx = Query_Object(Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
	
				    		if(ObjIdx < 0)	{	// Not exist
						     	printf("Add an object (%d, %d, %d, %d)\n", Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
						     	ObjIdx = Add_an_Object(Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
					     	}
						    else	{
						    	Update_an_Object(ObjIdx, Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
						    }
						    FoundObj[ObjIdx] = 1;
           	  }
           }
        }
//        printf("Predicted in %f seconds.\n", sec(clock()-time));
        printf("nObj = %d\n", nObj);


        Show_Obj_Index();
*/

        free_image(sized);
        free(boxes);
        free_ptrs((void **)probs, l.w*l.h*l.n);

//				waitKey(30);
//        if(NotFoundObj > 0) keyboard=waitKey(0);

/*
				frame.copyTo( img_all( Rect(0, 0, width, height) ) );
        Draw_Car_Gallery();
        imshow("Detection", img_all);
//        imshow("FG", fgMaskMOG2);
        keyboard=waitKey(5);
				video.write(img_all);
				if(nFrame > 8000)	{
//				if(nFrame > 1600)	{
					break;
				}
*/
//        keyboard=waitKey(30);

//        keyboard = waitKey( 30 );
//        if(nFrame%20==0) keyboard = waitKey( 0 );
//        if(keyboard == 'p') waitKey(0);
        if(nFrame >= 200)	break;
    }
    capture.release();

		fclose(fOut);


//    save_image(im, "output");

//    free_image(im);

    
//    imshow("MyWindow", img);
//    waitKey(0);


    free_image(im);

    return 0;
}

void Read_Yolo_Detection(FILE *fIn)
{
	int Frame_Yolo;
	int i, nReadItem;
	
	nReadItem = fscanf(fIn, "%d%d", &Frame_Yolo, &nObj_Yolo);
	if(nReadItem != 2)	{
		printf("Error in reading Yolo detection record.\nExit\n");
		fclose(fIn);
		exit(1);
	}
	if(Frame_Yolo != nFrame)	{
		printf("Error in reading Yolo detection record.\nFrame_Yolo != nFrame\n");
		fclose(fIn);
		exit(1);
	}
	
	for(i=0; i<nObj_Yolo; i++)	{
		nReadItem = fscanf(fIn, "%d%d%d%d%d%d", 
			&(Yolo_ObjList[i].type), &(Yolo_ObjList[i].x), &(Yolo_ObjList[i].y), &(Yolo_ObjList[i].xMax), &(Yolo_ObjList[i].yMax), &(Yolo_ObjList[i].prob) );
		if(nReadItem != 6)	{
			printf("Error in reading Yolo detection record. Frame = %d\nExit\n", Frame_Yolo);
			fclose(fIn);
			exit(1);
		}
	}
}

void Check_Yolo_Ojbects_Overlap(void)
{
  int i,j;
  float iou;
  box box_a, box_b;
  
  for(i=0; i<nObj_Yolo; i++)	{
  	box_a.x = Yolo_ObjList[i].x;
  	box_a.y = Yolo_ObjList[i].y;
  	box_a.w = Yolo_ObjList[i].xMax - Yolo_ObjList[i].x;
  	box_a.h = Yolo_ObjList[i].yMax - Yolo_ObjList[i].y;
  	for(j=i+1; j<nObj_Yolo; j++)	{
	  	box_b.x = Yolo_ObjList[j].x;
	  	box_b.y = Yolo_ObjList[j].y;
	  	box_b.w = Yolo_ObjList[j].xMax - Yolo_ObjList[j].x;
	  	box_b.h = Yolo_ObjList[j].yMax - Yolo_ObjList[j].y;
	  	
	  	iou = box_iou(box_a, box_b);
	  	if(iou > 0.6)	{
	  		rectangle(frame, Point(Yolo_ObjList[i].x, Yolo_ObjList[i].y), Point( Yolo_ObjList[i].xMax, Yolo_ObjList[i].yMax), Scalar(255, 0, 0), 2, 8, 0 );
	  		rectangle(frame, Point(Yolo_ObjList[j].x, Yolo_ObjList[j].y), Point( Yolo_ObjList[j].xMax, Yolo_ObjList[j].yMax), Scalar(255, 0, 0), 2, 8, 0 );
	  	}
  	}
  }
}

void Mat_into_image(Mat src, image im)
{
    unsigned char *data = (unsigned char *)(src.data);
    int h = src.rows;
    int w = src.cols;
    int c = src.channels();
    int step = src.step;
    int i, j, k, w_h, w_h_2;
    
    w_h = w * h;
    w_h_2 = w_h*2;

    for(i = 0; i < h; ++i){
        for(j = 0; j < w; ++j){
            for(k= 0; k < c; ++k){
                im.data[k*w*h + i*w + j] = data[i*step + j*c + k]/255.;	// convert to float
            }
        }
    }

    k = 0;	// Red
    for(i = 0; i < h; ++i){
        for(j = 0; j < w; ++j){
             im.data[i*w + j] = data[i*step + j*c + 2]/255.;	// convert to float
        }
    }

    k = 1;	// Green
    for(i = 0; i < h; ++i){
        for(j = 0; j < w; ++j){
             im.data[w_h + i*w + j] = data[i*step + j*c + 1]/255.;	// convert to float
        }
    }

    k = 2;	// Blue
    for(i = 0; i < h; ++i){
        for(j = 0; j < w; ++j){
             im.data[w_h_2 + i*w + j] = data[i*step + j*c    ]/255.;	// convert to float
        }
    }
//    rgbgr_image(out);	// swap R<->B
}


void Init_ObjList(void)
{
	int i;

	nObj = 0;

	for(i=0; i<MAX_OBJ; i++)	{
		ObjList[i].Used = 0;
		ObjList[i].Confirmed = 0;
		ObjList[i].Idx = -1;
		ObjList[i].nNotFounded = 0;
//		ObjList[i].tracker = NULL;
	}

	IdxAvailable = 0;
}

void Remove_an_Object(int idx)
{
	ObjList[idx].Used = 0;
	ObjList[idx].Confirmed = 0;
	ObjList[idx].Idx = -1;
	ObjList[idx].Img.~Mat();
}

int Find_Available_Space(void)
{
	int i;

	for(i=0; i<MAX_OBJ; i++)	{
		if(ObjList[i].Used == 0)	{
			return i;
		}
	}
	printf("Fail to find an available space for a new object!\n");
	exit(1);
	return -1;
}

int Query_Object(int x, int y, int width, int height)	// to avoid smaller subset or big overlap
{
  int i;
  float iou;
  box box_a, box_b;
	PTRACKER pTracker;
	int xc, yc, xc_2, yc_2, dist_SQ, dist_SQ_CUT;

  box_a.x = x;
  box_a.y = y;
  box_a.w = width;
  box_a.h = height;

	xc = (int)(x + 0.5*width);
	yc = (int)(y + 0.5*height);

  dist_SQ_CUT = (int)(width*height*0.25);
	for(i=0; i<MAX_OBJ; i++)	{
		if(ObjList[i].Used)	{
			pTracker = &(ObjList[i]);
			
			box_b.x = pTracker->x + pTracker->vx;
			box_b.y = pTracker->y + pTracker->vy;
			box_b.w = pTracker->width;
			box_b.h = pTracker->height;
			
		  iou = box_iou(box_a, box_b);
		  if(iou > BIG_IOU_OVERLAP)	{	// large overlap. The same object?
				pTracker->LastFrame = nFrame;
				pTracker->nFounded++;
				if(pTracker->nFounded > FRAME_to_CONFIRM)	{
					if(pTracker->Confirmed == 0)	{
						nObj_Confirmed++;
						pTracker->Idx = nObj_Confirmed;
						
						// Add into car gallery
						int width_new, height_new, x_new, y_new;
						Mat car=Mat(frame, Rect(x, y, width, height));
						if(width > height)	{
							width_new = CAR_WIN_SIZE;
							height_new = (int)(1.0*CAR_WIN_SIZE*height/width);
							x_new = 0;
							y_new = (width_new-height_new)/2;
						}
						else	{
							height_new = CAR_WIN_SIZE;
							width_new = (int)(1.0*CAR_WIN_SIZE*width/height);
							x_new = (height_new-width_new)/2;
							y_new = 0;
						}
						if(! car_gallery[(nObj_Confirmed-1)%nCarMax].empty())	{
							car_gallery[(nObj_Confirmed-1)%nCarMax].~Mat();
						}
						car_gallery[(nObj_Confirmed-1)%nCarMax] = Mat::zeros(Size(CAR_WIN_SIZE, CAR_WIN_SIZE), frame.type());
						Mat icon;
						resize(car, icon, Size(width_new, height_new));//resize image
						icon.copyTo( car_gallery[(nObj_Confirmed-1)%nCarMax]( Rect(x_new, y_new, width_new, height_new) ) );
					}
					pTracker->Confirmed = 1;
				}
       	pTracker->nNotFounded = 0;
				return i;
			}
			
		 	xc_2 = (int)( (pTracker->x + pTracker->xMax)/2 + pTracker->vx);
		 	yc_2 = (int)( (pTracker->y + pTracker->yMax)/2 + pTracker->vy);
		 	dist_SQ = (xc-xc_2)*(xc-xc_2) + (yc-yc_2)*(yc-yc_2);
		 	if(dist_SQ < dist_SQ_CUT)	{	// the centers are so close. 
       	pTracker->nNotFounded = 0;
				return i;
		 	}
			
		}
	}

	return (-1);
}

/*
int Query_Object(int x, int y, int width, int height)	// to avoid smaller subset or big overlap
{
	int i, xMax, yMax;
	int vx, vy;
	PTRACKER pTracker;
	static int count=0;
	int xc, yc, xc_2, yc_2, dist_SQ;

	xMax = x + width;
	yMax = y + height;

	for(i=0; i<MAX_OBJ; i++)	{
		if(ObjList[i].Used)	{
			pTracker = &(ObjList[i]);
			vx = pTracker->vx;
			vy = pTracker->vy;

			if( ( (xMax-vx) < pTracker->xMax_cmp) && ( (x-vx) > pTracker->xMin_cmp) && ( (yMax-vy) < pTracker->yMax_cmp) && ( (y-vy) > pTracker->yMin_cmp) )	{	// big overlap
//				printf("Found %d nFrame = %d\n", i, nFrame);
				pTracker->LastFrame = nFrame;
				pTracker->nFounded++;
				if(pTracker->nFounded > FRAME_to_CONFIRM)	{
					if(pTracker->Confirmed == 0)	{
						nObj_Confirmed++;
						pTracker->Idx = nObj_Confirmed;
					}
					pTracker->Confirmed = 1;
				}
				return i;
			}
		}
	}
	
	if(nFrame > 1)	{	// search the predicted box too. 
		xc = (int)(x + 0.5*width);
		yc = (int)(y + 0.5*height);
		
		for(i=0; i<MAX_OBJ; i++)	{
			if(ObjList[i].Used)	{
				pTracker = &(ObjList[i]);
		 		xc_2 = pTracker->boundingBox.x + 0.5*pTracker->boundingBox.width;
		 		yc_2 = pTracker->boundingBox.y + 0.5*pTracker->boundingBox.height;
		 		dist_SQ = (xc-xc_2)*(xc-xc_2) + (yc-yc_2)*(yc-yc_2);
		 		if(dist_SQ < 625)	{	// a cutoff
					return i;
		 		}
			}
		}

	}

	count++;

//	printf("Not found. nFrame = %d count = %d\n", nFrame, count);
	return (-1);
}
*/

/*
void Update_All_Trackers(void)
{
	int i;
	PTRACKER pTracker;
	
#pragma omp parallel for private(i, pTracker) schedule(static,1)
	for(i=0; i<MAX_OBJ; i++)	{
		if(ObjList[i].Used)	{
			pTracker = &(ObjList[i]);
			pTracker->tracker->update( frame, pTracker->boundingBox );	// update the position of central part with KCF tracker
			rectangle(frame, Point(pTracker->boundingBox.x, pTracker->boundingBox.y), Point( pTracker->boundingBox.x+pTracker->boundingBox.width, pTracker->boundingBox.y+pTracker->boundingBox.height), Scalar(255, 0, 0), 3, 8, 0 );
		}
	}
}
*/

#define L_WIDENED	(128)	// defines search range
void do_Img_Templet_Match(void)
{
	int i;
	PTRACKER pTracker;
	
#pragma omp parallel for private(i, pTracker) schedule(static,1)
	for(i=0; i<MAX_OBJ; i++)	{
		if(ObjList[i].Used && (FoundObj[i] == 0) )	{
			int x, y, xMax, yMax;
			char szTxt[64];
  		
			pTracker = &(ObjList[i]);
			x = max(0, pTracker->x - L_WIDENED);
			xMax = min(frame.cols-1, pTracker->xMax + L_WIDENED);
			y = max(0, pTracker->y - L_WIDENED);
			yMax = min(frame.rows-1, pTracker->yMax + L_WIDENED);
  		Mat result;
  		Mat search=Mat(frame, Rect(x, y, xMax-x, yMax-y));
			
//			printf("Info of big image: %d %d %d\n", search.depth(), search.type(), search.dims);
//			printf("Info of small image: %d %d %d\n", pTracker->Img.depth(), pTracker->Img.type(), pTracker->Img.dims);
//			if(pTracker->Img.dims == 0)	{
//				printf("Wrong object: %d\n", i);
//			}
			
      matchTemplate( search, pTracker->Img, result, CV_TM_CCORR_NORMED);
//      normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );
      double minVal; double maxVal; Point minLoc; Point maxLoc;
      Point matchLoc;
      minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );
      matchLoc = maxLoc;
      printf("Box(%d,%d,%d,%d) Found at (%d,%d) maxVal = %4.2lf\n", 
      pTracker->x, pTracker->y, pTracker->xMax, pTracker->yMax, matchLoc.x+x, matchLoc.y+y, maxVal);
// 			sprintf(szTxt, "%4.2lf", maxVal);
//      putText(frame, szTxt, cv::Point( matchLoc.x+x-16, matchLoc.x+y-16), FONT_HERSHEY_SIMPLEX, 1.0 , cv::Scalar(0,255,0));;
//      rectangle(frame, Point(matchLoc.x+x, matchLoc.y+y), Point( matchLoc.x + x + pTracker->Img.cols , matchLoc.y + y + pTracker->Img.rows ), Scalar(0,0,255), 2, 8, 0 );
      
      int x_new, y_new;
      x_new = (int)( 0.5*pTracker->x + 0.5*(matchLoc.x+x) );
      y_new = (int)( 0.5*pTracker->y + 0.5*(matchLoc.y+y) );
//      if(maxVal > 0.8) Update_an_Object(i, matchLoc.x+x, matchLoc.y+y, pTracker->width, pTracker->height);
      if(maxVal > 0.8) Update_an_Object(i, x_new, y_new, pTracker->width, pTracker->height);
		}
	}
}

void Match_ObjList_and_Yolo_List(void)
{
	int i, j, xc, yc, dist_SQ, dist_SQ_Min, xc_2, yc_2, Found, ObjIdx, Dist_SQ_CUT, idx_Save;
	PTRACKER pTracker;
  float iou;
  box box_a, box_b;

  for(j=0; j<MAX_OBJ; j++)	{
  	if(ObjList[j].Used && (FoundObj[j] == 0) )	{	// not assigned yet
 			pTracker = &(ObjList[j]);
// 			xc_2 = pTracker->boundingBox.x + 0.5*pTracker->boundingBox.width + pTracker->vx;
// 			yc_2 = pTracker->boundingBox.y + 0.5*pTracker->boundingBox.height + pTracker->vy;
 			xc_2 = pTracker->x + 0.5*pTracker->width + pTracker->vx;
 			yc_2 = pTracker->y + 0.5*pTracker->height + pTracker->vy;

  		box_a.x = pTracker->x + pTracker->vx;
  		box_a.y = pTracker->y + pTracker->vy;
  		box_a.w = pTracker->width;
  		box_a.h = pTracker->height;

    	Found = 0;
    	dist_SQ_Min = 100000;	// initialization with a large enough value
    	Dist_SQ_CUT = (int)(pTracker->width*pTracker->height*0.25);	// !!!!!!!!!!!!!!!!!!!
			for(i=0; i<nObj_Yolo; i++)	{
    		if( (Yolo_ObjList[i].type > 0) && (Assigned_Yolo_Obj[i]==0) )	{	// car, truck or bus. Person is not included.
		  		box_b.x = Yolo_ObjList[i].x;
  				box_b.y = Yolo_ObjList[i].y;
  				box_b.w = Yolo_ObjList[i].xMax-Yolo_ObjList[i].x;
  				box_b.h = Yolo_ObjList[i].yMax-Yolo_ObjList[i].y;
    			
				  iou = box_iou(box_a, box_b);
				  if(iou > BIG_IOU_OVERLAP)	{
 						idx_Save = i;
 						dist_SQ_Min = Dist_SQ_CUT - 1;
 						break;
				  }
    			
		    	xc = (Yolo_ObjList[i].x + Yolo_ObjList[i].xMax)/2;
		    	yc = (Yolo_ObjList[i].y + Yolo_ObjList[i].yMax)/2;
 					dist_SQ = (xc-xc_2)*(xc-xc_2) + (yc-yc_2)*(yc-yc_2);
 					if(dist_SQ < dist_SQ_Min)	{
 						idx_Save = i;
 						dist_SQ_Min = dist_SQ;
 					}
    		}
    	}
 			if(dist_SQ_Min < Dist_SQ_CUT)	{	// a cutoff
 				FoundObj[j] = 1;
 				Assigned_Yolo_Obj[idx_Save] = 1;
 				Update_an_Object(j, Yolo_ObjList[idx_Save].x, Yolo_ObjList[idx_Save].y, Yolo_ObjList[idx_Save].xMax - Yolo_ObjList[idx_Save].x, Yolo_ObjList[idx_Save].yMax-Yolo_ObjList[idx_Save].y);
       	Found = 1;
       	break;
 			}
 			else	{
// 				rectangle(frame, Point(pTracker->x, pTracker->y), Point(pTracker->xMax+pTracker->vx, pTracker->yMax+pTracker->vy), Scalar(0, 0, 255), 3, 8, 0 );
    		NotFoundObj++;
    	}
 		}
 	}

	printf("nFrame = %5d  NotFoundObj = %d\n", nFrame, NotFoundObj);
}

/*
void Match_ObjList_and_Yolo_List(void)
{
	int i, j, xc, yc, dist_SQ, dist_SQ_Min, xc_2, yc_2, Found, ObjIdx, Dist_SQ_CUT, idx_Save;
	PTRACKER pTracker;

  for(j=0; j<MAX_OBJ; j++)	{
  	if(ObjList[j].Used && (FoundObj[j] == 0) )	{	// not assigned yet
 			pTracker = &(ObjList[j]);
 			xc_2 = pTracker->boundingBox.x + 0.5*pTracker->boundingBox.width + pTracker->vx;
 			yc_2 = pTracker->boundingBox.y + 0.5*pTracker->boundingBox.height + pTracker->vy;
 			
    	Found = 0;
    	dist_SQ_Min = 100000;	// initialization with a large enough value
    	Dist_SQ_CUT = (int)(pTracker->width*pTracker->height*0.25);	// !!!!!!!!!!!!!!!!!!!
			for(i=0; i<nObj_Yolo; i++)	{
    		if( (Yolo_ObjList[i].type > 0) && (Assigned_Yolo_Obj[i]==0) )	{	// car, truck or bus. Person is not included.
		    	xc = (Yolo_ObjList[i].x + Yolo_ObjList[i].xMax)/2;
		    	yc = (Yolo_ObjList[i].y + Yolo_ObjList[i].yMax)/2;
 					dist_SQ = (xc-xc_2)*(xc-xc_2) + (yc-yc_2)*(yc-yc_2);
 					if(dist_SQ < dist_SQ_Min)	{
 						idx_Save = i;
 						dist_SQ_Min = dist_SQ;
 					}
    		}
    	}
 			if(dist_SQ_Min < Dist_SQ_CUT)	{	// a cutoff
 				FoundObj[j] = 1;
 				Assigned_Yolo_Obj[idx_Save] = 1;
 				Update_an_Object(j, Yolo_ObjList[idx_Save].x, Yolo_ObjList[idx_Save].y, Yolo_ObjList[idx_Save].xMax - Yolo_ObjList[idx_Save].x, Yolo_ObjList[idx_Save].yMax-Yolo_ObjList[idx_Save].y);;
       	Found = 1;
       	break;
 			}
 			else	{
 				rectangle(frame, Point(pTracker->x, pTracker->y), Point(pTracker->xMax+pTracker->vx, pTracker->yMax+pTracker->vy), Scalar(0, 0, 255), 3, 8, 0 );
    		NotFoundObj++;
    	}
 		}
 	}

	printf("nFrame = %5d  NotFoundObj = %d\n", nFrame, NotFoundObj);
}
*/

/*
void Match_ObjList_and_Yolo_List(void)
{
	int *pAssigned;
	int i, j, xc, yc, dist_SQ, dist_SQ_Min, xc_2, yc_2, Found, ObjIdx;
	PTRACKER pTracker;
	
	pAssigned = new int[MAX_OBJ];
	memset(pAssigned, 0, sizeof(int)*MAX_OBJ);
	
	for(i=0; i<nObj_Yolo; i++)	{
    if(Yolo_ObjList[i].type > 0)	{	// car, truck or bus. Person is not included.
    	dist_SQ_Min = 100000;	// initialization with a large enough value
    	xc = (Yolo_ObjList[i].x + Yolo_ObjList[i].xMax)/2;
    	yc = (Yolo_ObjList[i].y + Yolo_ObjList[i].yMax)/2;
    	Found = 0;
    	for(j=0; j<MAX_OBJ; j++)	{
    		if(ObjList[j].Used && (pAssigned[j] == 0) )	{	// not assigned yet
 					pTracker = &(ObjList[j]);
 					xc_2 = pTracker->boundingBox.x + 0.5*pTracker->boundingBox.width;
 					yc_2 = pTracker->boundingBox.y + 0.5*pTracker->boundingBox.height;
 					dist_SQ = (xc-xc_2)*(xc-xc_2) + (yc-yc_2)*(yc-yc_2);
 					if(dist_SQ < 400)	{	// a cutoff
 						pAssigned[j] = 1;
 						Update_an_Object(i, Yolo_ObjList[j].x, Yolo_ObjList[j].y, Yolo_ObjList[j].xMax - Yolo_ObjList[j].x, Yolo_ObjList[j].yMax-Yolo_ObjList[j].y);;
           	Found = 1;
           	break;
 					}
    		}
    	}
    	if(Found == 0)	{	// not found
	     	printf("Add an object (%d, %d, %d, %d)\n", Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
	     	ObjIdx = Add_an_Object(Yolo_ObjList[i].x, Yolo_ObjList[i].y, Yolo_ObjList[i].xMax - Yolo_ObjList[i].x, Yolo_ObjList[i].yMax-Yolo_ObjList[i].y);
 	    	FoundObj[ObjIdx] = 1;
    	}
		}
	}

  delete []pAssigned;	
}
*/

int Add_an_Object(int x, int y, int width, int height)
{
	PTRACKER pTracker;

	IdxAvailable = Find_Available_Space();
	
	pTracker = &(ObjList[IdxAvailable]);

//	pTracker->tracker = Tracker::create( tracker_algorithm );
//	if(pTracker->tracker == NULL)	{
//		printf("Error in the instantiation of the tracker\n");
//		exit(1);
//	}

	pTracker->x = x;
	pTracker->y = y;
	pTracker->width = width;
	pTracker->height = height;
	pTracker->xMax = x + width;
	pTracker->yMax = y + height;
	
	pTracker->Area_Max = width*height;

	pTracker->xMin_cmp = x - MAX_SHIFT;
	pTracker->xMax_cmp = x + width + MAX_SHIFT;
	pTracker->yMin_cmp = y - MAX_SHIFT;
	pTracker->yMax_cmp = y + height + MAX_SHIFT;

//	pTracker->trace[0].x = (int)(x + 0.5*width);
//	pTracker->trace[0].y = (int)(y + 0.5*height);

	pTracker->vx = 0;
	pTracker->vy = 0;

  pTracker->Area = width*height;

	pTracker->boundingBox.x = (int)(x+0.25*width);	// only trace the center part
	pTracker->boundingBox.y = (int)(y+0.25*height);
	pTracker->boundingBox.width = (int)(0.5*width);
	pTracker->boundingBox.height = (int)(0.5*height);

//	if( ! pTracker->tracker->init( frame, pTracker->boundingBox ) )	{
//		printf("Could not initialize tracker.\n");
//		exit(1);
//	}

	nObj++;
	pTracker->Idx = nObj;
	pTracker->Used = 1;
	pTracker->traj_Pos = 1;
	pTracker->FirstFrame=nFrame;
	pTracker->Confirmed=0;
	pTracker->nFounded = 1;
	pTracker->nNotFounded = 0;

	if(! pTracker->Img.empty())	{
		pTracker->Img.~Mat();
	}
	pTracker->Img = Mat(frame, Rect(x,y,width,height));

	return IdxAvailable;
}

void Update_an_Object(int idx, int x, int y, int width, int height)
{
	PTRACKER pTracker;
	int Area;
	float dx, dy;

	pTracker = &(ObjList[idx]);

  dx = (x + 0.5*width) - (pTracker->x + 0.5*pTracker->width);
  dy = (y + 0.5*height) - (pTracker->y + 0.5*pTracker->height);

  pTracker->vx = (int)(pTracker->vx*0.75 + dx*0.25);
  pTracker->vy = (int)(pTracker->vy*0.75 + dy*0.25);

	pTracker->x = x;
	pTracker->y = y;
	pTracker->width = width;
	pTracker->height = height;
	pTracker->xMax = x + width;
	pTracker->yMax = y + height;
	
	if(! pTracker->Img.empty())	{
		pTracker->Img.~Mat();
	}
	pTracker->Img = Mat(frame, Rect(x,y,width,height));
	
	Area = width * height;
	if(Area < pTracker->Area)	{	// if the area is decreasing, do not add the object
		pTracker->nFounded--;
	}
	
	if( Area > pTracker->Area_Max  )	{
		pTracker->Area_Max = Area;
		if(pTracker->Confirmed)	{
				// Add into car gallery
				int width_new, height_new, x_new, y_new;
				Mat car=Mat(frame, Rect(x, y, width, height));
				if(width > height)	{
					width_new = CAR_WIN_SIZE;
					height_new = (int)(1.0*CAR_WIN_SIZE*height/width);
					x_new = 0;
					y_new = (width_new-height_new)/2;
				}
				else	{
					height_new = CAR_WIN_SIZE;
					width_new = (int)(1.0*CAR_WIN_SIZE*width/height);
					x_new = (height_new-width_new)/2;
					y_new = 0;
				}
				if(! car_gallery[(pTracker->Idx-1)%nCarMax].empty())	{
					car_gallery[(pTracker->Idx-1)%nCarMax].~Mat();
				}
				car_gallery[(pTracker->Idx-1)%nCarMax] = Mat::zeros(Size(CAR_WIN_SIZE, CAR_WIN_SIZE), frame.type());
				Mat icon;
				resize(car, icon, Size(width_new, height_new));//resize image
				icon.copyTo( car_gallery[(pTracker->Idx-1)%nCarMax]( Rect(x_new, y_new, width_new, height_new) ) );
		}
	}
	else if( (1.0*Area/pTracker->Area_Max) < AREA_RATIO_TO_REMOVE_OBJECT )	{	// becoming smaller. Treated as NOT found
		pTracker->nNotFounded += 1;
//		pTracker->nNotFounded = FRAME_to_DEL_OBJ + 1;	// Remove the object
	}
	

	pTracker->xMin_cmp = x - MAX_SHIFT;
	pTracker->xMax_cmp = x + width + MAX_SHIFT;
	pTracker->yMin_cmp = y - MAX_SHIFT;
	pTracker->yMax_cmp = y + height + MAX_SHIFT;

//	pTracker->trace[pTracker->traj_Pos].x = (int)(x + 0.5*width);
//	pTracker->trace[pTracker->traj_Pos].y = (int)(y + 0.5*height);

/*
	Rect2d boundingBox(x, y, width, height);

	if( ! pTracker->tracker->init( frame, boundingBox ) )	{
		printf("Could not initialize tracker.\n");
		exit(1);
	}
*/
	pTracker->traj_Pos++;
}

void Get_RGB_Color(int idx, int *R, int *G, int *B)
{
	int i;

	i = idx%NUM_COLOR;
	*R =  Color_List[i] & 0xFF0000;
	*G =  Color_List[i] & 0x00FF00;
	*B =  Color_List[i] & 0x0000FF;
}

void Show_Obj_Index(void)
{
	int i, x, y;
	char szTxt[64];

	for(i=0; i<MAX_OBJ; i++)	{
		if(FoundObj[i] == 0)	{
			ObjList[i].nNotFounded++;
			if(ObjList[i].nNotFounded > FRAME_to_DEL_OBJ)	{
				Remove_an_Object(i);
			}
		}

		if(ObjList[i].Confirmed)	{
			x = (int)(ObjList[i].x + 0.5*ObjList[i].width) - 16;
			y = (int)(ObjList[i].y + 0.5*ObjList[i].height) + 6;
			sprintf(szTxt, "%d", ObjList[i].Idx);
			
			putText(frame, szTxt, cv::Point(x, y), FONT_HERSHEY_TRIPLEX, 1.5 , cv::Scalar(0,255,0));;
//			putText(frame, szTxt, cv::Point(x, y), FONT_HERSHEY_SIMPLEX, 1.5 , cv::Scalar(0,255,0));;
			rectangle(frame, Point(ObjList[i].x, ObjList[i].y), Point( ObjList[i].x+ObjList[i].width, ObjList[i].y+ObjList[i].height), Scalar(255, 0, 0), 3, 8, 0 );
//			rectangle(edge, Point(ObjList[i].x, ObjList[i].y), Point( ObjList[i].x+ObjList[i].width, ObjList[i].y+ObjList[i].height), Scalar(255, 0, 0), 3, 8, 0 );
//			rectangle(fgMaskMOG2, Point(ObjList[i].x, ObjList[i].y), Point( ObjList[i].x+ObjList[i].width, ObjList[i].y+ObjList[i].height), Scalar(255, 0, 0), 3, 8, 0 );
		}
	}

//	imshow("Edge", edge);
//    imshow("FG Mask MOG 2", fgMaskMOG2);
}

void Gen_FG_Mask_from_Diff(void)
{
	uchar *ptr_frame, *ptr_bg, *ptr_diff, R, G, B, Min, Max;
	int step_color, step_gray;
	int x, y, width, height, channel, drgb, diff_sq_Sum;

	width = frame.cols;
	height = frame.rows;
	step_color = frame.step;
	step_gray = diff.step;

//#pragma omp parallel for private(ptr_frame, ptr_bg, ptr_diff) 
	for(y=0; y<height; y++)        {
		for(x=0; x<width; x++)        {
			ptr_frame = frame.data + y*step_color + 3*x;
			ptr_bg = clean_bg.data + y*step_color + 3*x;
			ptr_diff = diff.data + y*step_gray + x;

//			diff_sq_Sum = 0;
//			for(channel=0; channel<3; channel++)	{
//				drgb = (int)(ptr_frame[channel] - ptr_bg[channel]);
//				diff_sq_Sum + (drgb*drgb);
//			}
//			*ptr_diff =  (diff_sq_Sum > FG_THRESHHOLD) ? ( (uchar)255) : ((uchar)0 );
			*ptr_diff = ( ((uchar)abs(ptr_frame[2] - ptr_bg[2])) > (uchar)(40) ) ? (uchar)255 : (uchar)0;

//			if( *ptr_diff > (uchar)(40) )	{	// moving object
//					ptr_frame[0] = 0;
//					ptr_frame[1] = 0;
//					ptr_frame[2] = 255;
//      }
/*
			if( *ptr_diff > (uchar)(50) )	{	// moving object
				Min = (ptr_frame[0] < ptr_frame[1]) ? ptr_frame[0] : ptr_frame[1];
				Min = ( Min < ptr_frame[2] ) ? Min : ptr_frame[2];

				Max = (ptr_frame[0] > ptr_frame[1]) ? ptr_frame[0] : ptr_frame[1];
				Max = ( Min > ptr_frame[2] ) ? Max : ptr_frame[2];

				if( (Min>=50) && (Max<=95) && ( (1.0*Max/Min) < 1.15) )	{	// shadow
					ptr_frame[0] = 0;
					ptr_frame[1] = 0;
					ptr_frame[2] = 255;
				}
			}
*/			
		}
	}

  imshow("FG", diff);
}

double Get_White_Pixle_Ratio(Mat *pMat, int xMin, int yMin, int width, int height)
{
	int Sum=0;
	int x, y, xMax, yMax, step;
	uchar *ptr;
	double ratio;
	
	xMax = xMin + width;
	yMax = yMin + height;
	step = pMat->step;
	
	for(y=yMin; y<yMax; y++)        {
		ptr = pMat->data + y*step;
		for(x=xMin; x<xMax; x++)        {
			Sum += ptr[x];
		}
	}

	ratio = 1.0*Sum/(255*width*height);

	return ratio;
}

void Draw_Car_Gallery(void)
{
	int nCar_to_Show, idx, idx_wrapped, count=0;
	char szTxt[64];
	
	nCar_to_Show = min(nCarMax, nObj_Confirmed);
	idx = (nObj_Confirmed - 1);
	idx_wrapped = (idx + nCarMax)%nCarMax;
	while(count < nCar_to_Show)	{
		car_gallery[idx_wrapped].copyTo( car_List( Rect(idx_wrapped*CAR_WIN_SIZE, 0, CAR_WIN_SIZE, CAR_WIN_SIZE) ) );
   	sprintf(szTxt, "%d", idx+1);	// car index
    putText(car_List, szTxt, cv::Point(idx_wrapped*CAR_WIN_SIZE+12, 28), FONT_HERSHEY_SIMPLEX, 1.1 , cv::Scalar(0,255,0));
    if(count == 0)	rectangle(car_List, Point(idx_wrapped*CAR_WIN_SIZE+2, 2), Point( (idx_wrapped+1)*CAR_WIN_SIZE-4, CAR_WIN_SIZE-2), Scalar(0, 0, 255), 3, 8, 0 );
		count++;
		idx--;
		idx_wrapped = (idx + nCarMax)%nCarMax;
	}
	car_List.copyTo( img_all( Rect(0, height, width, CAR_WIN_SIZE) ) );
}

// more filters: 
// 1) Only shows FG objects. Done.
// 2) Record max object size. When the area decreases a lot or too close to boarders. Delete the objects to avoid counting issue.
// 3) Tracking with KCF. Using the center part for KCF tracking. When object size varies, re-create the tracker.


