#include "CornerTracker.h"
#include <math.h>

void CornerTracker::trackCornersN2( const int *usersPoints, const int *detectedPoints, int numUserPoints, int numDetectedPoints, TrackedCorners *trackedCorners )
{
	int posMinDisElem = -1;
	int minDist = INT_MAX;
	int dist = 0;
	// Select the nearest point from each point
	for( int i = 0 ; i < numUserPoints ; i++ )
	{
		for( int j = 0 ; j < numDetectedPoints ; j++ )
		{
			dist = ( pow((double)(usersPoints[2*i]-detectedPoints[2*j]),2) + pow((double)(usersPoints[2*i+1]-detectedPoints[2*j+1]),2) );
			if( dist < minDist )
			{
				minDist = dist;
				posMinDisElem = j;
			}
		}
		if( minDist <= POINT_MINIMUM_SQ_DISTANCE && posMinDisElem >= 0 )
		{	
			float tr = trackedCorners->getTrustyRate( i );

			// INCREASE TRUSTYRATE by 10%
			tr *= (1.0 + TRUSTY_CHANGING_RATE);

			// an alternative to increase 10% is recovering trusty rate to 1 (more instable)
			/*tr = 1.0f;*/
			if( tr >= MINIMUM_TRUSTY_RATE )
				trackedCorners->setPointValid( i , true );
			
			trackedCorners->setTrustyRate( i, tr );
			trackedCorners->setPoint( i, detectedPoints[2*posMinDisElem],detectedPoints[2*posMinDisElem+1]  );
			trackedCorners->setTrustyRate( i, trackedCorners->getTrustyRate( i )*1.1 );
		}
		else
		{
			float tr = trackedCorners->getTrustyRate( i );

			// INCREASE TRUSTYRATE by 10%
			tr *= (1.0 - TRUSTY_CHANGING_RATE);
			if( tr < MINIMUM_TRUSTY_RATE )
				trackedCorners->setPointValid( i , false );
			
			trackedCorners->setTrustyRate( i, tr );
			// DECREASE TRUSTYRATE
		}
		posMinDisElem = -1;
		minDist = INT_MAX;
	}
}