#include "objects_tracker.h"
#include "track_link.h"

#define max(a,b)  (((a) > (b)) ? (a) : (b))
#define min(a,b)  (((a) < (b)) ? (a) : (b))

ObjectsTracker::ObjectsTracker() :
    parameters(),
    numTrackedSteps(0)
{
   
}

ObjectsTracker::~ObjectsTracker()
{

}

ObjectsTracker::Parameters::Parameters()
{
    maxTrackLifetime=6;
    numLastPositionsToTrack=4;
    numDetectedToWaitBeforeFirstShow=0;
    numStepsToWaitBeforeFirstShow=6;
    numStepsToTrackWithoutDetectingIfObjectHasNotBeenShown=4;
    numStepsToShowWithoutDetecting=5;
}

float CalculateIOU(Rect_T r1, Rect_T r2) {
	int xmin0 = r1.x;
	int ymin0 = r1.y;	
	int xmax0 = r1.x +r1.width;
	int ymax0 = r1.y +r1.height;
	int xmin1 = r2.x;
	int ymin1 = r2.y;	
	int xmax1 = r2.x +r2.width;
	int ymax1 = r2.y +r2.height;
    float w = max(0.f, min(xmax0, xmax1) - max(xmin0, xmin1));
    float h = max(0.f, min(ymax0, ymax1) - max(ymin0, ymin1));
    float i = w * h;
    float u = (xmax0 - xmin0) * (ymax0 - ymin0) + (xmax1 - xmin1) * (ymax1 - ymin1) - i;
    return u <= 0.f ? 0.f : (i / u);
}

float CalculateArea(Rect_T r) {
    float i = r.width * r.height;
    return i;
}

void ObjectsTracker::predict_loctation(TrackedObject& curObject, int image_width,int image_height, float& pre_x, float& pre_y, float& v_x, float& v_y){
    int numpositions = (int)curObject.lastPositions.size();
    Rect_T prevRect = curObject.lastPositions[numpositions-1];
    Rect_T tmpRect=curObject.lastPositions[numpositions-2];
    float vx_1 = (prevRect.x -tmpRect.x)/(1.0f +curObject.preNumFramesNotDetected);
    float vx_2 = (prevRect.x +prevRect.width -tmpRect.x -tmpRect.width)/(1.0f +curObject.preNumFramesNotDetected);
    float vx_ = min(vx_1, vx_2);
    float vy_1 = (prevRect.y -tmpRect.y)/(1.0f +curObject.preNumFramesNotDetected);
    float vy_2 = (prevRect.y +prevRect.height -tmpRect.y -tmpRect.height)/(1.0f +curObject.preNumFramesNotDetected);
    float vy_ = min(vy_1, vy_2);
    v_x = 0.5f*vx_ + 0.5f*curObject.vx;
    v_y = 0.5f*vy_ + 0.5f*curObject.vy;
    int x = (int)(prevRect.x  + v_x*(1 ) *0.7f +0.5f);
    int y = (int)(prevRect.y  + v_y*(1 ) *0.7f +0.5f);
    x = x >= 0 ? x : 0;
    y = y >= 0 ? y : 0;
    if(x + prevRect.width >= image_width)
        x -= (x + prevRect.width -image_width +1);
    if(y + prevRect.height >= image_height)
        y -= (y + prevRect.height -image_height +1);
    x = x >= 0 ? x : 0;
    y = y >= 0 ? y : 0;
    pre_x = x;
    pre_y = y;

}

void ObjectsTracker::getObjects(std::vector<ExtObject>& result)
{
    result.clear();
    for(size_t i=0; i < trackedObjects.size(); i++) {
        ObjectStatus status;
        Rect_T r=calcTrackedObjectPositionToShow((int)i, status);

		if (CalculateArea(r)==0.f || trackedObjects[i].numFramesDetected < 2){
            continue;
        }
		
        result.push_back(ExtObject(trackedObjects[i].id, r, trackedObjects[i].predict_loc_when_miss, trackedObjects[i].smooth_Positionn, status,
			trackedObjects[i].numFramesDetected, trackedObjects[i].numFramesNotDetected, trackedObjects[i].obj_class, trackedObjects[i].score, trackedObjects[i].miss));

	}
}

void ObjectsTracker::updateTrackedObjects(const std::vector<Rect_T>& detectedObjects, const std::vector<int> objects_class
        , const std::vector<float> objects_score, int maxTrackLifetime,  int image_width,int image_height)
{
    enum {
        NEW_RECTANGLE=-1,
        INTERSECTED_RECTANGLE=-2
    };

    int N1=(int)trackedObjects.size();
    int N2=(int)detectedObjects.size();

    for(int i=0; i < N1; i++) {
        trackedObjects[i].numDetectedFrames++;
    }
	
    std::vector<int> correspondence(detectedObjects.size(), NEW_RECTANGLE);
	std::vector<float> correspondenceScore(detectedObjects.size(), 0);
	
    for(int i=0; i < N1; i++) {
        TrackedObject& curObject=trackedObjects[i];
        int bestIndex=-1;
        float bestArea=-1;
        int numpositions=(int)curObject.lastPositions.size();
        Rect_T prevRect=curObject.lastPositions[numpositions-1];

		//save predict loctation
		if(numpositions>1)
		{
            float pre_x, pre_y, vx, vy;
            predict_loctation(curObject, image_width, image_height, pre_x, pre_y, vx, vy);
            curObject.vx = vx;
            curObject.vy = vy;
            prevRect.x = (int)pre_x;
            prevRect.y = (int)pre_y;
            curObject.predict_loc_when_miss = prevRect;
		}
		
		//search track loaction
        for(int j=0; j < N2; j++) {
            
			float percentage_IOU = CalculateIOU(prevRect, detectedObjects[j]);
            if ( percentage_IOU > 0.1f ) {//&& objects_class[j] ==  curObject.obj_class
				
				float trackScore = percentage_IOU *1.f / (curObject.numFramesNotDetected + 1);
                if ( percentage_IOU > bestArea && correspondenceScore[j] < trackScore) {
                    bestIndex = j;
                    bestArea = percentage_IOU;
					correspondenceScore[j] = trackScore;
                }
            }
			
        }
		if (bestIndex >= 0) {
			correspondence[bestIndex] = i;
		}	
    }

	//select track loaction
	for (int i = 0; i < N1; i++) {
		TrackedObject& curObject = trackedObjects[i];
		int bestIndex = -1;
		for (int j = 0; j < N2; j++) {
			if (correspondence[j] == i){
				bestIndex = j;
				break;
			}
		}
		if (bestIndex >= 0) {
			correspondence[bestIndex] = i;
			for (int j = 0; j < N2; j++) {
				if (correspondence[j] >= 0)
					continue;

				float percentage_IOU = CalculateIOU(detectedObjects[j], detectedObjects[bestIndex]);			
				if (percentage_IOU > 0.45f  ){//&& objects_class[j] == curObject.obj_class
					correspondence[j] = INTERSECTED_RECTANGLE;
				}
			}
			curObject.numFramesDetected++;
		}
		else {

			curObject.numFramesNotDetected++;
			curObject.miss = 1;
		}

	}

	//allocate new detected location
    for(int j=0; j < N2; j++) {
        int i = correspondence[j];
        if (i >= 0) {//add position
            trackedObjects[i].lastPositions.push_back(detectedObjects[j]);
            while ((int)trackedObjects[i].lastPositions.size() > (int) parameters.numLastPositionsToTrack) {
                trackedObjects[i].lastPositions.erase(trackedObjects[i].lastPositions.begin());
            }
			trackedObjects[i].preNumFramesNotDetected = trackedObjects[i].numFramesNotDetected;
            trackedObjects[i].numFramesNotDetected = 0;
            trackedObjects[i].score = objects_score[j];
            trackedObjects[i].obj_class = objects_class[j];
			trackedObjects[i].miss = 0;
            //smooth rect
            trackedObjects[i].smooth_Positionn.width = (trackedObjects[i].smooth_Positionn.width *1 +detectedObjects[j].width )/(2);
            trackedObjects[i].smooth_Positionn.height = (trackedObjects[i].smooth_Positionn.height *1 +detectedObjects[j].height)/(2);
            Rect_T r_smooth = trackedObjects[i].smooth_Positionn;
            float weight_p = 0.5f, weight_n = 1.f;
            trackedObjects[i].smooth_Positionn.x = (int)(((r_smooth.x +r_smooth.width*0.5f) *weight_p +(detectedObjects[j].x +detectedObjects[j].width*0.5f)*weight_n)/(weight_p +weight_n)
                                                         +trackedObjects[i].vx *weight_p /(weight_p +weight_n) -r_smooth.width *0.5f);

            trackedObjects[i].smooth_Positionn.y = (int)(((r_smooth.y +r_smooth.height*0.5f) *weight_p +(detectedObjects[j].y +detectedObjects[j].height*0.5f)*weight_n)/(weight_p +weight_n)
                                                         +trackedObjects[i].vy *weight_p /(weight_p +weight_n)-r_smooth.height *0.5f);
		} else if (i==NEW_RECTANGLE){ //new object
			
            trackedObjects.push_back(detectedObjects[j]);		
			int _N2 = (int)trackedObjects.size();
			trackedObjects[_N2-1].obj_class = objects_class[j];
            trackedObjects[_N2-1].score = objects_score[j];
			correspondence[j] = _N2 -1;
            trackedObjects[_N2-1].smooth_Positionn.width = detectedObjects[j].width;
            trackedObjects[_N2-1].smooth_Positionn.height = detectedObjects[j].height;
            trackedObjects[_N2-1].smooth_Positionn.x = (detectedObjects[j].x);
            trackedObjects[_N2-1].smooth_Positionn.y = (detectedObjects[j].y);
        }
    }

    std::vector<TrackedObject>::iterator it=trackedObjects.begin();
    while( it != trackedObjects.end() ) {
        if ( (it->numFramesNotDetected > maxTrackLifetime)
#if 0
                ||
                (
                 (it->numDetectedFrames <= parameters.numStepsToWaitBeforeFirstShow)
                 &&
                 (it->numFramesNotDetected > parameters.numStepsToTrackWithoutDetectingIfObjectHasNotBeenShown)
                )
#endif
           )
        {
            it=trackedObjects.erase(it);
			
        } else {
            it++;
        }
		
    }

}

Rect_T ObjectsTracker::calcTrackedObjectPositionToShow(int i, ObjectStatus& status) const
{
   Rect_T r;
   r.x = 0;
   r.y = 0;
   r.width = 0;
   r.height = 0;

    if ( (i < 0) || (i >= (int)trackedObjects.size()) ) {
        status = WRONG_OBJECT;
       
        return r;
    }

#if 0
    if (trackedObjects[i].numDetectedFrames <= parameters.numStepsToWaitBeforeFirstShow) {
        status = DETECTED_NOT_SHOWN_YET;
        return r;
    }


    if (trackedObjects[i].numFramesDetected <= parameters.numDetectedToWaitBeforeFirstShow) {
        status = DETECTED_NOT_SHOWN_YET;
        return r;
    }


    if (trackedObjects[i].numFramesNotDetected > parameters.numStepsToShowWithoutDetecting) {
        status = DETECTED_TEMPORARY_LOST;
		
        return r;
    }
#endif

    const TrackedObject::PositionsVector& lastPositions=trackedObjects[i].lastPositions;

    int N=(int)lastPositions.size();
    if (N<=0) {
        status = WRONG_OBJECT;
        return r;
    }

    return lastPositions[N-1];

}