ms.cpp

/*******************************************************

  Mean Shift Analysis Library
  =============================================
  
	The mean shift library is a collection of routines
	that use the mean shift algorithm. Using this algorithm,
	the necessary output will be generated needed
	to analyze a given input set of data.
	
  MeanShift Base Class:
  ====================
	  
	The mean shift library of routines is realized
	via the creation of a MeanShift base class. This class
	provides a mechanism for calculating the mean shift vector
	at a specified data point, using an arbitrary N-dimensional
	data set, and a user-defined kernel.
		
	For image processing the mean shift base class also allows
	for the definition of a data set that is on a two-dimensional
	lattice. The amount of time needed to compute the mean shift
	vector using such a data set is much less than that of an
	arbitrary one. Because images usually contain many data points,
	defining the image input data points as being on a lattice
	greatly improves computation time and makes algorithms such
	as image filtering practical.
		  
	The definition of the MeanShift class is provided below. Its
	prototype is provided in 'ms.h'.
			
The theory is described in the papers:

  D. Comaniciu, P. Meer: Mean Shift: A robust approach toward feature
									 space analysis.

  C. Christoudias, B. Georgescu, P. Meer: Synergism in low level vision.

and they are is available at:
  http://www.caip.rutgers.edu/riul/research/papers/

Implemented by Chris M. Christoudias, Bogdan Georgescu
********************************************************/


//Include Needed Libraries

#include	"ms.h"
#include	<string.h>
#include	<stdlib.h>
#include	<stdio.h>
#include	<math.h>

/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@      PUBLIC METHODS     @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** Constructor/Destructor ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Class Constructor                                    */
/*******************************************************/
/*Post:                                                */
/*      The MeanShift class has been properly          */
/*      initialized.                                   */
/*******************************************************/

MeanShift::MeanShift( void )
{
	
	//intialize input data set parameters...
	P							= NULL;
	L							= 0;
	N							= 0;
	kp							= 0;
	
	//initialize input data set storage structures...
	data						= NULL;
	
	//initialize input data set kd-tree
	root						= NULL;
	forest						= NULL;
	range						= NULL;
	
	//intialize lattice structure...
	height						= 0;
	width						= 0;
	
	//intialize kernel strucuture...
	h							= NULL;
	kernel						= NULL;
	w							= NULL;
	offset						= NULL;
	increment					= NULL;
	uniformKernel				= false;
	
	//initialize weight function linked list...
	head						= cur	= NULL;
	
	//intialize mean shift processing data structures...
	uv							= NULL;

	//set lattice weight map to null
	weightMap					= NULL;

	//indicate that the lattice weight map is undefined
	weightMapDefined			= false;
	
	//allocate memory for error message buffer...
	ErrorMessage				= new char [256];
	
	//initialize error status to OKAY
	ErrorStatus					= EL_OKAY;
	
	//Initialize class state...
	class_state.INPUT_DEFINED	= false;
	class_state.KERNEL_DEFINED	= false;
	class_state.LATTICE_DEFINED	= false;
	class_state.OUTPUT_DEFINED	= false;
	
}

/*******************************************************/
/*Class Destructor                                     */
/*******************************************************/
/*Post:                                                */
/*      The MeanShift class has been properly          */
/*      destroyed.                                     */
/*******************************************************/

MeanShift::~MeanShift( void )
{
	delete [] ErrorMessage;
   if (weightMap)
   {
      delete [] weightMap;
   }

	//de-allocate memory used to store
	//user defined weight functions
	ClearWeightFunctions();
	
	//de-allocate memory used for kernel
	DestroyKernel();
	
	//de-allocate memory used for input
	ResetInput();
	
}

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** Creation/Initialization of Mean Shift Kernel ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Define Kernel                                        */
/*******************************************************/
/*Creats custom user defined Kernel to be used by the  */
/*mean shift procedure.                                */
/*******************************************************/
/*Pre:                                                 */
/*      - kernel is an array of kernelTypes specifying */
/*        the type of kernel to be used on each sub-   */
/*        space of the input data set x                */
/*      - h is the set of bandwidths used to define the*/
/*        the search window                            */
/*      - P is a one dimensional array of integers of  */
/*        size kp, that specifies the dimension of each*/
/*        subspace of the input data set x             */
/*      - kp is the total number of subspaces used to  */
/*        the input data set x                         */
/*Post:                                                */
/*      - the custom kernel has been created for use   */
/*        by the mean shift procedure.                 */
/*******************************************************/

void MeanShift::DefineKernel(kernelType *kernel_, float *h_, int *P_, int kp_)
{
	
	// Declare variables
	int i, kN;
	
	//if a kernel has already been created then destroy it
	if(kp)
		DestroyKernel();
	
	//Obtain kp...
	if((kp = kp_) <= 0)
	{
		ErrorHandler("MeanShift", "CreateKernel", "Subspace count (kp) is zero or negative.");
		return;
	}
	
	//Allocate memory for h, P, kernel, offset, and increment
	if((!(P = new int [kp]))||(!(h = new float [kp]))||(!(kernel = new kernelType [kp]))||
		(!(offset = new float [kp]))||(!(increment = new double [kp])))
	{
		ErrorHandler("MeanShift", "CreateKernel", "Not enough memory available to create kernel.");
		return;
	}
	
	//Populate h, P and kernel, also use P to calculate
	//the dimension (N_) of the potential input data set x
	kN = 0;
	for(i = 0; i < kp; i++)
	{
		if((h[i] = h_[i]) <= 0)
		{
			ErrorHandler("MeanShift", "CreateKernel", "Negative or zero valued bandwidths are prohibited.");
			return;
		}
		if((P[i] = P_[i]) <= 0)
		{
			ErrorHandler("MeanShift", "CreateKernel", "Negative or zero valued subspace dimensions are prohibited.");
			return;
		}
		kernel[i] = kernel_[i];
		kN	   += P[i];
	}
	
	//Allocate memory for range vector and uv using N_
	if((!(range = new float [2*kN]))||(!(uv = new double [kN])))
	{
		ErrorHandler("MeanShift", "CreateKernel", "Not enough memory available to create kernel.");
		return;
	}
	
	// Generate weight function lookup table
	// using above information and user
	// defined weight function list
	generateLookupTable();
	
	//check for errors
	if(ErrorStatus == EL_ERROR)
		return;
	
	//indicate that the kernel has been defined
	class_state.KERNEL_DEFINED	= true;
	
	//done.
	return;
	
}

/*******************************************************/
/*Add Weight Function                                  */
/*******************************************************/
/*Adds a weight function to the Mean Shift class to be */
/*used by the mean shift procedure                     */
/*******************************************************/
/*Pre:                                                 */
/*      - g(u) is the normalized weight function with  */
/*        respect to u = (norm(x-xi))^2/h^2            */
/*      - sampleNumber is the number of samples to be  */
/*        taken of g(u) over halfWindow interval       */
/*      - halfWindow is the radius of g(u) such that   */
/*        g(u) is defined for 0 <= u <= halfWindow     */
/*      - subspace is the subspace number for which    */
/*        g(u) is to be applied during the mean shift  */
/*        procedure.                                   */
/*Post:                                                */
/*      - g(u) has been added to the Mean Shift class  */
/*        private data structure to be used by the     */
/*        mean shift procedure.                        */
/*      - if a weight function has already been spec-  */
/*        ified for the specified subspace, the weight */
/*        function for this subspace has been replaced.*/
/*******************************************************/

void MeanShift::AddWeightFunction(double g(double), float halfWindow, int sampleNumber, int subspace)
{
	
	// Declare Variables
	int   i;
	double increment;
	
	// Search to see if a weight function has already been
	// defined for specified subspace, if not then insert
	// into the head of the weight function list, otherwise
	// replace entry
	
	// Perform Search
	cur = head;
	while((cur)&&(cur->subspace != subspace))
		cur = cur->next;
	
	// Entry Exists - Replace It! 
	// Otherwise insert at the head of the the weight functon list
	if(cur)
		delete cur->w;
	else
    {
		cur       = new userWeightFunct;
		cur->next = head;
		head      = cur;
    }
	
	// Generate lookup table
	increment = halfWindow/(double)(sampleNumber);
	
	cur->w = new double [sampleNumber+1];
	for(i = 0; i <= sampleNumber; i++)
		cur->w[i] = g((double)(i*increment));
	
	// Set weight function parameters
	cur->halfWindow   = halfWindow;
	cur->sampleNumber = sampleNumber;
	cur->subspace     = subspace;
	
	//done.
	return;
	
}

/*******************************************************/
/*Clear Weight Functions                               */
/*******************************************************/
/*Clears user defined weight from the Mean Shift class */
/*private data structure.                              */
/*******************************************************/
/*Post:                                                */
/*      - all user defined weight functions ahve been  */
/*        cleared from the private data structure of   */
/*        the mean shift class.                        */
/*******************************************************/

void MeanShift::ClearWeightFunctions( void )
{
	
	while(head)
    {
		delete head->w;
		cur  = head;
		head = head->next;
		delete cur;
    }
	
}

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** Input Data Set Declaration ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Define Input                                         */
/*******************************************************/
/*Uploads input data set x into the mean shift class.  */
/*******************************************************/
/*Pre:                                                 */
/*      - x is a one dimensional array of L N-dimen-   */
/*        ional data points.                           */
/*Post:                                                */
/*      - x has been uploaded into the mean shift      */
/*        class.                                       */
/*      - the height and width of a previous data set  */
/*        has been undefined.                          */
/*******************************************************/

void MeanShift::DefineInput(float *x, int L_, int N_)
{
	
	
	//if input data is defined de-allocate memory, and
	//re-initialize the input data structure
	if((class_state.INPUT_DEFINED)||(class_state.LATTICE_DEFINED))
		ResetInput();
	
	//make sure x is not NULL...
	if(!x)
	{
		ErrorHandler("MeanShift", "UploadInput", "Input data set is NULL.");
		return;
	}
	
	//Obtain L and N
	if(((L = L_) <= 0)||((N = N_) <= 0))
	{
		ErrorHandler("MeanShift", "UploadInput", "Input data set has negative or zero length or dimension.");
		return;
	}
	
	//Allocate memory for data
	if(!(data = new float [L*N]))
	{
		ErrorHandler("MeanShift", "UploadInput", "Not enough memory.");
		return;
	}
	
	//Allocate memory for input data set, and copy
	//x into the private data members of the mean
	//shift class
	InitializeInput(x);
	
	//check for errors
	if(ErrorStatus == EL_ERROR)
		return;
	
	// Load x into the MeanShift object using
	// using a kd-tree, resulting in better
	// range searching of the input data points
	// x - also upload window centers into
	// msRawData
	CreateBST();
	
	//indicate that the input has been recently defined
	class_state.INPUT_DEFINED	= true;
	class_state.LATTICE_DEFINED	= false;
	class_state.OUTPUT_DEFINED	= false;
	
	//done.
	return;
	
}

/*******************************************************/
/*Define Lattice                                       */
/*******************************************************/
/*Defines the height and width of the input lattice.   */
/*******************************************************/
/*Pre:                                                 */
/*      - ht is the height of the lattice              */
/*      - wt is the width of the lattice               */
/*Post:                                                */
/*      - the height and width of the lattice has been */
/*        specified.                                   */
/*      - if a data set is presently loaded into the   */
/*        mean shift class, an error is flagged if the */
/*        number of elements in that data set does not */
/*        equal the product ht*wt.                     */
/*******************************************************/

void MeanShift::DefineLInput(float *x, int ht, int wt, int N_)
{
	
	//if input data is defined de-allocate memory, and
	//re-initialize the input data structure
	if((class_state.INPUT_DEFINED)||(class_state.LATTICE_DEFINED))
		ResetInput();
	
	//Obtain lattice height and width
	if(((height	= ht) <= 0)||((width	= wt) <= 0))
	{
		ErrorHandler("MeanShift", "DefineLInput", "Lattice defined using zero or negative height and/or width.");
		return;
	}
	
	//Obtain input data dimension
	if((N = N_) <= 0)
	{
		ErrorHandler("MeanShift", "DefineInput", "Input defined using zero or negative dimension.");
		return;
	}
	
	//compute the data length, L, of input data set
	//using height and width
	L		= height*width;
	
	//Allocate memory for input data set, and copy
	//x into the private data members of the mean
	//shift class
	InitializeInput(x);
	
	//check for errors
	if(ErrorStatus == EL_ERROR)
		return;

	//allocate memory for weight map
	if(!(weightMap = new float [L]))
	{
		ErrorHandler("MeanShift", "InitializeInput", "Not enough memory.");
		return;
	}

	//initialize weightMap to an array of zeros
	memset(weightMap, 0, L*(sizeof(float)));
	
	//Indicate that a lattice input has recently been
	//defined
	class_state.LATTICE_DEFINED	= true;
	class_state.INPUT_DEFINED	= false;
	class_state.OUTPUT_DEFINED	= false;
	
	//done.
	return;
	
}

/*******************************************************/
/*Set Lattice Weight Map                               */
/*******************************************************/
/*Populates the lattice weight map with specified      */
/*weight values.                                       */
/*******************************************************/
/*Pre:                                                 */
/*      - wm is a floating point array of size L       */
/*        specifying for each data point a weight      */
/*        value                                        */
/*Post:                                                */
/*      - wm has been used to populate the lattice     */
/*        weight map.                                  */
/*******************************************************/

void MeanShift::SetLatticeWeightMap(float *wm)
{
	//make sure wm is not NULL
	if(!wm)
	{
		ErrorHandler("MeanShift", "SetWeightMap", "Specified weight map is NULL.");
		return;
	}

	//populate weightMap using wm
	int i;
	for(i = 0; i < L; i++)
		weightMap[i] = wm[i];

	//indicate that a lattice weight map has been specified
	weightMapDefined	= true;

	//done.
	return;

}


/*******************************************************/
/*Remove Lattice Weight Map                            */
/*******************************************************/
/*Removes the lattice weight map.                      */
/*******************************************************/
/*Post:                                                */
/*      - the lattice weight map has been removed.     */
/*      - if a weight map did not exist NO error is    */
/*        flagged.                                     */
/*******************************************************/

void MeanShift::RemoveLatticeWeightMap(void)
{

	//only remove weight map if it exists, otherwise
	//do nothing...
	if(weightMapDefined)
	{
		//set values of lattice weight map to zero
		memset(weightMap, 0, L*sizeof(float));

		//indicate that a lattice weight map is no longer
		//defined
		weightMapDefined	= false;
	}

	//done.
	return;

}


  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** Mean Shift Operations  ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Mean Shift Vector                                    */
/*******************************************************/
/*Calculates the mean shift vector at a specified data */
/*point yk.                                            */
/*******************************************************/
/*Pre:                                                 */
/*      - a kernel has been created                    */
/*      - a data set has been uploaded                 */
/*      - Mh is an N dimensional mean shift vector     */
/*      - yk is an N dimensional data point            */
/*Post:                                                */
/*      - the mean shift vector at yk has been         */
/*        calculated and stored in and returned by Mh. */
/*******************************************************/

void MeanShift::msVector(double *Mh, double *yk)
{
	
	//make sure that Mh and/or yk are not NULL...
	if((!Mh)||(!yk))
	{
		ErrorHandler("MeanShift", "msVector", "Invalid argument(s) passed to this method.");
		return;
	}
	
	//make sure that a kernel has been created, data has
	//been uploaded, and that they are consistent with one
	//another...
	classConsistencyCheck(N, false);
	
	//calculate mean shift vector at yk using created kernel
	//and uploaded data set
	MSVector(Mh, yk);
	
	//done.
	return;
	
}

/*******************************************************/
/*Lattice Mean Shift Vector                            */
/*******************************************************/
/*Calculates the mean shift vector at a specified data */
/*point yk, assuming that the data set exhists on a    */
/*height x width two dimensional lattice.              */
/*******************************************************/
/*Pre:                                                 */
/*      - a kernel has been created                    */
/*      - a data set has been uploaded                 */
/*      - the height and width of the lattice has been */
/*        specified using method DefineLattice()       */
/*      - Mh is an N dimensional mean shift vector     */
/*      - yk is an N dimensional data point            */
/*Post:                                                */
/*      - the mean shift vector at yk has been         */
/*        calculated and stored in and returned by Mh. */
/*      - Mh was calculated using the defined input    */
/*        lattice.                                     */
/*******************************************************/

void MeanShift::latticeMSVector(double *Mh, double *yk)
{
	
	//make sure that Mh and/or yk are not NULL...
	if((!Mh)||(!yk))
	{
		ErrorHandler("MeanShift", "lmsVector", "Invalid argument(s) passed to this method.");
		return;
	}
	
	//make sure that a kernel has been created, data has
	//been uploaded, and that they are consistent with one
	//another...
	classConsistencyCheck(N+2, true);
	
	//calculate mean shift vector at yk using created kernel
	//and uploaded data set
	LatticeMSVector(Mh, yk);
	
	//done.
	return;
	
}

/*******************************************************/
/*Find Mode                                            */
/*******************************************************/
/*Calculates the mode of a specified data point yk.    */
/*******************************************************/
/*Pre:                                                 */
/*      - a kernel has been created                    */
/*      - a data set has been uploaded                 */
/*      - mode is the N dimensional mode of the N-dim- */
/*        ensional data point yk                       */
/*Post:                                                */
/*      - the mode of yk has been calculated and       */
/*        stored in mode.                              */
/*******************************************************/

void MeanShift::FindMode(double *mode, double *yk)
{
	
	//make sure that mode and/or yk are not NULL...
	if((!mode)||(!yk))
	{
		ErrorHandler("MeanShift", "FindMode", "Invalid argument(s) passed to this method.");
		return;
	}
	
	//make sure that a kernel has been created, data has
	//been uploaded, and that they are consistent with one
	//another...
	classConsistencyCheck(N, false);
	
	//allocate memory for Mh
	double	*Mh	= new double [N];
	
	//copy yk into mode
	int i;
	for(i = 0; i < N; i++)
		mode[i] = yk[i];
	
	//calculate mean shift vector at yk
	MSVector(Mh, yk);
	
	//calculate mvAbs = |Mh|^2
	double mvAbs = 0;
	for(i = 0; i < N; i++)
		mvAbs	+= Mh[i]*Mh[i];
	
	//shift mode until convergence (mvAbs = 0)...
	int iterationCount = 1;
	while((mvAbs >= EPSILON)&&(iterationCount < LIMIT))
	{
		//shift mode...
		for(i = 0; i < N; i++)
			mode[i]	+= Mh[i];
		
		//re-calculate mean shift vector at new
		//window location have center defined by
		//mode
		MSVector(Mh, mode);
		
		//calculate mvAbs = |Mh|^2
		mvAbs = 0;
		for(i = 0; i < N; i++)
			mvAbs	+= Mh[i]*Mh[i];
		
		//increment interation count...
		iterationCount++;
		
	}
	
	//shift mode...
	for(i = 0; i < N; i++)
		mode[i]	+= Mh[i];
	
	//de-allocate memory
	delete [] Mh;
	
	//done.
	return;
	
}

/*******************************************************/
/*Find Lattice Mode                                    */
/*******************************************************/
/*Calculates the mode of a specified data point yk,    */
/*assuming that the data set exhists on a height x     */
/*width two dimensional lattice.                       */
/*******************************************************/
/*Pre:                                                 */
/*      - a kernel has been created                    */
/*      - a data set has been uploaded                 */
/*      - the height and width of the lattice has been */
/*        specified using method DefineLattice()       */
/*      - mode is the N dimensional mode of the N-dim- */
/*        ensional data point yk                       */
/*Post:                                                */
/*      - the mode of yk has been calculated and       */
/*        stored in mode.                              */
/*      - mode was calculated using the defined input  */
/*        lattice.                                     */
/*******************************************************/

void MeanShift::FindLMode(double *mode, double *yk)
{
	
	//make sure that mode and/or yk are not NULL...
	if((!mode)||(!yk))
	{
		ErrorHandler("MeanShift", "FindLMode", "Invalid argument(s) passed to this method.");
		return;
	}
	
	//make sure the lattice height and width have been defined...
	if(!height)
	{
		ErrorHandler("MeanShift", "FindLMode", "Lattice height and width is undefined.");
		return;
	}
	
	//make sure that a kernel has been created, data has
	//been uploaded, and that they are consistent with one
	//another...
	classConsistencyCheck(N+2, true);
	
	//define gridN
	int gridN = N+2;
	
	//allocate memory for Mh
	double	*Mh	= new double [gridN];
	
	//copy yk into mode
	int i;
	for(i = 0; i < gridN; i++)
		mode[i] = yk[i];
	
	//calculate mean shift vector at yk
	LatticeMSVector(Mh, mode);
	
	//calculate mvAbs = |Mh|^2
	double mvAbs = 0;
	for(i = 0; i < gridN; i++)
		mvAbs	+= Mh[i]*Mh[i];
	
	//shift mode until convergence (mvAbs = 0)...
	int iterationCount = 1;
	while((mvAbs >= EPSILON)&&(iterationCount < LIMIT))
	{
		//shift mode...
		for(i = 0; i < gridN; i++)
			mode[i]	+= Mh[i];
		
		//re-calculate mean shift vector at new
		//window location have center defined by
		//mode
		LatticeMSVector(Mh, mode);
		
		//calculate mvAbs = |Mh|^2
		mvAbs = 0;
		for(i = 0; i < gridN; i++)
			mvAbs	+= Mh[i]*Mh[i];
		
		//increment interation count...
		iterationCount++;
		
	}
	
	//shift mode...
	for(i = 0; i < gridN; i++)
		mode[i]	+= Mh[i];
	
	//de-allocate memory
	delete [] Mh;
	
	//done.
	return;
	
}

/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@    PROTECTED METHODS    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /* Mean Shift: Using kd-Tree  */
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Mean Shift Vector                                    */
/*******************************************************/
/*Computes the mean shift vector at a window location  */
/*yk using input data set x using a custom, user defin-*/
/*ed kernel.                                           */
/*******************************************************/
/*Pre:                                                 */
/*      - input data has been uploaded into the private*/
/*        data members of the MeanShift class          */
/*      - a window center yk has been defined          */
/*      - uniformKernel indicates the which type of    */
/*        kernel to be used by this procedure: uniform */
/*        or general                                   */
/*Post:                                                */
/*      - the mean shift vector calculated at yk       */
/*        using a either a custom, user defined kernel */
/*        or a uniform kernel is returned              */
/*******************************************************/

void MeanShift::MSVector(double *Mh_ptr, double *yk_ptr)
{
	
	// Declare Variables
	int i,j;
	
	// Initialize mean shift vector
	for(i = 0; i < N; i++)
		Mh_ptr[i] = 0;
	
	// Initialize wsum to zero, the sum of the weights of each
	// data point found to lie within the search window (sphere)
	wsum = 0;
	
	// Build Range Vector using h[i] and yk
	
	int s = 0;
	
	// The flag uniformKernel is used to determine which
	// kernel function is to be used in the calculation
	// of the mean shift vector
	if(uniformKernel)
    {
		for(i = 0; i < kp; i++)
		{
			for(j = 0; j < P[i]; j++)
			{
				range[2*(s+j)  ] = (float)(yk_ptr[s+j] - h[i]);
				range[2*(s+j)+1] = (float)(yk_ptr[s+j] + h[i]);
			}
			s += P[i];
		}
    }
	else
    {
		for(i = 0; i < kp; i++)
		{
			for(j = 0; j < P[i]; j++)
			{
				range[2*(s+j)  ] = (float)(yk_ptr[s+j] - h[i]*float(sqrt(offset[i])));
				range[2*(s+j)+1] = (float)(yk_ptr[s+j] + h[i]*float(sqrt(offset[i])));
			}
			s += P[i];
		}
    }
	
	// Traverse through the data set x, performing the
	// weighted sum of each point xi that lies within
	// the search window (sphere) using a general,
	// user defined kernel or uniform kernel depending
	// on the uniformKernel flag
	if(uniformKernel)
		uniformSearch(root, 0, Mh_ptr, yk_ptr);
	else
		generalSearch(root, 0, Mh_ptr, yk_ptr);
	
	// Calculate the mean shift vector using Mh and wsum
	for(i = 0; i < N; i++)
    {
		
		// Divide Sum by wsum
		Mh_ptr[i] /= wsum;
		
		// Calculate mean shift vector: Mh(yk) = y(k+1) - y(k)
		Mh_ptr[i] -= yk_ptr[i];
		
    }
	
	//done.
	return;
	
}

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*  Mean Shift: Using Lattice */
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Lattice Mean Shift Vector                            */
/*******************************************************/
/*Computes the mean shift vector at a specfied window  */
/*yk using the lattice data structure.                 */
/*******************************************************/
/*Pre:                                                 */
/*      - Mh_ptr and yh_ptr are arrays of doubles con- */
/*        aining N+2 elements                          */
/*      - Mh_ptr is the mean shift vector calculated   */
/*        at window center yk_ptr                      */
/*Post:                                                */
/*      - the mean shift vector at the window center   */
/*        pointed to by yk_ptr has been calculated and */
/*        stored in the memory location pointed to by  */
/*        Mh_ptr                                       */
/*******************************************************/

void MeanShift::LatticeMSVector(double *Mh_ptr, double *yk_ptr)
{
	
	// Initialize mean shift vector
	register int i;
	for(i = 0; i < N+2; i++)
		Mh_ptr[i] = 0;
	
	// Initialize wsum
	wsum = 0;
	
	// Perform lattice search summing
	// all the points that lie within the search
	// window defined using the kernel specified
	//by uniformKernel
	if(uniformKernel)
		uniformLSearch(Mh_ptr, yk_ptr);
	else
		generalLSearch(Mh_ptr, yk_ptr);
	
	// Compute mean shift vector using sum computed
	// by lattice search, wsum, and yk_ptr:
	// Mh = Mh/wsum - yk_ptr 
	
	if (wsum > 0)
	{
		for(i = 0; i < N+2; i++)
			Mh_ptr[i] = Mh_ptr[i]/wsum - yk_ptr[i];
	}
	else
	{
		for(i = 0; i < N+2; i++)
			Mh_ptr[i] = 0;
	}
	
	// done.
	return;
	
}

/*******************************************************/
/*Optimized Lattice Mean Shift Vector                  */
/*******************************************************/
/*Computes the mean shift vector at a specfied window  */
/*yk using the lattice data structure. Also the points */
/*that lie within the window are stored into the basin */
/*of attraction structure used by the optimized mean   */
/*shift algorithms.									   */
/*******************************************************/
/*Pre:                                                 */
/*      - Mh_ptr and yh_ptr are arrays of doubles con- */
/*        aining N+2 elements                          */
/*      - Mh_ptr is the mean shift vector calculated   */
/*        at window center yk_ptr                      */
/*Post:                                                */
/*      - the mean shift vector at the window center   */
/*        pointed to by yk_ptr has been calculated and */
/*        stored in the memory location pointed to by  */
/*        Mh_ptr                                       */
/*      - the data points lying within h of of yk_ptr  */
/*        have been stored into the basin of attract-  */
/*        ion data structure.                          */
/*******************************************************/

void MeanShift::OptLatticeMSVector(double *Mh_ptr, double *yk_ptr)
{
	
	// Initialize mean shift vector
	register int i;
	for(i = 0; i < N+2; i++)
		Mh_ptr[i] = 0;
	
	// Initialize wsum
	wsum = 0;
	
	// Perform lattice search summing
	// all the points that lie within the search
	// window defined using the kernel specified
	//by uniformKernel
	if(uniformKernel)
		optUniformLSearch(Mh_ptr, yk_ptr);
	else
		optGeneralLSearch(Mh_ptr, yk_ptr);
	
	// Compute mean shift vector using sum computed
	// by lattice search, wsum, and yk_ptr:
	// Mh = Mh/wsum - yk_ptr 
	
   if (wsum > 0)
   {
	   for(i = 0; i < N+2; i++)
   		Mh_ptr[i] = Mh_ptr[i]/wsum - yk_ptr[i];
   } else
   {
      for (i=0; i< N+2; i++)
         Mh_ptr[i] = 0;
   }
	
	// done.
	return;
	
}

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** Kernel-Input Data Consistency  ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Class Consistency Check                              */
/*******************************************************/
/*Checks the state of the class prior to the applicat- */
/*ion of mean shift.                                   */
/*******************************************************/
/*Pre:                                                 */
/*      - iN is the specified dimension of the input,  */
/*        iN = N for a general input data set, iN = N  */
/*        + 2 for a input set defined using a lattice  */
/*Post:                                                */
/*      - if the kernel has not been created, an input */
/*        has not been defined and/or the specified    */
/*        input dimension (iN) does not match that of  */
/*        the kernel a fatal error is flagged.         */
/*******************************************************/

void MeanShift::classConsistencyCheck(int iN, bool usingLattice)
{
	
	//make sure that kernel has been created...
	if(class_state.KERNEL_DEFINED == false)
	{
		ErrorHandler("MeanShift", "classConsistencyCheck", "Kernel not created.");
		return;
	}
	
	//make sure input data set has been loaded into mean shift object...
	if((class_state.INPUT_DEFINED == false)&&(!usingLattice))
	{
		ErrorHandler("MeanShift", "classConsistencyCheck", "No input data specified.");
		return;
	}
	
	//make sure that the lattice is defined if it is being used
	if((class_state.LATTICE_DEFINED == false)&&(usingLattice))
	{
		ErrorHandler("MeanShift", "classConsistencyCheck", "Latice not created.");
		return;
	}
	
	//make sure that dimension of the kernel and the input data set
	//agree
	
	//calculate dimension of kernel (kN)
	int i, kN	= 0;
	for(i = 0; i < kp; i++)
		kN	+= P[i];
	
	//perform comparison...
	if(iN != kN)
	{
		ErrorHandler("MeanShift", "classConsitencyCheck", "Kernel dimension does not match defined input data dimension.");
		return;
	}
	
	//done.
	return;
	
}

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** Class Error Handler  ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Error Handler                                        */
/*******************************************************/
/*Class error handler.                                 */
/*******************************************************/
/*Pre:                                                 */
/*      - className is the name of the class that fl-  */
/*        agged an error                               */
/*      - methodName is the name of the method that    */
/*        flagged an error                             */
/*      - errmsg is the error message given by the     */
/*        calling function                             */
/*Post:                                                */
/*      - the error message errmsg is flagged on beh-  */
/*        ave of method methodName belonging to class  */
/*        className:                                   */
/*                                                     */
/*        (1) ErrorMessage has been updated with the   */
/*            appropriate error message using the arg- */
/*            ments passed to this method.             */
/*        (2) ErrorStatus is set to ERROR              */
/*            (ErrorStatus = 1)                        */
/*******************************************************/

void MeanShift::ErrorHandler(const char *className, const char *methodName, const char* errmsg)
{
	
	//store trace into error message
	strcpy(ErrorMessage, className);
	strcat(ErrorMessage, "::");
	strcat(ErrorMessage, methodName);
	strcat(ErrorMessage, " Error: ");
	
	//store message into error message
	strcat(ErrorMessage, errmsg);
	
	//set error status to ERROR
	ErrorStatus = EL_ERROR;
	
	
}

/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@     PRIVATE METHODS     @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** Kernel Creation/Manipulation ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Generate Lookup Table                                */
/*******************************************************/
/*A weight function look up table is generated.        */
/*******************************************************/
/*Pre:                                                 */
/*      - kernel is an array of kernelTypes specifying */
/*        the type of kernel to be used on each sub-   */
/*        space of the input data set x                */
/*      - kp is the total number of subspaces used to  */
/*        the input data set x                         */
/*      - the above information has been pre-loaded    */
/*        into the MeanShift class private members     */
/*Post:                                                */
/*      - a lookup table is generated for the weight   */
/*        function of the resulting kernel             */
/*      - uniformKernel is set to true if the kernel   */
/*        to be used is uniform, false is returned     */
/*        otherwise                                    */
/*      - if a user defined weight function is requred */
/*        for a given subspace but not defined in the  */
/*        user defined weight function list, an error  */
/*        is flagged and the program is halted         */
/*******************************************************/

void MeanShift::generateLookupTable( void )
{
	
	// Declare Variables
	int i,j;
	
	// Allocate memory for lookup table w
	w = new double*[kp];
	
	// Traverse through kernel generating weight function
	// lookup table w
	
	// Assume kernel is uniform
	uniformKernel = true;
	
	for(i = 0; i < kp; i++)
    {
		switch(kernel[i])
		{
			// *Uniform Kernel* has weight funciton w(u) = 1
			// therefore, a weight funciton lookup table is
			// not needed for this kernel --> w[i] = NULL indicates
			// this
		case Uniform:
			
			w        [i] = NULL;  //weight function not needed for this kernel
			offset   [i] =    1;  //uniform kernel has u < 1.0
			increment[i] =    1;  //has no meaning
			break;
			
			// *Gaussian Kernel* has weight function w(u) = constant*exp(-u^2/[2h[i]^2])
		case Gaussian:
			
			// Set uniformKernel to false
			uniformKernel = false;
			
			// generate weight function using expression,
			// exp(-u/2), where u = norm(xi - x)^2/h^2
			
			// Allocate memory for weight table
			w[i] = new double [GAUSS_NUM_ELS+1];
			
			for(j = 0; j <= GAUSS_NUM_ELS; j++)
				w[i][j] = exp(-j*GAUSS_INCREMENT/2);
			
			// Set offset = offset^2, and set increment
			offset   [i] = (float)(GAUSS_LIMIT*GAUSS_LIMIT);
			increment[i] = GAUSS_INCREMENT;
			
			// done
			break;
			
			// *User Define Kernel* uses the weight function wf(u)
		case UserDefined:
			
			// Set uniformKernel to false
			uniformKernel = false;
			
			// Search for user defined weight function
			// defined for subspace (i+1)
			cur = head;
			while((cur)&&(cur->subspace != (i+1)))
				cur = cur->next;
			
			// If a user defined subspace has not been found
			// for this subspace, flag an error
			if(cur == NULL)
			{
				fprintf(stderr, "\ngenerateLookupTable Fatal Error: User defined kernel for subspace %d undefined.\n\nAborting Program.\n\n", i+1);
				exit(1);
			}
			
			// Otherwise, copy weight function lookup table to w[i]
			w[i] = new double [cur->sampleNumber+1];
			for(j = 0; j <= cur->sampleNumber; j++)
				w[i][j] = cur->w[j];
			
			// Set offset and increment accordingly
			offset   [i] = (float)(cur->halfWindow);
			increment[i] = cur->halfWindow/(float)(cur->sampleNumber);
			
			// done
			break;
			
		default:
			
			ErrorHandler("MeanShift", "generateLookupTable", "Unknown kernel type.");
			
		}
		
    }
}

/*******************************************************/
/*Destroy Kernel                                       */
/*******************************************************/
/*Destroys and initializes kernel.                     */
/*******************************************************/
/*Post:                                                */
/*      - memory for the kernel private data members   */
/*        have been destroyed and the kernel has been  */
/*        initialized for re-use.                      */
/*******************************************************/

void MeanShift::DestroyKernel( void )
{
	
	//de-allocate memory...
	if(kernel)	delete	[] kernel;
	if     (h)	delete	[] h;
	if     (P)	delete	[] P;
	if (range)	delete	[] range;

   if (uv) delete [] uv;
   if(increment) delete [] increment;
   if (offset) delete [] offset;
   
   if (kp>0)
   {
      if (w)
      {
         int i;
         for (i=0; i<kp; i++)
            delete [] w[i];
         delete [] w;
      }
      w = NULL;
   }
   
	//intialize kernel for re-use...
	kp		= 0;
	kernel	= NULL;
	h		= NULL;
	P		= NULL;
	range	= NULL;

   increment = NULL;
   uv = NULL;
   offset = NULL;
	
	//done.
	return;
	
}

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** Input Data Initialization/Destruction  ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Create Binary Search Tree                            */
/*******************************************************/
/*Uploads input data set x into a kd-BST.              */
/*******************************************************/
/*Pre:                                                 */
/*      - x is a one dimensional array of L N-dimensi- */
/*        onal data points                             */
/*Post:                                                */
/*      - x has been uploaded into a balanced kd-BST   */
/*        data structure for use by the mean shift     */
/*        procedure                                    */
/*******************************************************/

void MeanShift::CreateBST( void )
{
	
	// Create BST using data....
	
	// Allocate memory for tree
	forest = new tree[L];
	
	// Populate 'forest' of tree's with
	// the values stored in x
	int i;
	for(i = 0; i < L; i++)
    {
		forest[i].x      = &data[i*N];
		forest[i].right  = NULL;
		forest[i].left   = NULL;
		forest[i].parent = NULL;
    }
	
	// Build balanced Nd-tree from the
	// forest of trees generated above
	// retaining the root of this tree
	
	root = BuildKDTree(forest, L, 0, NULL);
	
	//done.
	return;
	
}

/*******************************************************/
/*Initialize Input                                     */
/*******************************************************/
/*Allocates memory for and initializes the input data  */
/*structure.                                           */
/*******************************************************/
/*Pre:                                                 */
/*      - x is a floating point array of L, N dimens-  */
/*        ional input data points                      */
/*Post:                                                */
/*      - memory has been allocated for the input data */
/*        structure and x has been stored using into   */
/*        the mean shift class using the resulting     */
/*        structure.                                   */
/*******************************************************/

void MeanShift::InitializeInput(float *x)
{
	
	//allocate memory for input data set
	if(!(data = new float [L*N]))
	{
		ErrorHandler("MeanShift", "InitializeInput", "Not enough memory.");
		return;
	}
	
	//copy x into data
	int i;
	for(i = 0; i < L*N; i++)
		data[i]	= x[i];
	
	//done.
	return;
	
}

/*******************************************************/
/*Reset Input                                          */
/*******************************************************/
/*De-allocates memory for and re-intializes input data */
/*structure.                                           */
/*******************************************************/
/*Post:                                                */
/*      - the memory of the input data structure has   */
/*        been de-allocated and this strucuture has    */
/*        been initialized for re-use.                 */
/*******************************************************/

void MeanShift::ResetInput( void )
{
	
	//de-allocate memory of input data structure (BST)
	if(data)	delete [] data;
	if(forest)	delete [] forest;
	
	//initialize input data structure for re-use
	data	= NULL;
	forest	= NULL;
	root	= NULL;
	L		= 0;
	N		= 0;
	width	= 0;
	height	= 0;
	
	//re-set class input to indicate that
	//an input is not longer stored by
	//the private data members of this class
	class_state.INPUT_DEFINED	= class_state.LATTICE_DEFINED = false;
	
}

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** k-dimensional Binary Search Tree ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Build KD Tree (for Tree Structure)                   */
/*******************************************************/
/*Builds a KD Tree given a forest of tree's.           */
/*******************************************************/
/*Pre:                                                 */
/*      - subset is a subset of L un-ordered tree nodes*/
/*        each containing an N-dimensional data point  */
/*      - d is the depth of the subset, used to specify*/
/*        the dimension used to construct the tree at  */
/*        the given depth                              */
/*      - parent is the parent tree of subset          */
/*Post:                                                */
/*      - a balanced KD tree has been constructed using*/
/*        the forest subset, the root of this tree has */
/*        been returned                                */
/*******************************************************/

tree *MeanShift::BuildKDTree(tree *subset, int length, int d, tree* parent)
{
	
	// If the subset is a single tree
	// then return this tree otherwise
	// partition the subset and place
	// these subsets recursively into
	// the left and right sub-trees having
	// their root specified by the median
	// of this subset in dimension d
	if(length == 1)
	{
		subset->parent = parent;
		return subset;
	}
	else if(length > 1)
    {
		
		// Sort Subset
		QuickMedian(subset, 0, length-1, d);
		
		// Get Median of Subset and Partition
		// it into two sub-trees - create
		// a tree with its root being the median
		// of the subset and its left and right
		// children being the medians of the subsets
		int median            = length/2;
		subset[median].parent = parent;
		subset[median].left   = BuildKDTree(subset           , median         , (d+1)%N, &subset[median]);
		subset[median].right  = BuildKDTree(&subset[median+1], length-median-1, (d+1)%N, &subset[median]);
		
		// Output tree structure
		return &subset[median];
		
    }
	else
		return NULL;
	
	//done.
	
}

/*******************************************************/
/*Quick Median (for Tree Structure)                    */
/*******************************************************/
/*Finds the median element in an un-ordered set, re-   */
/*structuring the set such that points less than the   */
/*median point are located to the left of the median   */
/*and points greater than the median point are located */
/*to the right.                                        */
/*******************************************************/
/*Pre:                                                 */
/*      - arr is a subset of tree nodes whose leftmost */
/*        element is specified by left and rightmost   */
/*        element is specified by left                 */
/*      - d is the dimension of the data set stored by */
/*        the tree structure that is used to find      */
/*        the median                                   */
/*Post:                                                */
/*      - the median point is found and the subset     */
/*        of trees is re-ordered such that all trees   */
/*        whose data points with d dimensional value   */
/*        less than that of the median tree node are   */
/*        located to the left of the median tree node, */
/*        otherwise they are located to the right      */
/*******************************************************/

void MeanShift::QuickMedian(tree *arr, int left, int right, int d)
{
	unsigned long k;
	unsigned long n;
	float* a;
	float* temp;
	n = right-left+1;
	k = n/2 + 1;
	unsigned long i, ir, j, l, mid;
	
	l = 1;
	ir = n;
	for (;;)
	{
		if (ir <= l+1)
		{
			if (ir == l+1 && arr[ir-1].x[d] < arr[l-1].x[d])
			{
				SWAP(arr[l-1].x, arr[ir-1].x)
			}
			return;
		} else
		{
			mid = (l+ir) >> 1;
			SWAP(arr[mid-1].x, arr[l+1-1].x)
				if (arr[l-1].x[d] > arr[ir-1].x[d])
				{
					SWAP(arr[l-1].x, arr[ir-1].x)
				}
				if (arr[l+1-1].x[d] > arr[ir-1].x[d])
				{
					SWAP(arr[l+1-1].x, arr[ir-1].x)
				}
				if (arr[l-1].x[d] > arr[l+1-1].x[d])
				{
					SWAP(arr[l-1].x, arr[l+1-1].x)
				}
				i = l+1;
				j = ir;
				a = arr[l+1-1].x;
				for (;;) {
					do i++; while (arr[i-1].x[d] < a[d]);
					do j--; while (arr[j-1].x[d] > a[d]);
					if (j<i) break;
					SWAP(arr[i-1].x, arr[j-1].x)
				}
				arr[l+1-1].x = arr[j-1].x;
				arr[j-1].x = a;
				if (j>=k) ir = j-1;
				if (j<=k) l = i;
		}
	}
}

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /*** Mean Shift: Using kd-Tree  ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Uniform Search                                       */
/*******************************************************/
/*Searches the input data using a kd-tree, performs the*/
/*sum on the data within the Hypercube defined by the  */
/*tree using a uniform kernel.                         */
/*******************************************************/
/*Pre:                                                 */
/*      - gt is a possibly NULL pointer to a kd tree   */
/*      - Mh_ptr is a pointer to the mean shift vector */
/*        being calculated                             */
/*      - yk_ptr is a pointer to the current window    */
/*        center location                              */
/*      - gd is the depth of the current subtree       */
/*Post:                                                */
/*      - the mean of the points within the Hypercube  */
/*        of the kd tree is computed using a uniform   */
/*        kernel                                       */
/*******************************************************/

void MeanShift::uniformSearch(tree *gt, int gd, double *Mh_ptr, double *yk_ptr)
{
	tree* c_t;
	int c_d;
	int i;
	int actionType;
	
	c_t = gt;
	c_d = gd;
	actionType = 0;
	
	double el, diff;
	int k, j, s;
	
	while (c_t != NULL)
	{
		switch(actionType) {
		case 0: // forward
			if ((c_t->x[c_d] > range[2*c_d]) && ((c_t->left) != NULL))
			{
				c_t = c_t->left;
				c_d = (c_d+1)%N;
			} else
			{
				actionType = 1; 
			}
			break;
		case 1: // backleft
			
			for(i = 0; i < N; i++)
			{
				if((c_t->x[i] < range[2*i])||(c_t->x[i] > range[2*i+1]))
					break;
			}
			
			if(i == N)
			{
				
				// ***     Visit Tree       ***
				
				// Check if xi is in the window centered about yk_ptr
				// If so - use it to compute y(k+1)
				diff = 0;
				j = 0;
				s = 0;
				while((diff < 1.0)&&(j < kp)) // Partial Distortion Search (PDS)
				{
					
					// test each sub-dimension independently
					diff  = 0;
					for(k = 0; k < P[j]; k++)
					{
						el = (c_t->x[s+k] - yk_ptr[s+k])/h[j];
						diff += el*el;
					}
					
					s += P[j];                        // next subspace
					j++;
					
				}
				
				if(diff < 1.0)
				{
					wsum += 1;
					for(j = 0; j < N; j++)
						Mh_ptr[j] += c_t->x[j];
				}
				
			}
			if ((c_t->x[c_d] < range[2*c_d+1]) && ((c_t->right) != NULL))
			{
				c_t = c_t->right;
				c_d = (c_d+1)%N;
				actionType = 0;
			} else
			{
				actionType = 2;
			}
			break;
		case 2: // backright
			c_d = (c_d+N-1)%N;
			
			if (c_t->parent == NULL)
			{
				c_t = NULL;
				break;
			}
			
			if (c_t->parent->left == c_t)
				actionType = 1;
			else
				actionType = 2;
			c_t = c_t->parent;
			break;
		}
	}
}

/*******************************************************/
/*General Search                                       */
/*******************************************************/
/*Searches the input data using a kd tree, performs the*/
/*sum on the data within the Hypercube defined by the  */
/*tree using a general kernel.                         */
/*******************************************************/
/*Pre:                                                 */
/*      - gt is a possibly NULL pointer to a kd tree   */
/*      - Mh_ptr is a pointer to the mean shift vector */
/*        being calculated                             */
/*      - yk_ptr is a pointer to the current window    */
/*        center location                              */
/*      - gd is the depth of the current subtree       */
/*Post:                                                */
/*      - the mean of the points within the Hypercube  */
/*        of the kd tree is computed using a general   */
/*        kernel                                       */
/*******************************************************/

void MeanShift::generalSearch(tree *gt, int gd, double *Mh_ptr, double *yk_ptr)
{
	tree* c_t;
	int c_d;
	int i;
	int actionType;
	
	c_t = gt;
	c_d = gd;
	actionType = 0;
	
	double el, diff, u, tw, y0, y1;
	int k, j, s, x0, x1;
	
	while (c_t != NULL)
	{
		switch(actionType) {
		case 0: // forward
			if ((c_t->x[c_d] > range[2*c_d]) && ((c_t->left) != NULL))
			{
				c_t = c_t->left;
				c_d = (c_d+1)%N;
			} else
			{
				actionType = 1; 
			}
			break;
		case 1: // backleft
			
			for(i = 0; i < N; i++)
			{
				if((c_t->x[i] < range[2*i])||(c_t->x[i] > range[2*i+1]))
					break;
			}
			
			if(i == N)
			{
				
				// ***      Visit Tree      ***
				
				// Check if xi is in the window centered about yk_ptr
				// If so - use it to compute y(k+1)
				s = 0;
				for(j = 0; j < kp; j++)
				{
					
					// test each sub-dimension independently
					diff  = 0;
					for(k = 0; k < P[j]; k++)
					{
						el = (c_t->x[s+k] - yk_ptr[s+k])/h[j];
						diff += uv[s+k] = el*el;      // Update uv and diff
						if(diff >= offset[j])         // Partial Distortion Search (PDS)
							break;
					}
					
					if(diff >= offset[j])             // PDS
						break;
					
					s += P[j];                        // next subspace
					
				}
				
				// j == kp indicates that all subspaces passed the test:
				// the data point is within the search window
				if(j == kp) j--;
				if(diff < offset[j])
				{
					
					// Initialize total weight to 1
					tw = 1;
					
					// Calculate weight factor using weight function
					// lookup tables and uv
					s = 0;
					for(j = 0; j < kp; j++)
					{
						if(kernel[j]) // not uniform kernel
						{
							// Compute u[i]
							u = 0;
							for(k = 0; k < P[j]; k++)
								u += uv[s+k];
							
							// Accumulate tw using calculated u
							// and weight function lookup table
							
							// Linear interpolate values given by
							// lookup table
							
							// Calculate x0 and x1, the points surounding
							// u
							x0 = (int)(u/increment[j]);
							x1 = x0+1;
							
							// Get y0 and y1 from the lookup table
							y0 = w[j][x0];
							y1 = w[j][x1];
							
							// Accumulate tw using linear interpolation
							tw *= (((double)(x1)*increment[j] - u)*y0+(u - (double)(x0)*increment[j])*y1)/(double)(x1*increment[j] - x0*increment[j]);
							
						}
						s += P[j];                               // next subspace
					}
					
					// Perform weighted sum using xi
					for(j = 0; j < N; j++)
						Mh_ptr[j] += tw*c_t->x[j];
					
					// Increment wsum by tw
					wsum += tw;
					
				}
			}
			if ((c_t->x[c_d] < range[2*c_d+1]) && ((c_t->right) != NULL))
			{
				c_t = c_t->right;
				c_d = (c_d+1)%N;
				actionType = 0;
			} else
			{
				actionType = 2;
			}
			break;
		case 2: // backright
			c_d = (c_d+N-1)%N;
			
			if (c_t->parent == NULL)
			{
				c_t = NULL;
				break;
			}
			
			if (c_t->parent->left == c_t)
				actionType = 1;
			else
				actionType = 2;
			c_t = c_t->parent;
			break;
      }
   }
}

  /*/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
  /***  Mean Shift: Using Lattice ***/
  /*\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/

/*******************************************************/
/*Uniform Lattice Search                               */
/*******************************************************/
/*Performs search on data set for all points lying     */
/*within the search window defined using a uniform     */
/*kernel. Their point-wise sum and count is computed   */
/*and returned.                                        */
/*                                                     */
/*NOTE: This method is the only method in the          */
/*      MeanShift class that uses the weight           */
/*      map asside from optUniformLSearch.             */
/*******************************************************/
/*Pre:                                                 */
/*      - Mh_ptr is a length N array of doubles        */
/*      - yk_ptr is a length N array of doubles        */
/*      - Mh_ptr is the sum of the data points found   */
/*        within search window having center yk_ptr    */
/*Post:                                                */
/*      - a search on the data set using the lattice   */
/*        has been performed, and all points found to  */
/*        lie within the search window defined using   */
/*        a uniform kernel are summed and counted.     */
/*      - their point wise sum is pointed to by Mh_ptr */
/*        and their count is stored by wsum.           */
/*******************************************************/

void MeanShift::uniformLSearch(double *Mh_ptr, double *yk_ptr)
{
	
	//Declare variables
	register int	i, j, k;
	int				s, p, dataPoint, lN;
	double			diff, el, dx, dy, tx, weight;
	
	//Define lattice data dimension...
	lN	= N + 2;
	
	//Define bounds of lattice...
	
	//the lattice is a 2dimensional subspace whose
	//search window bandwidth is specified by
	//h[0]:
	tx = yk_ptr[0] - h[0] + DELTA + 0.99;
	if (tx < 0)
		LowerBoundX = 0;
	else
		LowerBoundX = (int) tx;
	tx = yk_ptr[1] - h[0] + DELTA + 0.99;
	if (tx < 0)
		LowerBoundY = 0;
	else
		LowerBoundY = (int) tx;
	tx = yk_ptr[0] + h[0] - DELTA;
	if (tx >= width)
		UpperBoundX = width-1;
	else
		UpperBoundX = (int) tx;
	tx = yk_ptr[1] + h[0] - DELTA;
	if (tx >= height)
		UpperBoundY = height - 1;
	else
		UpperBoundY = (int) tx;
	
	//Perform search using lattice
	for(i = LowerBoundY; i <= UpperBoundY; i++)
		for(j = LowerBoundX; j <= UpperBoundX; j++)
		{
			
			//get index into data array
			dataPoint = N*(i*width+j);
			
			//Determine if inside search window
			k		= 1;
			s		= 0;
			dx      = j - yk_ptr[0];
			dy      = i - yk_ptr[1];
			diff	= (dx*dx+dy*dy)/(h[0]*h[0]);
			while((diff < 1.0)&&(k != kp)) // Partial Distortion Search
			{
				//Calculate distance squared of sub-space s	
				diff = 0;
				for(p = 0; p < P[k]; p++)
				{
					el    = (data[dataPoint+p+s]-yk_ptr[p+s+2])/h[k];               
					if((!p)&&(yk_ptr[2] > 80))
						diff += 4*el*el;
					else
						diff += el*el;
				}
				
				//next subspace
				s += P[k];
				k++;
			}
			
			//if its inside search window perform sum and count
			if(diff < 1.0)
			{
				weight = 1 - weightMap[i*width+j];
				Mh_ptr[0] += weight*j;
				Mh_ptr[1] += weight*i;
				for(k = 2; k < lN; k++)
					Mh_ptr[k] += weight*data[dataPoint+k-2];
				wsum += weight;
			}
			//done.
		}
		//done.		
		return;
		
}

/*******************************************************/
/*Optimized Uniform Latice Search                      */
/*******************************************************/
/*Performs search on data set for all points lying     */
/*within the search window defined using a uniform     */
/*kernel. Their point-wise sum and count is computed   */
/*and returned. Also the points that lie within the    */
/*window are stored into the basin of attraction stru- */
/*cture used by the optimized mean shift algorithms.   */
/*                                                     */
/*NOTE: This method is the only method in the          */
/*      MeanShift class that uses the weight           */
/*      map asside from uniformLSearch.                */
/*******************************************************/
/*Pre:                                                 */
/*      - Mh_ptr is a length N array of doubles        */
/*      - yk_ptr is a length N array of doubles        */
/*      - Mh_ptr is the sum of the data points found   */
/*        within search window having center yk_ptr    */
/*Post:                                                */
/*      - a search on the data set using the latice    */
/*        has been performed, and all points found to  */
/*        lie within the search window defined using   */
/*        a uniform kernel are summed and counted.     */
/*      - their point wise sum is pointed to by Mh_ptr */
/*        and their count is stored by wsum.           */
/*      - the data points lying within h of of yk_ptr  */
/*        have been stored into the basin of attract-  */
/*        ion data structure.                          */
/*******************************************************/

void MeanShift::optUniformLSearch(double *Mh_ptr, double *yk_ptr)
{
	
	//Declare variables
	register int	i, j, k;
	int				s, p, dataPoint, pointIndx, lN;
	double			diff, el, dx, dy, tx, weight;
	
	//Define latice data dimension...
	lN	= N + 2;
	
	//Define bounds of latice...
	
	//the latice is a 2dimensional subspace whose
	//search window bandwidth is specified by
	//h[0]:
	tx = yk_ptr[0] - h[0] + DELTA + 0.99;
	if (tx < 0)
		LowerBoundX = 0;
	else
		LowerBoundX = (int) tx;
	tx = yk_ptr[1] - h[0] + DELTA + 0.99;
	if (tx < 0)
		LowerBoundY = 0;
	else
		LowerBoundY = (int) tx;
	tx = yk_ptr[0] + h[0] - DELTA;
	if (tx >= width)
		UpperBoundX = width-1;
	else
		UpperBoundX = (int) tx;
	tx = yk_ptr[1] + h[0] - DELTA;
	if (tx >= height)
		UpperBoundY = height - 1;
	else
		UpperBoundY = (int) tx;
	
	//Perform search using latice
	for(i = LowerBoundY; i <= UpperBoundY; i++)
		for(j = LowerBoundX; j <= UpperBoundX; j++)
		{
			
			//get index into data array
			pointIndx	= i*width+j;
			dataPoint	= N*(pointIndx);
			
			//Determine if inside search window
			k		= 1;
			s		= 0;
			dx      = j - yk_ptr[0];
			dy      = i - yk_ptr[1];
			diff	= (dx*dx+dy*dy)/(h[0]*h[0]);
			while((diff < 1.0)&&(k != kp)) // Partial Distortion Search
			{
				//Calculate distance squared of sub-space s	
				diff = 0;
				for(p = 0; p < P[k]; p++)
				{
					el    = (data[dataPoint+p+s]-yk_ptr[p+s+2])/h[k];
					if((!p)&&(yk_ptr[2] > 80))
						diff += 4*el*el;
					else               
					   diff += el*el;
				}
				
				//next subspace
				s += P[k];
				k++;
			}
			
			//if its inside search window perform sum and count
			if(diff < 1.0)
			{
				weight = 1 - weightMap[i*width+j];
				Mh_ptr[0] += weight*j;
				Mh_ptr[1] += weight*i;
				for(k = 2; k < lN; k++)
					Mh_ptr[k] += weight*data[dataPoint+k-2];
				wsum += weight;
				
				//set basin of attraction mode table
            if (diff < 0.5)
            {
				   if(modeTable[pointIndx] == 0)
				   {
   					pointList[pointCount++]	= pointIndx;
					   modeTable[pointIndx]	= 2;
				   }
            }
				
			}
			
			//done.
			
		}
		
		//done.		
		return;
		
}

/*******************************************************/
/*General Lattice Search                               */
/*******************************************************/
/*Performs search on data set for all points lying     */
/*within the search window defined using a general     */
/*kernel. Their point-wise sum and count is computed   */
/*and returned.                                        */
/*******************************************************/
/*Pre:                                                 */
/*      - Mh_ptr is a length N array of doubles        */
/*      - yk_ptr is a length N array of doubles        */
/*      - Mh_ptr is the sum of the data points found   */
/*        within search window having center yk_ptr    */
/*Post:                                                */
/*      - a search on the data set using the lattice   */
/*        has been performed, and all points found to  */
/*        lie within the search window defined using   */
/*        a general kernel are summed and counted      */
/*      - their point wise sum is pointed to by Mh_ptr */
/*        and their count is stored by wsum            */
/*******************************************************/

void MeanShift::generalLSearch(double *Mh_ptr, double *yk_ptr)
{
	
	//Declare variables
	register int i, j, k;
	int			 s, p, dataPoint, lN, x0, x1;
	double		 diff, el, dx, dy, tw, u, y0, y1, tx;
	
	//Define lattice data dimension...
	lN	= N + 2;
	
	//Define bounds of lattice...
	
	//the lattice is a 2dimensional subspace whose
	//search window bandwidth is specified by
	//h[0]:
	tx = yk_ptr[0] - h[0] + DELTA + 0.99;
	if (tx < 0)
		LowerBoundX = 0;
	else
		LowerBoundX = (int) tx;
	tx = yk_ptr[1] - h[0] + DELTA + 0.99;
	if (tx < 0)
		LowerBoundY = 0;
	else
		LowerBoundY = (int) tx;
	tx = yk_ptr[0] + h[0] - DELTA;
	if (tx >= width)
		UpperBoundX = width-1;
	else
		UpperBoundX = (int) tx;
	tx = yk_ptr[1] + h[0] - DELTA;
	if (tx >= height)
		UpperBoundY = height - 1;
	else
		UpperBoundY = (int) tx;
	
	//Perform search using lattice
	for(i = LowerBoundY; i <= UpperBoundY; i++)
		for(j = LowerBoundX; j <= UpperBoundX; j++)
		{
			
			//get index into data array
			dataPoint = N*(i*width+j);
			
			//Determine if inside search window
			k		= 1;
			s		= 0;
			dx      = j - yk_ptr[0];
			dy      = i - yk_ptr[1];
			uv[0]	= (dx*dx)/(h[0]*h[0]);
			uv[1]	= (dy*dy)/(h[0]*h[0]);
			diff	= uv[0] + uv[1];
			while((diff < offset[k-1])&&(k != kp)) // Partial Distortion Search
			{
				//Calculate distance squared of sub-space s	
				diff = 0;
				for(p = 0; p < P[k]; p++)
				{
					el    = (data[dataPoint+p+s]-yk_ptr[p+s+2])/h[k];
					diff += uv[p+s+2] = el*el;
				}
				
				//next subspace
				s += P[k];
				k++;
			}
			
			//if its inside search window perform weighted sum and count
			if(diff < offset[k-1])
			{
				
				// Initialize total weight to 1
				tw = 1;
				
				// Calculate weight factor using weight function
				// lookup tables and uv
				s = 0;
				for(k = 0; k < kp; k++)
				{
					if(kernel[k]) // not uniform kernel
					{
						// Compute u[i]
						u = 0;
						for(p = 0; p < P[k]; p++)
							u += uv[s+p];
						
						// Accumulate tw using calculated u
						// and weight function lookup table
						
						// Linear interpolate values given by
						// lookup table
						
						// Calculate x0 and x1, the points surounding
						// u
						x0 = (int)(u/increment[k]);
						x1 = x0+1;
						
						// Get y0 and y1 from the lookup table
						y0 = w[k][x0];
						y1 = w[k][x1];
						
						// Accumulate tw using linear interpolation
						tw *= (((double)(x1)*increment[k] - u)*y0+(u - (double)(x0)*increment[k])*y1)/(double)(x1*increment[k] - x0*increment[k]);
						
					}
					s += P[k];                               // next subspace
				}
				
				// Perform weighted sum using xi
				Mh_ptr[0]	+= tw*j;
				Mh_ptr[1]	+= tw*i;
				for(k = 0; k < N; k++)
					Mh_ptr[k+2] += tw*data[dataPoint+k];
				
				// Increment wsum by tw
				wsum += tw;
				
			}
			
			//done.
			
		}
		
		//done.		
		return;
		
}

/*******************************************************/
/*Optimized General Lattice Search                     */
/*******************************************************/
/*Performs search on data set for all points lying     */
/*within the search window defined using a general     */
/*kernel. Their point-wise sum and count is computed   */
/*and returned. Also the points that lie within the    */
/*window are stored into the basin of attraction stru- */
/*cture used by the optimized mean shift algorithms.   */
/*******************************************************/
/*Pre:                                                 */
/*      - Mh_ptr is a length N array of doubles        */
/*      - yk_ptr is a length N array of doubles        */
/*      - Mh_ptr is the sum of the data points found   */
/*        within search window having center yk_ptr    */
/*Post:                                                */
/*      - a search on the data set using the lattice   */
/*        has been performed, and all points found to  */
/*        lie within the search window defined using   */
/*        a general kernel are summed and counted      */
/*      - their point wise sum is pointed to by Mh_ptr */
/*        and their count is stored by wsum            */
/*      - the data points lying within h*offset of     */
/*        yk_ptr have been stored into the basin of    */
/*        attraction data structure.                   */
/*******************************************************/

void MeanShift::optGeneralLSearch(double *Mh_ptr, double *yk_ptr)
{
	
	//Declare variables
	register int	i, j, k;
	int				s, p, dataPoint, pointIndx, lN, x0, x1;
	double			diff, el, dx, dy, tw, u, y0, y1, tx;
	
	//Define lattice data dimension...
	lN	= N + 2;
	
	//Define bounds of lattice...
	
	//the lattice is a 2dimensional subspace whose
	//search window bandwidth is specified by
	//h[0]:
	tx = yk_ptr[0] - h[0] + DELTA + 0.99;
	if (tx < 0)
		LowerBoundX = 0;
	else
		LowerBoundX = (int) tx;
	tx = yk_ptr[1] - h[0] + DELTA + 0.99;
	if (tx < 0)
		LowerBoundY = 0;
	else
		LowerBoundY = (int) tx;
	tx = yk_ptr[0] + h[0] - DELTA;
	if (tx >= width)
		UpperBoundX = width-1;
	else
		UpperBoundX = (int) tx;
	tx = yk_ptr[1] + h[0] - DELTA;
	if (tx >= height)
		UpperBoundY = height - 1;
	else
		UpperBoundY = (int) tx;
	
	//Perform search using lattice
	for(i = LowerBoundY; i <= UpperBoundY; i++)
		for(j = LowerBoundX; j <= UpperBoundX; j++)
		{
			
			//get index into data array
			pointIndx	= i*width+j;
			dataPoint = N*(i*width+j);
			
			//Determine if inside search window
			k		= 1;
			s		= 0;
			dx      = j - yk_ptr[0];
			dy      = i - yk_ptr[1];
			uv[0]	= (dx*dx)/(h[0]*h[0]);
			uv[1]	= (dy*dy)/(h[0]*h[0]);
			diff	= uv[0] + uv[1];
			while((diff < offset[k-1])&&(k != kp)) // Partial Distortion Search
			{
				//Calculate distance squared of sub-space s	
				diff = 0;
				for(p = 0; p < P[k]; p++)
				{
					el    = (data[dataPoint+p+s]-yk_ptr[p+s+2])/h[k];
					diff += uv[p+s+2] = el*el;
				}
				
				//next subspace
				s += P[k];
				k++;
			}
			
			//if its inside search window perform weighted sum and count
			if(diff < offset[k-1])
			{
				
				// Initialize total weight to 1
				tw = 1;
				
				// Calculate weight factor using weight function
				// lookup tables and uv
				s = 0;
				for(k = 0; k < kp; k++)
				{
					if(kernel[k]) // not uniform kernel
					{
						// Compute u[i]
						u = 0;
						for(p = 0; p < P[k]; p++)
							u += uv[s+p];
						
						// Accumulate tw using calculated u
						// and weight function lookup table
						
						// Linear interpolate values given by
						// lookup table
						
						// Calculate x0 and x1, the points surounding
						// u
						x0 = (int)(u/increment[k]);
						x1 = x0+1;
						
						// Get y0 and y1 from the lookup table
						y0 = w[k][x0];
						y1 = w[k][x1];
						
						// Accumulate tw using linear interpolation
						tw *= (((double)(x1)*increment[k] - u)*y0+(u - (double)(x0)*increment[k])*y1)/(double)(x1*increment[k] - x0*increment[k]);
						
					}
					s += P[k];                               // next subspace
				}
				
				// Perform weighted sum using xi
				Mh_ptr[0]	+= tw*j;
				Mh_ptr[1]	+= tw*i;
				for(k = 0; k < N; k++)
					Mh_ptr[k+2] += tw*data[dataPoint+k];
				
				// Increment wsum by tw
				wsum += tw;
				
				//set basin of attraction mode table
				if(modeTable[pointIndx] == 0)
				{
					pointList[pointCount++]	= pointIndx;
					modeTable[pointIndx]	= 2;
				}
				
			}
			
			//done.
			
		}
		
		//done.		
		return;
		
}

/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ END OF CLASS DEFINITION @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/