NICE_Segmentation/edisonSegm/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 ( ( char * ) "MeanShift", ( char * ) "CreateKernel", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "CreateKernel", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "CreateKernel", ( char * ) "Negative or zero valued bandwidths are prohibited." );
      return;
    }
    if ( ( P[i] = P_[i] ) <= 0 )
    {
      ErrorHandler ( ( char * ) "MeanShift", ( char * ) "CreateKernel", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "CreateKernel", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "UploadInput", ( char * ) "Input data set is NULL." );
    return;
  }

  //Obtain L and N
  if ( ( ( L = L_ ) <= 0 ) || ( ( N = N_ ) <= 0 ) )
  {
    ErrorHandler ( ( char * ) "MeanShift", ( char * ) "UploadInput", ( char * ) "Input data set has negative or zero length or dimension." );
    return;
  }

  //Allocate memory for data
  if ( ! ( data = new float [L*N] ) )
  {
    ErrorHandler ( ( char * ) "MeanShift", ( char * ) "UploadInput", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "DefineLInput", ( char * ) "Lattice defined using zero or negative height and/or width." );
    return;
  }

  //Obtain input data dimension
  if ( ( N = N_ ) <= 0 )
  {
    ErrorHandler ( ( char * ) "MeanShift", ( char * ) "DefineInput", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "InitializeInput", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "SetWeightMap", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "msVector", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "lmsVector", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "FindMode", ( char * ) "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 >= EPSILON2 ) && ( 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 ( ( char * ) "MeanShift", ( char * ) "FindLMode", ( char * ) "Invalid argument(s) passed to this method." );
    return;
  }

  //make sure the lattice height and width have been defined...
  if ( !height )
  {
    ErrorHandler ( ( char * ) "MeanShift", ( char * ) "FindLMode", ( char * ) "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 >= EPSILON2 ) && ( 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 ( ( char * ) "MeanShift", ( char * ) "classConsistencyCheck", ( char * ) "Kernel not created." );
    return;
  }

  //make sure input data set has been loaded into mean shift object...
  if ( ( class_state.INPUT_DEFINED == false ) && ( !usingLattice ) )
  {
    ErrorHandler ( ( char * ) "MeanShift", ( char * ) "classConsistencyCheck", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "classConsistencyCheck", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "classConsitencyCheck", ( char * ) "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 ( char *className, char *methodName, char* errmsg )
{

  //store trace into error message
  strcpy ( ErrorMessage, className );
  strcat ( ErrorMessage, ( char * ) "::" );
  strcat ( ErrorMessage, methodName );
  strcat ( ErrorMessage, ( char * ) " Error: (char *)" );

  //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, ( char * ) "\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 ( ( char * ) "MeanShift", ( char * ) "generateLookupTable", ( char * ) "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 ( ( char * ) "MeanShift", ( char * ) "InitializeInput", ( char * ) "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 @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/