//--------------------------------------------------------------------------------------
// GestureDetector.cpp
//
// Definitions for the gesture detector and gesture detector trainer, as well definitions
// for weak and strong classifiers. The gesture detector trainer uses the AdaBoost
// learning alogorith.
//
// Advanced Technology Group (ATG)
// Copyright (C) Microsoft Corporation. All rights reserved.
//--------------------------------------------------------------------------------------
#pragma once 
#if defined(GESTURE_TRAINER)

#include "GestureDetector.h"
#include <float.h>
#include <algorithm>
#include <assert.h>
#include <omp.h>
#include <malloc.h>

#include "FExample.h"
#include "IExample.h"
using namespace Examples;

#include <zlib.h>

struct SVariable
{
	float	mean;
	float	stddev;
	float	W;	//sum_weights;

	SVariable() {Reset();}
	void Reset() {mean = stddev = W = 0.0f;}
	void	AddSample(float f, float weight = 1.0f)
	{
		static float maxSamples = 10000.0f;
		float Q = (stddev*stddev) * W;
		Q += weight * (W / (W+weight)) * (f-mean) * (f-mean);
		if (W < maxSamples)	//avoid totally losing influence
			W += weight;
		mean += (f-mean) * (weight/ W);
		stddev = sqrt(Q / W);
	}
};

using namespace std;

namespace KinectGesture
{
    //--------------------------------------------------------------------------------------
    // Constants
    //--------------------------------------------------------------------------------------

    // Each feature used to generate weak classifiers has a min, max and step. This controls
    // how many weak classifiers gets generated per feature set. These values are either
    // easy to understand, e.g. min and max values of an angle is 0 and 180, or they were
    // empirically derived, e.g. what is the min and max velocity that your hand joint can
    // move at. By making the step smaller, you'll get a finer grain of decision stump, resulting
    // in more weak classifiers generated for that feature.
    static const FLOAT fAngleMin            = 0.0f;
    static const FLOAT fAngleMax            = 180.0f;
    static const FLOAT fAngleStep           = 2.0f;

    static const FLOAT fTimeSpaceAngleMin   = 90.0f;
    static const FLOAT fTimeSpaceAngleMax   = 180.0f;
    static const FLOAT fTimeSpaceAngleStep  = 1.0f;

	static const FLOAT fSpeedMin            = 0.0f;
	static const FLOAT fSpeedMax            = 10.0f;
	static const FLOAT fSpeedStep           = 0.05f;

	static const FLOAT fVelocityMin         = -5.0f;
	static const FLOAT fVelocityMax         = 5.0f;
	static const FLOAT fVelocityStep        = 0.1f;

    static const FLOAT fAngleVelocityMin    = -5.0f;
    static const FLOAT fAngleVelocityMax    = 5.0f;
    static const FLOAT fAngleVelocityStep   = 0.25f;

    static const FLOAT fAngleAccelMin       = -500.0f;
    static const FLOAT fAngleAccelMax       = 500.0f;
    static const FLOAT fAngleAccelStep      = 10.0f;

    static const FLOAT fMuscleForceMin      = -5.0f;
    static const FLOAT fMuscleForceMax      = 5.0f;
    static const FLOAT fMuscleForceStep     = 0.1f;

    static const FLOAT fMuscleTorqueMin     = -5.0f;
    static const FLOAT fMuscleTorqueMax     = 5.0f;
    static const FLOAT fMuscleTorqueStep    = 0.1f;

	static const FLOAT fMusclePowerMin      = -100.0f;
	static const FLOAT fMusclePowerMax      = 100.0f;
	static const FLOAT fMusclePowerStep     = 1.0f;

    static const FLOAT fDiffMuscleForceMin  = -1.0f;
    static const FLOAT fDiffMuscleForceMax  = 1.0f;
    static const FLOAT fDiffMuscleForceStep = 0.2f;

	static const FLOAT fPositionMin         = -1.0f;
	static const FLOAT fPositionMax         = 1.0f;
	static const FLOAT fPositionStep        = 0.1f;

    static const FLOAT fVelocitySQMin       = 0.0f;
    static const FLOAT fVelocitySQMax       = 25.0f;
    static const FLOAT fVelocitySQStep      = 0.1f;

    static const FLOAT fSpeedSQMin          = 0.0f;
    static const FLOAT fSpeedSQMax          = 100.0f;
    static const FLOAT fSpeedSQStep         = 0.25f;

    static const FLOAT fAccelMin		    = 0.0f;
    static const FLOAT fAccelMax		    = 20.0f;
    static const FLOAT fAccelStep			= 0.1f;

    static const FLOAT fBoneChangesMin      = 0.0f;
    static const FLOAT fBoneChangesMax      = 2.0f;
    static const FLOAT fBoneChangesStep     = 0.01f;

	static const FLOAT fOpticalFlowMin      = -0.5f;
	static const FLOAT fOpticalFlowMax      = 0.5f;
	static const FLOAT fOpticalFlowStep     = 0.01f;

	static const FLOAT fOpticalFlowLenSQMin  = 0.0f;
	static const FLOAT fOpticalFlowLenSQMax  = 0.25f;
	static const FLOAT fOpticalFlowLenSQStep = 0.001f;

	static const FLOAT fOpticalFlowTanMin   = -3.15f;
	static const FLOAT fOpticalFlowTanMax   = 3.15f;
	static const FLOAT fOpticalFlowTanStep  = 0.1f;

	static const FLOAT fOpticalFlowDiffMin  = -0.5f;
	static const FLOAT fOpticalFlowDiffMax  = 0.5f;
	static const FLOAT fOpticalFlowDiffStep = 0.01f;


    static const DOUBLE fMinErrorThreshold  = 0.25;
    static const DOUBLE fMaxErrorThreshold  = 0.5;

    // At runtime we get a per frame results, so we need to filter the results to a per gesture
    // result. The filter is implemented as a sliding window with two parameters, the size of
    // the sliding window and a threshold, almost like a amplitude and frequency. These constants
    // define a matrix of possible values of these two parameters which we'll use to find the
    // most optimum pair of parameters for filtering.
    static const FLOAT fDetectionParamsMinThreshold     = 0.0f;
    static const FLOAT fDetectionParamsMaxThreshold     = 0.1f;
    static const FLOAT fDetectionParamsThresholdStep    = 0.001f;
    static const UINT nDetectionParamsMinNumFrames      = 1;
    static const UINT nDetectionParamsMaxNumFrames      = 10;


    //--------------------------------------------------------------------------------------
    // Name: GestureDetectorTrainer()
    // Desc: Constructor
    //--------------------------------------------------------------------------------------

    GestureDetectorTrainer::GestureDetectorTrainer() : GestureDetector()
    {
        Reset();
        m_nTotalNumGestures             = 0;
        m_nNumTrainingGestures          = 0;
        m_nNumThreadsForTraining        = 0;
        m_nMaxNumThreads                = omp_get_max_threads();
        m_nNumWeakClassifiersAtRuntime  = 0;
        m_fErrorThreshold               = 0.0f;

		m_bUseSkeleton					= TRUE;
		m_bUseOpticalFlow				= FALSE;
		m_nFramesToSkip					= 0;
    }


    //--------------------------------------------------------------------------------------
    // Name: ~GestureDetectorTrainer
    // Desc: Destructor
    //--------------------------------------------------------------------------------------

    GestureDetectorTrainer::~GestureDetectorTrainer()
    {
        Reset();
    }


    //--------------------------------------------------------------------------------------
    // Name: Reset
    // Desc: Reset all state and delete allocated memory for labeled example data
    //--------------------------------------------------------------------------------------

    VOID GestureDetectorTrainer::Reset()
    {
        for ( UINT i = 0; i < KINECT_GESTURE_MAX_SIMULTANEOUS_GESTURES; i++ )
        {
            m_StrongClassifier.Reset( i );
        }

        m_uPreviousTimeStamp = 0;

        UINT nNumExamples = (UINT)( m_LabeledExamples.m_pExamples.size() );
        for ( UINT i = 0; i < nNumExamples; i++ )
        {
            if ( m_LabeledExamples.m_pExamples[ i ] )
            {
                _aligned_free( m_LabeledExamples.m_pExamples[ i ] );
                m_LabeledExamples.m_pExamples[ i ] = NULL;
            }
        }

        m_LabeledExamples.m_pExamples.clear();
        m_LabeledExamples.m_iLabels.clear();
        m_LabeledExamples.m_uTimeStamps.clear();

        m_nTotalNumGestures = 0;
        m_nNumTrainingGestures = 0;
    }


    //--------------------------------------------------------------------------------------
    // Name: Save
    // Desc: Save data
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::Save( const CHAR* szFileName )
    {
        FILE* pFile = NULL;
        fopen_s( &pFile, szFileName, "wb" );
        RETURN_ON_NULL( pFile );

        // Write a text identifier
        fwrite( getGestureFileIDs()[0], strlen( getGestureFileIDs()[0] ) + 1, 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

        // Write current version number
        FLOAT fBigEndianValue = ByteSwap32BitWrite( g_fCurrentVersion );
        fwrite( &fBigEndianValue, sizeof( fBigEndianValue ), 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

        const UINT nNumWeakClassifiers = m_StrongClassifier.GetNumWeakClassifiers();
        const UINT nNumClassifierData = (UINT)m_ClassifierData.size();

		//collect list of used ClassifierData
		vector<UINT> vUsed;
		vUsed.reserve(nNumClassifierData);
		for ( UINT j = 0; j < nNumClassifierData; j++ )
		{
			vUsed.push_back(0xffffffff);
		}
		UINT numUsed = 0;
		for ( UINT i = 0; i < nNumWeakClassifiers; i++ )
		{
			WeakClassifier *pWC = m_StrongClassifier.GetWeakClassifierAt(i);
			UINT uDataIndex = pWC->GetDataIndex();
			if ( vUsed[ uDataIndex ]==0xffffffff )
			{
				vUsed[ uDataIndex ] = numUsed++;
			}
		}

		// Write numbers
        UINT32 uBigEndianValue = ByteSwap32BitWrite( (UINT32)nNumWeakClassifiers );
        fwrite( &uBigEndianValue, sizeof( uBigEndianValue ), 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

        uBigEndianValue = ByteSwap32BitWrite( (UINT32)numUsed );
        fwrite( &uBigEndianValue, sizeof( uBigEndianValue ), 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

		// Write ClassifierData
		UINT index = 0;
        for ( UINT i = 0; i < nNumClassifierData; i++ )
        {
			if ( vUsed[ i ]!=0xffffffff )
			{
				//also temporarily set UID to index to match saved index in weak classifier (just for compatibility, but doesn't really matter)
				m_ClassifierData[ i ]->SetID( index );
				RETURN_ON_FAIL( m_ClassifierData[ i ]->Write( pFile ) );
				vUsed[ i ] = index;
				index++;
			}
        }

        RETURN_ON_FAIL( m_StrongClassifier.Write( pFile ) );

		// Write weak classifiers
        for ( UINT i = 0; i < nNumWeakClassifiers; i++ )
        {
			WeakClassifier *pWC = m_StrongClassifier.GetWeakClassifierAt(i);
			//temporarily convert in-memory index to in-file index (should match between ClassifierData and vUsed)
			UINT memIndex = pWC->GetDataIndex();
			ClassifierData *pData = m_ClassifierData[ pWC->GetDataIndex() ];
			assert( vUsed[ pWC->GetDataIndex() ] != 0xffffffff );
			assert( pData->GetID() == vUsed[ pWC->GetDataIndex() ] );
			pWC->SetDataIndex( pData->GetID() );
            RETURN_ON_FAIL( m_StrongClassifier.Write( pFile, i ) );
			//restore in-mem index
			pWC->SetDataIndex( memIndex );
        }

        fclose( pFile );

		//restore UIDs
		for ( UINT j = 0; j < nNumClassifierData; j++ )
		{
			if ( vUsed[ j ]!=0xffffffff )
			{
				m_ClassifierData[ j ]->SetID( m_ClassifierData[ j ]->MakeUID() );
			}
		}

        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: Train
    // Desc: Uses the AdaBoost training algorithm to train a strong classifier H(x) as a
    //       weighted sum of weak classifiers h(x)
    //--------------------------------------------------------------------------------------

	HRESULT GestureDetectorTrainer::Train( CHAR* const* szLeFiles, const UINT uiLeFileCount, const DOUBLE fAccuracyLevel, const UINT nNumWeakClassifiersAtRuntime,
                                            const FLOAT fWeightOfFalsePositivesWhenFiltering, const BOOL bOnlyRetrainDetectionParameters,
                                            const UINT uCPUAvailableForTraining, BOOL useSkeleton, BOOL useOpticalFlow, UINT framesToSkip )
    {
        m_nNumWeakClassifiersAtRuntime = nNumWeakClassifiersAtRuntime;

		m_bUseSkeleton = useSkeleton;
		m_bUseOpticalFlow = useOpticalFlow;
		m_nFramesToSkip = framesToSkip;

        // Accuracy level is an input value between between [0..1], which simply gets converted to a error threshold
        // value of maximum 0.5, since for AdaBoost weak classifiers has to be better than a 50/50 change to be correct
        m_fErrorThreshold = ( fAccuracyLevel * ( fMaxErrorThreshold - fMinErrorThreshold ) ) + fMinErrorThreshold;

        // Set how much CPU resources the user is willing to use for training. Calculate the number of threads from the % utilization
        m_nNumThreadsForTraining = (UINT)( 0.5f + ( m_nMaxNumThreads * uCPUAvailableForTraining / 100.0f ) );
        BOOL bSetDynamic = ( m_nNumThreadsForTraining == m_nMaxNumThreads ) ? FALSE : TRUE;
        omp_set_dynamic( bSetDynamic );
        omp_set_num_threads( m_nNumThreadsForTraining );

		Reset();

        printf( "\n\nStep 1 of 4: Loading Labeled Training Examples" );
        DWORD dwStart = GetTickCount();

		for ( UINT i = 0; i < uiLeFileCount; i++ )
		{
			RETURN_ON_FAIL( LoadLabeledExamples( szLeFiles[ i ] ) );
		}
		if( m_LabeledExamples.m_pExamples.size() == 0 )
		{
			printf( "\nERROR: No labeled examples loaded.", GetNumExamples() );
			return E_FAIL;
		}

        DWORD dwStop = GetTickCount();

        DWORD dwSeconds = ( (dwStop - dwStart ) / 1000 ) % 60;
        DWORD dwMinutes = ( (dwStop - dwStart ) / 1000) / 60;
        printf( "\n\tNum Labeled Examples: %d", GetNumExamples() );
        printf( "\n\tDuration: %d minutes, %d seconds",  dwMinutes, dwSeconds );
        printf( "\nDone\n" );

        printf( "\n\nStep 2 of 4: Generating a Pool of Weak Classifiers" );
        dwStart = GetTickCount();
        if ( !bOnlyRetrainDetectionParameters )
        {
            RETURN_ON_FAIL( TrainWeakClassifiers() );
        }
        dwStop = GetTickCount();

        dwSeconds = ( ( dwStop - dwStart ) / 1000 ) % 60;
        dwMinutes = ( ( dwStop - dwStart ) / 1000) / 60;
        printf( "\n\tNum weak classifiers generated: %d", GetNumWeakClassifiers() );
        printf( "\n\tDuration: %d minutes, %d seconds",  dwMinutes, dwSeconds );
        printf( "\nDone\n" );

        printf( "\n\nStep 3 of 4: Training Strong Classifier" );
        dwStart = GetTickCount();
        if ( !bOnlyRetrainDetectionParameters )
        {
            RETURN_ON_FAIL( TrainStrongClassifier( TRUE ) );
            Optimize( nNumWeakClassifiersAtRuntime );
        }
        dwStop = GetTickCount();

        dwSeconds = ( ( dwStop - dwStart ) / 1000 ) % 60;
        dwMinutes = ( ( dwStop - dwStart ) / 1000) / 60;
        printf( "\n\tNum weak classifiers: %d", GetNumWeakClassifiers() );
        printf( "\n\tDuration: %d minutes, %d seconds",  dwMinutes, dwSeconds );

        printf( "\n\nStep 4 of 4: Optimizing detection parameters" );
        dwStart = GetTickCount(); 
        OptimizeDetectionParameters( fWeightOfFalsePositivesWhenFiltering );
        dwStop = GetTickCount();

        dwSeconds = ( ( dwStop - dwStart ) / 1000 ) % 60;
        dwMinutes = ( ( dwStop - dwStart ) / 1000) / 60;
        printf( "\n\tFiltering %d frames using detection threshold %f", GetNumFramesToFilter(), GetDetectionThreshold() );
        printf( "\n\tDuration: %d minutes, %d seconds",  dwMinutes, dwSeconds );

        printf( "\nDone\n" );

        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: Test
    // Desc: Test the accuracy of the training algorithm
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::Test( const CHAR* szPath, const BOOL bTestOnTrainingData )
    {
        if ( !bTestOnTrainingData )
        {
            RETURN_ON_FAIL( LoadLabeledExamples( szPath ) );
        }

        // Clear all the data
        ClassifierData::Initialize();
        m_StrongClassifier.Reset( 0 );
        m_uPreviousTimeStamp = 0;
        XMVECTOR vUpVector = XMVectorSet( 0.0f, 1.0f, 0.0f, 0.0f );

		printf( "\nTesting...\n" );

		//test using our scoring method
		{
			int nGesturesLabeled[2] = {0,0};
			int nFrames[3] = {0,0,0};
			INT8 iLastLabel = 0;
			SVariable Stats[2];

			const UINT nNumExamples = (UINT)( m_LabeledExamples.m_pExamples.size() );
			for ( UINT i = 0; i < nNumExamples; i++ )
			{
				// Get the example from the ground truth training set
				GESTURE_SKELETON_TYPE* pSkeletonData = m_LabeledExamples.m_pExamples[ i ];
				UINT64 uTimeStamp = m_LabeledExamples.m_uTimeStamps[ i ];

				// Update the classifier data. Time stamps from Xed files are different to timestamps from runtime NUI_SKELETON_FRAMES
				LARGE_INTEGER liTimeStamp;
				liTimeStamp.QuadPart = uTimeStamp;
				Update( 0, pSkeletonData, liTimeStamp, vUpVector );

				INT8 label = m_LabeledExamples.m_iLabels[ i ];
				if (label == 0)
				{
					nFrames[2]++;
					iLastLabel = label;
					continue;	//skip
				}

				if (label == g_iClassificationLabelCorrect)
				{
					nFrames[1]++;
					if (iLastLabel!=label)
					{
						nGesturesLabeled[1]++;
					}
				} else
				if (label == g_iClassificationLabelIncorrect)
				{
					nFrames[0]++;
					if (iLastLabel!=label)
					{
						nGesturesLabeled[0]++;
						//ToDo: evaluate per-move stats
					}
				}
				iLastLabel = label;

				Results results;
				m_StrongClassifier.Detect( 0, m_ClassifierData, &results, FALSE );

				float fMin = m_StrongClassifier.GetMean( KinectGesture::StrongClassifier::eLabel_Incorrect ) +
							 m_StrongClassifier.GetStdDev( KinectGesture::StrongClassifier::eLabel_Incorrect );
				float fMax = m_StrongClassifier.GetDetectionThreshold();

				float score = ( results.m_fConfidence - fMin ) / ( fMax - fMin );

				Stats[label==g_iClassificationLabelCorrect ? 0 : 1].AddSample(score);
			}

			printf( "\nResults:" );
			printf( "\n\tpositive frames: %d", nFrames[1] );
			printf( "\n\tnegative frames: %d", nFrames[0] );
			printf( "\n\tignored frames: %d", nFrames[2] );
			printf( "\n\tpositive gestures: %d", nGesturesLabeled[1] );
			printf( "\n\tnegative gestures: %d", nGesturesLabeled[0] );
			printf( "\n\tpositive frames' score mean, std.dev: %f, %f", Stats[0].mean, Stats[0].stddev );
			printf( "\n\tnegative frames' score mean, std.dev: %f, %f", Stats[1].mean, Stats[1].stddev );
		}

		return S_OK;

        // run strong classifier on training data for verification
        UINT nNumGesturesDetected = 0;
        UINT nNumWrongGesturesDetected = 0;
        INT nNumPositiveExamples = 0;
        INT nNumTrueDetections = 0;
        INT nNumFalseDetections = 0;
        // We need to make sure the indices match up, so add two in the beginning and two at the end
        vector<BOOL> fFilteredClassificationResults;
        fFilteredClassificationResults.push_back( FALSE );
        fFilteredClassificationResults.push_back( FALSE );

        const UINT nNumExamples = (UINT)( m_LabeledExamples.m_pExamples.size() );
        for ( UINT i = 2; i < nNumExamples - 2; i++ )
        {
            // Get the example from the ground truth training set
            GESTURE_SKELETON_TYPE* pSkeletonData = m_LabeledExamples.m_pExamples[ i ];
            UINT64 uTimeStamp = m_LabeledExamples.m_uTimeStamps[ i ];

            // Update the classifier data. Time stamps from Xed files are different to timestamps from runtime NUI_SKELETON_FRAMES
            LARGE_INTEGER liTimeStamp;
            liTimeStamp.QuadPart = uTimeStamp;
            Update( 0, pSkeletonData, liTimeStamp, vUpVector );

            // Run the strong classifier on player 0
            Results results;
            if ( m_StrongClassifier.Detect( 0, m_ClassifierData, &results, FALSE ) )
            {
                // Testing for per gesture results
                if ( results.m_bFirstFrameDetected )
                {
                    // We cannot just detect if the first frame of the gesture correlate to the a
                    // ground truth labeled example, since the detection might be a few frames off
                    // but still detected the gesture correctly. We therefore allow for 2 frames
                    // to either side when testing the detection results against the ground truth
                    if ( m_LabeledExamples.m_iLabels[ i ] == g_iClassificationLabelCorrect ||
                        m_LabeledExamples.m_iLabels[ i - 1 ] == g_iClassificationLabelCorrect ||
                        m_LabeledExamples.m_iLabels[ i + 1 ] == g_iClassificationLabelCorrect ||
                        m_LabeledExamples.m_iLabels[ i - 2 ] == g_iClassificationLabelCorrect ||
                        m_LabeledExamples.m_iLabels[ i + 2 ] == g_iClassificationLabelCorrect )
                    {
                        if ( ( m_LabeledExamples.m_iLabels[ i - 1 ] == g_iClassificationLabelCorrect && m_LabeledExamples.m_iLabels[ i - 2 ] != g_iClassificationLabelCorrect ) ||
                            ( m_LabeledExamples.m_iLabels[ i ] == g_iClassificationLabelCorrect && m_LabeledExamples.m_iLabels[ i - 1 ] != g_iClassificationLabelCorrect ) ||
                            ( m_LabeledExamples.m_iLabels[ i + 1] == g_iClassificationLabelCorrect && m_LabeledExamples.m_iLabels[ i ] != g_iClassificationLabelCorrect ) ||
                            ( m_LabeledExamples.m_iLabels[ i + 2 ] == g_iClassificationLabelCorrect && m_LabeledExamples.m_iLabels[ i + 1 ] != g_iClassificationLabelCorrect ) )
                        {
                            nNumGesturesDetected++;
                        }
                        else
                        {
                            nNumWrongGesturesDetected++;
                        }
                    }
                    else
                    {
                        nNumWrongGesturesDetected++;
                    }
                }
            }

            fFilteredClassificationResults.push_back( results.m_bDetected );

            // Testing for per frame results
            if ( m_StrongClassifier.Detect( 0, m_ClassifierData, &results, FALSE ) )
            {
                if ( m_LabeledExamples.m_iLabels[ i ] == g_iClassificationLabelCorrect )
                {
                    nNumTrueDetections++;
                }
                else
                {
                    nNumFalseDetections++;
                }
            }

            if ( m_LabeledExamples.m_iLabels[ i ] == g_iClassificationLabelCorrect )
            {
                nNumPositiveExamples++;
            }
        }

        fFilteredClassificationResults.push_back( FALSE );
        fFilteredClassificationResults.push_back( FALSE );

        INT nNumTruePositiveGesturesGT = 0;
        INT nNumTruePositiveGesturesObserved = 0;
        INT nNumFalsePositiveGesturesObserved = 0;

        for ( UINT i = 1; i < nNumExamples; i++ )
        {      
            // Find a true positive gesture in GT. Count only the start of each sequence
            // of GT frames that make up the gesture
            if ( m_LabeledExamples.m_iLabels[ i ] == g_iClassificationLabelCorrect &&
                m_LabeledExamples.m_iLabels[ i - 1 ] != g_iClassificationLabelCorrect )
            {
                nNumTruePositiveGesturesGT++;
            }
            else
            {
                continue;
            }

            // Find a true positive gesture in the observed data during the GT frames.
            for ( UINT j = i; j < nNumExamples; j++)
            {
                if ( fFilteredClassificationResults[ j ] )
                {
                    nNumTruePositiveGesturesObserved++;
                    break;
                }

                // Check for the end of the gesture in GT
                if ( m_LabeledExamples.m_iLabels[ j ] != g_iClassificationLabelCorrect &&
                    m_LabeledExamples.m_iLabels[ j - 1 ] == g_iClassificationLabelCorrect )
                {
                    break;
                }
            }

            // Find all true positive gesture in the observed data during the GT frames.
            INT nNumObserved = 0;
            for ( UINT j = i; j < nNumExamples; j++ )
            {
                if ( fFilteredClassificationResults[ j ] &&
                    !fFilteredClassificationResults[ j - 1 ] )
                {
                    nNumObserved++;
                }

                // Check for the end of the gesture in GT
                if ( m_LabeledExamples.m_iLabels[ j ] != g_iClassificationLabelCorrect &&
                    m_LabeledExamples.m_iLabels[ j - 1 ] == g_iClassificationLabelCorrect )
                {
                    break;
                }
            }

            nNumFalsePositiveGesturesObserved += max( 0, nNumObserved - 1 );    // we only allow 1 detection during GT gesture
        }

        // Now we try to find false positives from observed data with no GT
        for ( UINT i = 1; i < nNumExamples; i++ )
        {
            // Find an observed detection
            if ( fFilteredClassificationResults[ i ] &&
                !fFilteredClassificationResults[i - 1] )
            {
            }
            else
            {
                continue;
            }

            // Find a true positive gesture in the GT data during the observed frames.
            INT numGT = 0;
            for ( UINT j = i; j < nNumExamples; j++ )
            {
                if ( m_LabeledExamples.m_iLabels[ j ] == g_iClassificationLabelCorrect )
                {
                    numGT++;
                    break;
                }

                // Check for the end of the gesture in observed data
                if ( !fFilteredClassificationResults[ j ] &&
                    fFilteredClassificationResults[ j - 1 ] )
                {
                    break;
                }
            }

            nNumFalsePositiveGesturesObserved += max( 0, 1 - numGT );
        }

        FLOAT fTruePositives = 0.0f;
        FLOAT fFalsePositives = 0.0f;

        if ( nNumTruePositiveGesturesGT < 1 &&
            nNumTruePositiveGesturesObserved < 1 )
        {   // We have no GT or observed gestures, so no error
            fTruePositives = 100.0f;
        }
        else if ( nNumTruePositiveGesturesGT < 1 &&
            nNumTruePositiveGesturesObserved >= 1 )
        {   // We have no GT gestures, but did find observed gestures, so report no error
            // since we're calculating false negatives here, not false positives
            fTruePositives = 100.0f;
        }
        else
        {
            fTruePositives = 100.0f * nNumTruePositiveGesturesObserved / (FLOAT)nNumTruePositiveGesturesGT;
        }

        if ( nNumTruePositiveGesturesGT < 1 &&
            nNumFalsePositiveGesturesObserved < 1 )
        {   // We have no GT or observed gestures, so no error
            fFalsePositives = 0.0f;
        }
        else if ( nNumTruePositiveGesturesGT < 1 &&
            nNumFalsePositiveGesturesObserved >= 1 )
        {   // We have no GT gestures, but did find observed gestures, so return highest error
            fFalsePositives = 100.0f;
        }
        else
        {
            fFalsePositives = 100.0f * nNumFalsePositiveGesturesObserved / (FLOAT)nNumTruePositiveGesturesGT;
        }

        // Output accuracy in true positives and false positives for per frame results
        FLOAT fAccuracy;
        if ( nNumPositiveExamples == 0 )
        {
            fAccuracy = 100.0f;
        }
        else
        {
            fAccuracy = nNumTrueDetections * 100.0f / nNumPositiveExamples;
        }
        FLOAT fErrorFalsePositives = (FLOAT)nNumFalseDetections * 100.0f / ( nNumExamples - 4 - nNumPositiveExamples );
        printf( "\n    Raw Per Frame Results:" );
        printf( "\n\t%% Accuracy True Positives: %f %% (%d/%d)", fAccuracy, nNumTrueDetections, nNumPositiveExamples );
        printf( "\n\t%% Error False Positives: %f %% (%d/%d)", fErrorFalsePositives, nNumFalseDetections, nNumExamples - 4 - nNumPositiveExamples );

        // Output accuracy in true positives and false positives for filtered per gesture results
        printf( "\n    Filtered Per Gesture Results:" );
        printf( "\n\t%% Accuracy True Positives: %f %% (%d/%d)", fTruePositives, nNumTruePositiveGesturesObserved, nNumTruePositiveGesturesGT );
        printf( "\n\t%% Error False Positives: %f %% (%d/%d)", fFalsePositives, nNumFalsePositiveGesturesObserved, nNumTruePositiveGesturesGT );


        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: Test
    // Desc: Test the accuracy of the training algorithm. Used to find the best filtering
    //       parameters
    //--------------------------------------------------------------------------------------

    VOID GestureDetectorTrainer::Test( FLOAT* pTruePositives, FLOAT* pFalsePositives, vector<FLOAT>& fRawClassificationResults )
    {
        // Clear all the data
        ClassifierData::Initialize();
        m_StrongClassifier.Reset( 0 );
        m_uPreviousTimeStamp = 0;

        // run strong classifier on training data for verification
        INT nNumTruePositiveGesturesGT = 0;
        INT nNumTruePositiveGesturesObserved = 0;
        INT nNumFalsePositiveGesturesObserved = 0;

        const UINT nNumExamples = (UINT)( m_LabeledExamples.m_pExamples.size() );

        // Get all filtered data
        vector<BOOL> fFilteredClassificationResults;
        for ( UINT i = 0; i < nNumExamples; i++ )
        {
            // Filter the cached raw detection results with the current detection thresholds
            Results results;
            m_StrongClassifier.FilterDetectionResults( 0, fRawClassificationResults[ i ], &results );
            fFilteredClassificationResults.push_back( results.m_bDetected );
        }

        for ( UINT i = 1; i < nNumExamples; i++ )
        {      
            // Find a true positive gesture in GT. Count only the start of each sequence
            // of GT frames that make up the gesture
            if ( m_LabeledExamples.m_iLabels[ i ] == g_iClassificationLabelCorrect &&
                m_LabeledExamples.m_iLabels[ i - 1 ] != g_iClassificationLabelCorrect )
            {
                nNumTruePositiveGesturesGT++;
            }
            else
            {
                continue;
            }

            // Find a true positive gesture in the observed data during the GT frames.
            for ( UINT j = i; j < nNumExamples; j++)
            {
                if ( fFilteredClassificationResults[ j ] )
                {
                    nNumTruePositiveGesturesObserved++;
                    break;
                }

                // Check for the end of the gesture in GT
                if ( m_LabeledExamples.m_iLabels[ j ] != g_iClassificationLabelCorrect &&
                    m_LabeledExamples.m_iLabels[ j - 1 ] == g_iClassificationLabelCorrect )
                {
                    break;
                }
            }

            // Find all true positive gesture in the observed data during the GT frames.
            INT nNumObserved = 0;
            for ( UINT j = i; j < nNumExamples; j++ )
            {
                if ( fFilteredClassificationResults[ j ] &&
                    !fFilteredClassificationResults[ j - 1 ] )
                {
                    nNumObserved++;
                }

                // Check for the end of the gesture in GT
                if ( m_LabeledExamples.m_iLabels[ j ] != g_iClassificationLabelCorrect &&
                    m_LabeledExamples.m_iLabels[ j - 1 ] == g_iClassificationLabelCorrect )
                {
                    break;
                }
            }

            nNumFalsePositiveGesturesObserved += max( 0, nNumObserved - 1 );    // we only allow 1 detection during GT gesture
        }

        // Now we try to find false positives from observed data with no GT
        for ( UINT i = 1; i < nNumExamples; i++ )
        {
            // Find an observed detection
            if ( fFilteredClassificationResults[ i ] &&
                !fFilteredClassificationResults[i - 1] )
            {
            }
            else
            {
                continue;
            }

            // Find a true positive gesture in the GT data during the observed frames.
            INT numGT = 0;
            for ( UINT j = i; j < nNumExamples; j++ )
            {
                if ( m_LabeledExamples.m_iLabels[ j ] == g_iClassificationLabelCorrect )
                {
                    numGT++;
                    break;
                }

                // Check for the end of the gesture in observed data
                if ( !fFilteredClassificationResults[ j ] &&
                    fFilteredClassificationResults[ j - 1 ] )
                {
                    break;
                }
            }

            nNumFalsePositiveGesturesObserved += max( 0, 1 - numGT );
        }


        if ( nNumTruePositiveGesturesGT < 1 &&
            nNumTruePositiveGesturesObserved < 1 )
        {   // We have no GT or observed gestures, so no error
            *pTruePositives = 100.0f;
        }
        else if ( nNumTruePositiveGesturesGT < 1 &&
            nNumTruePositiveGesturesObserved >= 1 )
        {   // We have no GT gestures, but did find observed gestures, so report no error
            // since we're calculating false negatives here, not false positives
            *pTruePositives = 100.0f;
        }
        else
        {
            *pTruePositives = 100.0f * nNumTruePositiveGesturesObserved / (FLOAT)nNumTruePositiveGesturesGT;
        }

        if ( nNumTruePositiveGesturesGT < 1 &&
            nNumFalsePositiveGesturesObserved < 1 )
        {   // We have no GT or observed gestures, so no error
            *pFalsePositives = 0.0f;
        }
        else if ( nNumTruePositiveGesturesGT < 1 &&
            nNumFalsePositiveGesturesObserved >= 1 )
        {   // We have no GT gestures, but did find observed gestures, so return highest error
            *pFalsePositives = 100.0f;
        }
        else
        {
            *pFalsePositives = 100.0f * nNumFalsePositiveGesturesObserved / (FLOAT)nNumTruePositiveGesturesGT;
        }
    }


    //--------------------------------------------------------------------------------------
    // Private structure used to find the most optimimum filter parameters for per gesture
    // detection.
    //--------------------------------------------------------------------------------------

    struct DetectionParameters
    {
        FLOAT   m_fThreshold;
        UINT    m_nNumFramesToFilter;
        FLOAT   m_fError;
        INT     x;  
        INT     y;

        inline DetectionParameters& operator = ( const DetectionParameters& rhs)
        {
            m_fThreshold = rhs.m_fThreshold;
            m_nNumFramesToFilter = rhs.m_nNumFramesToFilter;
            m_fError = rhs.m_fError;
            x = rhs.x;
            y = rhs.y;
            return *this;
        }

        inline bool operator < ( const DetectionParameters& rhs )
        {
            return ( m_fError < rhs.m_fError );
        }
    };


    //--------------------------------------------------------------------------------------
    // Name: CalcSumOfNeighbours
    // Desc: Calculates the sum of all values in a 3x3 kernal
    //--------------------------------------------------------------------------------------

    FLOAT CalcSumOfNeighbours( const INT x, const INT y, vector<vector<DetectionParameters>>& fValues )
    {
        FLOAT fSum = 0.0f;

        for ( INT iNeighborY = y - 1; iNeighborY <= y + 1; iNeighborY++ )
        {
            for ( INT iNeighborX = x - 1; iNeighborX <= x + 1; iNeighborX++ )
            {
                fSum += fValues[ iNeighborY ][ iNeighborX ].m_fError;
            }
        }

        return fSum;
    }

    //--------------------------------------------------------------------------------------
    // Name: OptimizeDetectionParameters
    // Desc: Since the classifier results are per frame and not per gesture, we need to
    //       apply a filter on the raw per frame results. This is in the form of a sum of
    //       a sliding window with a threshold. We therefore have two parameters to
    //       find that will minimize both the error in true positives and false postives.
    //--------------------------------------------------------------------------------------

    VOID GestureDetectorTrainer::OptimizeDetectionParameters( const FLOAT fWeightOfFalsePositivesWhenFiltering )
    {
        printf( "\n\t-Generate matrix of test results using different detection parameters..." );
        XMVECTOR vUpVector = XMVectorSet( 0.0f, 1.0f, 0.0f, 0.0f );
        {
            // Cache raw results from classification
            vector<FLOAT> fRawClassificationResults;

            // Clear all the data
            ClassifierData::Initialize();
            m_StrongClassifier.Reset( 0 );
            m_uPreviousTimeStamp = 0;

            // Run strong classifier
            const UINT nNumExamples = (UINT)( m_LabeledExamples.m_pExamples.size() );
            for ( UINT i = 0; i < nNumExamples; i++ )
            {
                // Get the example from the ground truth training set
                GESTURE_SKELETON_TYPE* pSkeletonData = m_LabeledExamples.m_pExamples[ i ];
                UINT64 uTimeStamp = m_LabeledExamples.m_uTimeStamps[ i ];

                // Update the classifier data. Time stamps from Xed files are different to timestamps from runtime NUI_SKELETON_FRAMES
                LARGE_INTEGER liTimeStamp;
                liTimeStamp.QuadPart = uTimeStamp;
                Update( 0, pSkeletonData, liTimeStamp, vUpVector );

                // Run the strong classifier on player 0, without any filtering
                Results results;
                m_StrongClassifier.Detect( 0, m_ClassifierData, &results, FALSE );

                // Store the raw classification result
                fRawClassificationResults.push_back( results.m_fConfidence );
            }

            vector<vector<DetectionParameters>> values;
            vector<vector<DetectionParameters>> summedValues;

            // Fill in a 2d matrix of possible parameters
            UINT i = 0;
            for ( FLOAT fThreshold = fDetectionParamsMinThreshold; fThreshold <= fDetectionParamsMaxThreshold; fThreshold += fDetectionParamsThresholdStep )
            {
                values.resize( i + 1 );

                for ( UINT nNumFrames = nDetectionParamsMinNumFrames; nNumFrames <= nDetectionParamsMaxNumFrames; nNumFrames++ )        
                {
                    DetectionParameters parameters;
                    parameters.m_fThreshold = fThreshold;
                    parameters.m_nNumFramesToFilter = nNumFrames;
                    parameters.x = nNumFrames - nDetectionParamsMinNumFrames;
                    parameters.y = i;

                    SetDetectionThreshold( fThreshold );
                    SetNumFramesToFilter( nNumFrames );

                    FLOAT fTruePositiveResults;
                    FLOAT fFalsePositiveResults;
                    Test( &fTruePositiveResults, &fFalsePositiveResults, fRawClassificationResults );

                    // Combine the results of false negatives and false positives with a weighted sum. We can bias towards optimizing
                    // for fewer false positives or fewer false negatives.
                    parameters.m_fError = ( fabsf( 100.0f - fTruePositiveResults ) * ( 1.0f - fWeightOfFalsePositivesWhenFiltering ) ) +
                        ( fFalsePositiveResults * fWeightOfFalsePositivesWhenFiltering );

                    values[ i ].push_back( parameters );
                }
                i++;
            }

            INT nNumY = (INT)values.size();
            INT nNumX = (INT)values[ 0 ].size();

            // Fill it with the sum of the 8 surrounding neighbours and itself for each value in the 2d array
            summedValues.resize( nNumY );
            for ( INT y = 0; y < nNumY; y++ )
            {
                for ( INT x = 0; x < nNumX; x++ )
                {
                    DetectionParameters params = values[ y ][ x ];

                    // Just use FLT_MAX around the edge pixels, since these will be invalid anyway
                    if ( y == 0 || y == ( nNumY - 1 ) ||
                        x == 0 || x == ( nNumX - 1 ) )
                    {
                        params.m_fError = FLT_MAX / 9.0f;
                    }
                    else
                    {
                        params.m_fError = CalcSumOfNeighbours( x, y, values );
                    }
                    summedValues[ y ].push_back( params );
                }
            }

            nNumY = (INT)summedValues.size();
            nNumX = (INT)summedValues[ 0 ].size();
            DetectionParameters bestParams = { 0, 0, FLT_MAX };
            vector<DetectionParameters> medianXFilterMinValues;
            vector<DetectionParameters> medianYFilterMinValues;

            // Find the minimum summed error values
            for ( INT y = 0; y < nNumY; y++ )
            {
                for ( INT x = 0; x < nNumX; x++ )
                {
                    if ( summedValues[ y ][ x ] < bestParams )
                    {
                        bestParams = summedValues[ y ][ x ];
                    }
                }
            }

            // Find all the values equal to this minimum value and add them to an array for finding the median value
            // Traverse horizontally
            for ( INT y = 0; y < nNumY; y++ )
            {
                for ( INT x = 0; x < nNumX; x++ )
                {
                    if ( fabsf( summedValues[ y ][ x ].m_fError - bestParams.m_fError ) < FLT_EPSILON )
                    {
                        medianXFilterMinValues.push_back( summedValues[ y ][ x ] );
                    }
                }
            }

            // Find all the values equal to this minimum value and add them to an array for finding the median value
            // Traverse vertically
            for ( INT x = 0; x < nNumX; x++ )
            {
                for ( INT y = 0; y < nNumY; y++ )
                {
                    if ( fabsf( summedValues[ y ][ x ].m_fError - bestParams.m_fError ) < FLT_EPSILON )
                    {
                        medianYFilterMinValues.push_back( summedValues[ y ][ x ] );
                    }
                }
            }

            // Find the median value in each array
            sort( medianXFilterMinValues.begin(), medianXFilterMinValues.end() );
            sort( medianYFilterMinValues.begin(), medianYFilterMinValues.end() );

            INT iMedian = (INT)(medianXFilterMinValues.size() - 1 ) / 2;
            DetectionParameters medianX = medianXFilterMinValues[ iMedian ];
            DetectionParameters medianY = medianYFilterMinValues[ iMedian ];

            // Find out which one is best
            FLOAT fSmallestXError = FLT_MAX;
            for ( INT i = max( iMedian - 2, 0 ); i < min( iMedian + 2, (INT)medianXFilterMinValues.size() ); i++ )
            {
                FLOAT fSum = CalcSumOfNeighbours( medianXFilterMinValues[ i ].x, medianXFilterMinValues[ i ].y, summedValues );
                if ( fSum < fSmallestXError )
                {
                    fSmallestXError = fSum;
                    medianX = medianXFilterMinValues[ i ];
                }
            }

            FLOAT fSmallestYError = FLT_MAX;
            for ( INT i = max( iMedian - 2, 0 ); i < min( iMedian + 2, (INT)medianYFilterMinValues.size() ); i++ )
            {
                FLOAT fSum = CalcSumOfNeighbours( medianYFilterMinValues[ i ].x, medianYFilterMinValues[ i ].y, summedValues );
                if ( fSum < fSmallestYError )
                {
                    fSmallestYError = fSum;
                    medianY = medianYFilterMinValues[ i ];
                }
            }

            // The best one is the one with the smallest summed error value
            bestParams = ( fSmallestXError > fSmallestYError ) ? medianY : medianX;

            SetDetectionThreshold( bestParams.m_fThreshold );
            SetNumFramesToFilter( bestParams.m_nNumFramesToFilter );
        }
        {
            printf( "\n\t-Generating stats for confidence values (for scoring)..." );

            // now we have this threshold, we need to establish a range to use for scoring
            // we use the incorrect-labeled examples to find a minimum and the correct labeled examples to find a maximum
            // so we need to find the smallest response for ground truth (the range min)
            // and the largest response for a ground truth (the range max)
            FLOAT fRangeMin[StrongClassifier::eLabel_Count] = { FLT_MAX, FLT_MAX };
            FLOAT fRangeMax[StrongClassifier::eLabel_Count] = { -FLT_MAX, -FLT_MAX };
            FLOAT fMean[StrongClassifier::eLabel_Count] = { 0.0f, 0.0f };
            FLOAT fStdDev[StrongClassifier::eLabel_Count] = { 0.0f, 0.0f };
            FLOAT fEnergyMean = 0.0f;
            FLOAT fEnergyStdDev = 0.0f;

            // Cache raw results from classification
            vector<FLOAT> fCorrectResults;
            vector<FLOAT> fIncorrectResults;
            vector<FLOAT> fEnergyResults;

			UINT	nNotLabeledExamples = 0;

            const UINT nNumExamples = (UINT)( m_LabeledExamples.m_pExamples.size() );
            for ( UINT i = 0; i < nNumExamples; i++ )
            {
                // Get the example from the ground truth training set
                GESTURE_SKELETON_TYPE* pSkeletonData = m_LabeledExamples.m_pExamples[ i ];
                UINT64 uTimeStamp = m_LabeledExamples.m_uTimeStamps[ i ];

                // Update the classifier data. Time stamps from Xed files are different to timestamps from runtime NUI_SKELETON_FRAMES
                LARGE_INTEGER liTimeStamp;
                liTimeStamp.QuadPart = uTimeStamp;
                Update( 0, pSkeletonData, liTimeStamp, vUpVector );

                // Run the strong classifier on player 0 unfiltered
                Results results;
                m_StrongClassifier.Detect( 0, m_ClassifierData, &results, FALSE );

                if( m_LabeledExamples.m_iLabels[i] == g_iClassificationLabelCorrect )
                {
                    fRangeMax[StrongClassifier::eLabel_Correct] = max( fRangeMax[StrongClassifier::eLabel_Correct], results.m_fConfidence );
                    fRangeMin[StrongClassifier::eLabel_Correct] = min( fRangeMin[StrongClassifier::eLabel_Correct], results.m_fConfidence );
                    fMean[StrongClassifier::eLabel_Correct] += results.m_fConfidence;

                    // Store the raw classification result
                    fCorrectResults.push_back( results.m_fConfidence );

                    // only store energy levels for 'correct' labels
                    FLOAT fEnergy = GetEnergyLevel( 0 );
                    fEnergyMean += fEnergy;
                    fEnergyResults.push_back( fEnergy );
                }
                else
				if( m_LabeledExamples.m_iLabels[i] == g_iClassificationLabelIncorrect )
                {
                    fRangeMax[StrongClassifier::eLabel_Incorrect] = max( fRangeMax[StrongClassifier::eLabel_Incorrect], results.m_fConfidence );
                    fRangeMin[StrongClassifier::eLabel_Incorrect] = min( fRangeMin[StrongClassifier::eLabel_Incorrect], results.m_fConfidence );
                    fMean[StrongClassifier::eLabel_Incorrect] += results.m_fConfidence;

                    // Store the raw classification result
                    fIncorrectResults.push_back( results.m_fConfidence );
                } else
				{
					nNotLabeledExamples++;
				}
            }

            if( fCorrectResults.size() > 0 )
            {
                fMean[StrongClassifier::eLabel_Correct] /= (FLOAT)fCorrectResults.size();
            }
            if( fIncorrectResults.size() > 0 )
            {
                fMean[StrongClassifier::eLabel_Incorrect] /= (FLOAT)fIncorrectResults.size();
            }
            if( fEnergyResults.size() > 0 )
            {
                fEnergyMean /= (FLOAT)fEnergyResults.size();
            }
            
            FLOAT fSumOfDifferences[StrongClassifier::eLabel_Count] = { 0.0f, 0.0f };
            const UINT nNumResults[StrongClassifier::eLabel_Count] = { (UINT)( fCorrectResults.size() ), (UINT)( fIncorrectResults.size() ) };
            for ( UINT i = 0; i < nNumResults[StrongClassifier::eLabel_Correct]; i++ )
            {
                float fDiff = ( fMean[StrongClassifier::eLabel_Correct] - fCorrectResults[i] );
                fSumOfDifferences[StrongClassifier::eLabel_Correct] += ( fDiff * fDiff );
            }

            for ( UINT i = 0; i < nNumResults[StrongClassifier::eLabel_Incorrect]; i++ )
            {
                float fDiff = ( fMean[StrongClassifier::eLabel_Incorrect] - fIncorrectResults[i] );
                fSumOfDifferences[StrongClassifier::eLabel_Incorrect] += ( fDiff * fDiff );
            }

            FLOAT fSumOfEnergyDifferences = 0.0f;
            const UINT nNumEnergyResults = (UINT)( fEnergyResults.size() );
            for ( UINT i = 0; i < nNumEnergyResults; i++ )
            {
                float fDiff = ( fEnergyMean - fEnergyResults[i] );
                fSumOfEnergyDifferences += ( fDiff * fDiff );
            }

            if( fCorrectResults.size() > 0 )
            {
                fStdDev[StrongClassifier::eLabel_Correct] = sqrtf( fSumOfDifferences[StrongClassifier::eLabel_Correct] / (FLOAT)nNumResults[StrongClassifier::eLabel_Correct] );
            }
            if( fIncorrectResults.size() > 0 )
            {
                fStdDev[StrongClassifier::eLabel_Incorrect] = sqrtf( fSumOfDifferences[StrongClassifier::eLabel_Incorrect] / (FLOAT)nNumResults[StrongClassifier::eLabel_Incorrect] );
            }
            if( fEnergyResults.size() > 0 )
            {
                fEnergyStdDev = sqrtf( fSumOfEnergyDifferences / (FLOAT)nNumEnergyResults );
            }

            // output stats
            printf( "\n\tExamples labeled as correct: %d", fCorrectResults.size() );
			if( fCorrectResults.size() > 0 )
			{
				printf( "\n\tMin = %3.5f, Max = %3.5f, Mean = %3.5f, StdDev = %3.5f", fRangeMin[StrongClassifier::eLabel_Correct], fRangeMax[StrongClassifier::eLabel_Correct], fMean[StrongClassifier::eLabel_Correct], fStdDev[StrongClassifier::eLabel_Correct] );
			}
            printf( "\n\tExamples labeled as incorrect: %d", fIncorrectResults.size() );
			if( fIncorrectResults.size() > 0 )
			{
				printf( "\n\tMin = %3.5f, Max = %3.5f, Mean = %3.5f, StdDev = %3.5f", fRangeMin[StrongClassifier::eLabel_Incorrect], fRangeMax[StrongClassifier::eLabel_Incorrect], fMean[StrongClassifier::eLabel_Incorrect], fStdDev[StrongClassifier::eLabel_Incorrect] );
			}
            printf( "\n\tEnergy:" );
            printf( "\n\tMean = %3.5f, StdDev = %3.5f", fEnergyMean, fEnergyStdDev );

			printf( "\n\tExamples not labeled: %d", nNotLabeledExamples );

            SetRangeMax( fRangeMax[StrongClassifier::eLabel_Correct], StrongClassifier::eLabel_Correct );
            SetRangeMax( fRangeMax[StrongClassifier::eLabel_Incorrect], StrongClassifier::eLabel_Incorrect );
            SetRangeMin( fRangeMin[StrongClassifier::eLabel_Correct], StrongClassifier::eLabel_Correct );
            SetRangeMin( fRangeMin[StrongClassifier::eLabel_Incorrect], StrongClassifier::eLabel_Incorrect );
            SetMean( fMean[StrongClassifier::eLabel_Correct], StrongClassifier::eLabel_Correct );
            SetMean( fMean[StrongClassifier::eLabel_Incorrect], StrongClassifier::eLabel_Incorrect );
            SetStdDev( fStdDev[StrongClassifier::eLabel_Correct], StrongClassifier::eLabel_Correct );
            SetStdDev( fStdDev[StrongClassifier::eLabel_Incorrect], StrongClassifier::eLabel_Incorrect );

            SetEnergyMean( fEnergyMean );
            SetEnergyStdDev( fEnergyStdDev );
        }
    }

    //--------------------------------------------------------------------------------------
    // Name: GetNumPositiveExamples
    // Desc: Returns the number of positive labeled examples
    //--------------------------------------------------------------------------------------

    UINT GestureDetectorTrainer::GetNumPositiveExamples()
    {
        UINT nNumPositiveExamples = 0;

        for ( UINT i = 0; i < m_LabeledExamples.m_iLabels.size(); i++ )
        {
            if ( m_LabeledExamples.m_iLabels[ i ] == g_iClassificationLabelCorrect )
            {
                nNumPositiveExamples++;
            }
        }

        return nNumPositiveExamples;
    }


    //--------------------------------------------------------------------------------------
    // Name: SaveLabeledExamples
    // Desc: Save the labeled examples to a binary file
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::SaveLabeledExamples( const CHAR* szFileName )
    {
        FILE* pFile = NULL;
        fopen_s( &pFile, szFileName, "wb" );
        if ( !pFile )
        {
            printf( "\nFailed to open %s for saving labeled examples...\n", szFileName );
            return E_FAIL;
        }

        // Write a text identifier
        fwrite( getLabeledExampleFileID(), getLabeledExampleFileIDLen(), 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

        // Write current version number
        fwrite( &g_fCurrentVersion, sizeof( g_fCurrentVersion ), 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

        fwrite( &m_nTotalNumGestures, sizeof( m_nTotalNumGestures ), 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

        fwrite( &m_nNumTrainingGestures, sizeof( m_nNumTrainingGestures), 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

        UINT nNumExamples = (UINT)( m_LabeledExamples.m_pExamples.size() );
        fwrite( &nNumExamples, sizeof( UINT ), 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

        INT8 iLabel;
        UINT64 uTimeStamp;

        for ( UINT i = 0; i < nNumExamples; i++ )
        {
            fwrite( m_LabeledExamples.m_pExamples[ i ], sizeof( GESTURE_SKELETON_TYPE ), 1, pFile );
            RETURN_ON_FILE_ERROR( pFile );

            iLabel = m_LabeledExamples.m_iLabels[ i ];
            fwrite( &iLabel, sizeof( INT8 ), 1, pFile );
            RETURN_ON_FILE_ERROR( pFile );

            uTimeStamp = m_LabeledExamples.m_uTimeStamps[ i ];
            fwrite( &uTimeStamp, sizeof( UINT64 ), 1, pFile );
            RETURN_ON_FILE_ERROR( pFile );
        }

        fclose( pFile );

        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: LoadLabeledExamples
    // Desc: Load labeled examples from binary file
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::LoadLabeledExamples( const CHAR* szFileName )
    {
        FILE* pFile = NULL;
        fopen_s( &pFile, szFileName, "rb" );
        if ( !pFile )
        {
            printf( "\nERROR: Failed to open %s for loading labeled examples...\n", szFileName );
            return E_FAIL;
        }

//        Reset();

        // Check that this is indeed a gesture file
        CHAR szFileID[ 255 ];
        fread( szFileID, getLabeledExampleFileIDLen(), 1, pFile );
        RETURN_ON_FILE_ERROR( pFile );

        // Check the file header
        if ( !strcmp( szFileID, getLabeledExampleFileID() ) )
        {
            // Read the version number. This is only added for backwards compatibility when we need to change file formats in the future
            FLOAT fVersion;
            fread( &fVersion, sizeof( fVersion ), 1, pFile );
            RETURN_ON_FILE_ERROR( pFile );
#ifdef _XBOX
            fVersion = ByteSwap32Bit( fVersion );
#endif

            // Check that the file version is the same as the current version
            if ( fabsf( fVersion - g_fCurrentVersion ) > 1.0f )
            {
                fclose( pFile );
                printf( "\nError: File version %f != current version %f\n", fVersion, g_fCurrentVersion );
                return E_FAIL;
            }

            fread( &m_nTotalNumGestures, sizeof( m_nTotalNumGestures ), 1, pFile );
            RETURN_ON_FILE_ERROR( pFile );

            fread( &m_nNumTrainingGestures, sizeof( m_nNumTrainingGestures), 1, pFile );
            RETURN_ON_FILE_ERROR( pFile );

            UINT nNumExamples;
            fread( &nNumExamples, sizeof( UINT ), 1, pFile );
            RETURN_ON_FILE_ERROR( pFile );

			if( nNumExamples > 0 )
			{
				printf("\nLoading %d labeled examples from %s...", nNumExamples, szFileName);
				for ( UINT i = 0; i < nNumExamples; i++ )
				{
					GESTURE_SKELETON_TYPE* pSkeletonData = (GESTURE_SKELETON_TYPE*)_aligned_malloc( sizeof( GESTURE_SKELETON_TYPE ), 16 );
					RETURN_ON_NULL( pSkeletonData );

					INT8 iLabel;
					UINT64 uTimeStamp;

					fread( pSkeletonData, sizeof( GESTURE_SKELETON_TYPE ), 1, pFile );
					RETURN_ON_FILE_ERROR( pFile );

					fread( &iLabel, sizeof( INT8 ), 1, pFile );
					RETURN_ON_FILE_ERROR( pFile );

					fread( &uTimeStamp, sizeof( UINT64 ), 1, pFile );
					RETURN_ON_FILE_ERROR( pFile );

					m_LabeledExamples.m_pExamples.push_back( pSkeletonData );
					m_LabeledExamples.m_iLabels.push_back( iLabel );
					m_LabeledExamples.m_uTimeStamps.push_back( uTimeStamp );
				}
			}
			else
			{
				printf("\nWARNING: No labeled examples in the file %s.", szFileName);
			}
        }
		else
		{
            printf( "\nError: File type %s != required type %s in file %s\n", szFileID, getLabeledExampleFileID(), szFileName );
			fclose( pFile );
            return E_FAIL;
		}

        fclose( pFile );

        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: TrainWeakClassifiers
    // Desc: In order to optimize training time and use less memory at training time, we
    //       do the training in two passes. The first pass finds all the interesting
    //       weak classifiers that contribute to each future, and the second pass uses
    //       these weak classifiers to do the final training. More accurate results can 
    //       be found when using all the weak classifiers from all features in just one
    //       sinlge pass, but in our experiments memory usage easily went up to more than
    //       14 GBytes RAM and training times were in the order of hours, versus using only
    //       about 3 GBytes RAM and a few minutes of training.
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::TrainWeakClassifiers()
    {
        DebugOutput output;

        // First pass finds all the interesting weak classifiers that contribute per feature
        GestureDetectorTrainer trainers[ ClassifierData::NUM_FEATURES ];
        for ( UINT i = 0; i < ClassifierData::NUM_FEATURES; i++ )
        {
            printf( "\n\t%d/%d - Feature: %s ", i + 1, ClassifierData::NUM_FEATURES, output.Print( (ClassifierData::EType)i ) );
            trainers[ i ].m_nNumWeakClassifiersAtRuntime = m_nNumWeakClassifiersAtRuntime;
			trainers[ i ].m_bUseSkeleton = m_bUseSkeleton;
			trainers[ i ].m_bUseOpticalFlow = m_bUseOpticalFlow;
			trainers[ i ].m_nFramesToSkip = m_nFramesToSkip;
			RETURN_ON_FAIL( trainers[ i ].TrainWeakClassifiers( (ClassifierData::EType)i, m_LabeledExamples, m_fErrorThreshold ) );
            printf( "(%d)", trainers[ i ].GetNumWeakClassifiers() );
        }

        // Now add all these classifiers into one list that can be used in the seconds pass
        CombineWeakClassifiers( trainers, ClassifierData::NUM_FEATURES );

        return S_OK;
    }

    //--------------------------------------------------------------------------------------
    // Name: TrainWeakClassifiers
    // Desc: Trains a strong classifier per feature set and returns the weak classifiers
    //       that contributes to that strong classifier
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::TrainWeakClassifiers( ClassifierData::EType classifierDataType, LabeledExamples& labeledExamples, const DOUBLE fErrorThreshold )
    {
		RETURN_ON_FAIL( GenerateWeakClassifiers( classifierDataType ) );

		if (m_StrongClassifier.GetNumWeakClassifiers()==0)
		{
			return S_FALSE;
		}

        // Clear and duplicate the training example data
        m_LabeledExamples.m_iLabels.clear();
        m_LabeledExamples.m_pExamples.clear();
        m_LabeledExamples.m_uTimeStamps.clear();

        for ( UINT i = 0; i < labeledExamples.m_iLabels.size(); i++ )
        {
            m_LabeledExamples.m_iLabels.push_back( labeledExamples.m_iLabels[ i ] );
        }

        for ( UINT i = 0; i < labeledExamples.m_pExamples.size(); i++ )
        {
            m_LabeledExamples.m_pExamples.push_back( labeledExamples.m_pExamples[ i ] );
        }

        for ( UINT i = 0; i < labeledExamples.m_uTimeStamps.size(); i++ )
        {
            m_LabeledExamples.m_uTimeStamps.push_back( labeledExamples.m_uTimeStamps[ i ] );
        }

        // Run Adaboost only on this feature set
        m_fErrorThreshold = fErrorThreshold;
        RETURN_ON_FAIL( TrainStrongClassifier( FALSE ) );
        Optimize( 0, FALSE );

        m_LabeledExamples.m_iLabels.clear();
        m_LabeledExamples.m_pExamples.clear();
        m_LabeledExamples.m_uTimeStamps.clear();

        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: GenerateWeakClassifiers
    // Desc: Generates a set of weak classifiers as decision stumps, given some parameters
    //       for min, max and the step value between min and max
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::GenerateWeakClassifiers( ClassifierData* pClassifierData,
        const FLOAT fMin,
        const FLOAT fMax,
        const FLOAT fStep,
		const BOOL bUseReject)
    {
        WeakClassifier  weakClassifier;
        UINT uID;

		UINT uDataIndex[ 2 ] = { 0, 0 };

		uDataIndex[ 0 ] = (UINT)m_ClassifierData.size();
// 		// Use the index in the array as the unique id
// 		uID = (UINT)m_ClassifierData.size();
		uID = pClassifierData->MakeUID();
        pClassifierData->SetID( uID );

        // Add the data
        m_ClassifierData.push_back( pClassifierData );

		ClassifierData* pClassifierDataClone = pClassifierData;

		if (bUseReject)
		{
			// Now add rejection of inferred joints
			pClassifierDataClone = pClassifierData->Clone();
			RETURN_ON_NULL( pClassifierDataClone );
			pClassifierDataClone->SetRejectInfferedJoints( TRUE );

			uDataIndex[ 1 ] = (UINT)m_ClassifierData.size();
// 			// Use the index in the array as the unique id
// 			uID = (UINT)m_ClassifierData.size();
			uID = pClassifierData->MakeUID();
			pClassifierDataClone->SetID( uID );

			// Add the cloned data
			m_ClassifierData.push_back( pClassifierDataClone );       
		}

        // For each threshold we generate 4 classifiers. Two for choosing to use or
        // reject inferred joints, and each one of those are also reversed, so that
        // the learning algorithm can find the best weak classifier
//        ClassifierData* pData[ 2 ] = { pClassifierData,  pClassifierDataClone };

		UINT max = bUseReject ? 2 : 1;

        for ( UINT i = 0; i < max; i++ )
        {
            for ( FLOAT fThreshold = fMin; fThreshold <= fMax; fThreshold += fStep )
            {
                // Setup the weak classifier
                weakClassifier.SetThreshold( fThreshold );
//                weakClassifier.SetData( pData[ i ] );
				weakClassifier.SetDataIndex( uDataIndex[ i ] );

                // Add the weak classifiers
                weakClassifier.SetIsReversed( FALSE );
                m_StrongClassifier.Add( weakClassifier );
            }
        }

        for ( UINT i = 0; i < max; i++ )
        {
            for ( FLOAT fThreshold = fMin; fThreshold <= fMax; fThreshold += fStep )
            {
                // Setup the weak classifier
                weakClassifier.SetThreshold( fThreshold );
//                weakClassifier.SetData( pData[ i ] );
				weakClassifier.SetDataIndex( uDataIndex[ i ] );

                // Add the weak classifiers
                weakClassifier.SetIsReversed( TRUE );
                m_StrongClassifier.Add( weakClassifier );
            }
        }

        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: GenerateWeakClassifiers
    // Desc: This is where all the classifiers are generated for each feature
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::GenerateWeakClassifiers( ClassifierData::EType classifierDataType )
    {
        WeakClassifier  weakClassifier;
        ClassifierData* pClassifierData;

		//a bit of a patch but we need to support both at the same time while building separate classifiers
		if (!m_bUseSkeleton && classifierDataType < ClassifierData::TYPE_OPTICAL_FLOW_X)
		{
			return S_FALSE;
		}
		if (!m_bUseOpticalFlow && classifierDataType >= ClassifierData::TYPE_OPTICAL_FLOW_X)
		{
			return S_FALSE;
		}


        GESTURE_JOINT_INDEX verticalAngleJoints[] = {   GESTURE_JOINT_SHOULDER_LEFT,
                                                        GESTURE_JOINT_ELBOW_LEFT,
                                                        GESTURE_JOINT_WRIST_LEFT,
                                                        GESTURE_JOINT_SHOULDER_RIGHT,
                                                        GESTURE_JOINT_ELBOW_RIGHT,
                                                        GESTURE_JOINT_WRIST_RIGHT,
                                                        GESTURE_JOINT_HIP_LEFT,
                                                        GESTURE_JOINT_KNEE_LEFT,
                                                        GESTURE_JOINT_ANKLE_LEFT,
                                                        GESTURE_JOINT_HIP_RIGHT,
                                                        GESTURE_JOINT_KNEE_RIGHT,
                                                        GESTURE_JOINT_ANKLE_RIGHT };

        UINT nNumClassifierData = ARRAYSIZE( verticalAngleJoints );
        for ( UINT i = 0; i < nNumClassifierData; i++ )
        {
#ifdef ADD_TYPE_ANGLE
            if ( classifierDataType == ClassifierData::TYPE_ANGLE )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngles ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngles( GESTURE_JOINT_SPINE_MID,
                                                                        GESTURE_JOINT_SPINE_SHOULDER,
                                                                        verticalAngleJoints[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleMin, fAngleMax, fAngleStep ) );
            }
#endif

#ifdef ADD_TYPE_ANGLE_VELOCITY
            if ( classifierDataType == ClassifierData::TYPE_ANGLE_VELOCITY )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngleVelocities ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngleVelocities(    GESTURE_JOINT_SPINE_MID,
                                                                                    GESTURE_JOINT_SPINE_SHOULDER,
                                                                                    verticalAngleJoints[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleVelocityMin, fAngleVelocityMax, fAngleVelocityStep ) );
            }
#endif

#ifdef ADD_TYPE_ANGLE_ACCELERATION
            if ( classifierDataType == ClassifierData::TYPE_ANGLE_ACCELERATION )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngleAcceleration ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngleAcceleration( GESTURE_JOINT_SPINE_MID,
                                                                                    GESTURE_JOINT_SPINE_SHOULDER,
                                                                                    verticalAngleJoints[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleAccelMin, fAngleAccelMax, fAngleAccelStep ) );
            }
#endif
        }

        GESTURE_JOINT_INDEX leftHorizontalAngleJoints[] = { GESTURE_JOINT_HEAD,
                                                                    GESTURE_JOINT_ELBOW_LEFT,
                                                                    GESTURE_JOINT_WRIST_LEFT,
                                                                    GESTURE_JOINT_HIP_LEFT,
                                                                    GESTURE_JOINT_KNEE_LEFT,
                                                                    GESTURE_JOINT_ANKLE_LEFT };

        nNumClassifierData = ARRAYSIZE( leftHorizontalAngleJoints );
        for ( UINT i = 0; i < nNumClassifierData; i++ )
        {
#ifdef ADD_TYPE_ANGLE
            if ( classifierDataType == ClassifierData::TYPE_ANGLE )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngles ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngles(    GESTURE_JOINT_SPINE_SHOULDER,
                                                                    GESTURE_JOINT_SHOULDER_LEFT,
                                                                    leftHorizontalAngleJoints[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleMin, fAngleMax, fAngleStep ) );
            }
#endif

#ifdef ADD_TYPE_ANGLE_VELOCITY
            if ( classifierDataType == ClassifierData::TYPE_ANGLE_VELOCITY )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngleVelocities ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngleVelocities(   GESTURE_JOINT_SPINE_SHOULDER,
                                                                            GESTURE_JOINT_SHOULDER_LEFT,
                                                                            leftHorizontalAngleJoints[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleVelocityMin, fAngleVelocityMax, fAngleVelocityStep ) );
            }
#endif

#ifdef ADD_TYPE_ANGLE_ACCELERATION
            if ( classifierDataType == ClassifierData::TYPE_ANGLE_ACCELERATION )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngleAcceleration ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngleAcceleration(  GESTURE_JOINT_SPINE_SHOULDER,
                                                                                    GESTURE_JOINT_SHOULDER_LEFT,
                                                                                    leftHorizontalAngleJoints[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleAccelMin, fAngleAccelMax, fAngleAccelStep ) );
            }
#endif
        }

        GESTURE_JOINT_INDEX rightHorizontalAngleJoints[] = {    GESTURE_JOINT_HEAD,
                                                                        GESTURE_JOINT_ELBOW_RIGHT,
                                                                        GESTURE_JOINT_WRIST_RIGHT,
                                                                        GESTURE_JOINT_HIP_RIGHT,
                                                                        GESTURE_JOINT_KNEE_RIGHT,
                                                                        GESTURE_JOINT_ANKLE_RIGHT };

        nNumClassifierData = ARRAYSIZE( rightHorizontalAngleJoints );
        for ( UINT i = 0; i < nNumClassifierData; i++ )
        {
#ifdef ADD_TYPE_ANGLE
            if ( classifierDataType == ClassifierData::TYPE_ANGLE )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngles ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngles( GESTURE_JOINT_SPINE_SHOULDER,
                                                                        GESTURE_JOINT_SHOULDER_RIGHT,
                                                                        rightHorizontalAngleJoints[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleMin, fAngleMax, fAngleStep ) );
            }
#endif

#ifdef ADD_TYPE_ANGLE_VELOCITY
            if ( classifierDataType == ClassifierData::TYPE_ANGLE_VELOCITY )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngleVelocities ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngleVelocities(    GESTURE_JOINT_SPINE_SHOULDER,
                                                                                    GESTURE_JOINT_SHOULDER_RIGHT,
                                                                                    rightHorizontalAngleJoints[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleVelocityMin, fAngleVelocityMax, fAngleVelocityStep ) );
            }
#endif

#ifdef ADD_TYPE_ANGLE_ACCELERATION
            if ( classifierDataType == ClassifierData::TYPE_ANGLE_ACCELERATION )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngleAcceleration ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngleAcceleration(  GESTURE_JOINT_SPINE_SHOULDER,
                                                                                    GESTURE_JOINT_SHOULDER_RIGHT,
                                                                                    rightHorizontalAngleJoints[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleAccelMin, fAngleAccelMax, fAngleAccelStep ) );
            }
#endif
        }

        GESTURE_JOINT_INDEX generalAngleJoints[][3] = 
        { 
            { GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_SHOULDER_LEFT },
            { GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_SHOULDER_RIGHT },
            { GESTURE_JOINT_ANKLE_LEFT, GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_HIP_LEFT },
            { GESTURE_JOINT_ANKLE_RIGHT, GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_HIP_RIGHT },
            { GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_WRIST_RIGHT },
            { GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_KNEE_RIGHT },
            { GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_KNEE_LEFT },
            { GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_KNEE_RIGHT },
            { GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_KNEE_RIGHT },
            { GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_KNEE_LEFT },
        };

        nNumClassifierData = ARRAYSIZE( generalAngleJoints );
        for ( UINT i = 0; i < nNumClassifierData; i++ )
        {
#ifdef ADD_TYPE_ANGLE
            if ( classifierDataType == ClassifierData::TYPE_ANGLE )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngles ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngles( generalAngleJoints[i][0], generalAngleJoints[i][1], generalAngleJoints[i][2] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleMin, fAngleMax, fAngleStep ) );
            }
#endif

#ifdef ADD_TYPE_ANGLE_VELOCITY
            if ( classifierDataType == ClassifierData::TYPE_ANGLE_VELOCITY )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngleVelocities ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngleVelocities( generalAngleJoints[i][0], generalAngleJoints[i][1], generalAngleJoints[i][2] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleVelocityMin, fAngleVelocityMax, fAngleVelocityStep ) );
            }
#endif

#ifdef ADD_TYPE_ANGLE_ACCELERATION
            if ( classifierDataType == ClassifierData::TYPE_ANGLE_ACCELERATION )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingAngleAcceleration ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingAngleAcceleration( generalAngleJoints[i][0], generalAngleJoints[i][1], generalAngleJoints[i][2] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAngleAccelMin, fAngleAccelMax, fAngleAccelStep ) );
            }
#endif
        }

#ifdef ADD_TYPE_TIME_SPACE_ANGLE
        if ( classifierDataType == ClassifierData::TYPE_TIME_SPACE_ANGLE )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingTimeSpaceAngles ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingTimeSpaceAngles( (GESTURE_JOINT_INDEX)i );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fTimeSpaceAngleMin, fTimeSpaceAngleMax, fTimeSpaceAngleStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_MUSCLE_POWER
        if ( classifierDataType == ClassifierData::TYPE_MUSCLE_POWER )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingMusclePower ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingMusclePower( (GESTURE_JOINT_INDEX)i );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fMusclePowerMin, fMusclePowerMax, fMusclePowerStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_MUSCLE_FORCES
        if ( classifierDataType == ClassifierData::TYPE_MUSCLE_FORCE_X )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingMuscleForceX ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingMuscleForceX( (GESTURE_JOINT_INDEX)i );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fMuscleForceMin, fMuscleForceMax, fMuscleForceStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_MUSCLE_FORCES
        if ( classifierDataType == ClassifierData::TYPE_MUSCLE_FORCE_Y )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingMuscleForceY ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingMuscleForceY( (GESTURE_JOINT_INDEX)i );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fMuscleForceMin, fMuscleForceMax, fMuscleForceStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_MUSCLE_FORCES
        if ( classifierDataType == ClassifierData::TYPE_MUSCLE_FORCE_Z )
        {

            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingMuscleForceZ ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingMuscleForceZ( (GESTURE_JOINT_INDEX)i );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fMuscleForceMin, fMuscleForceMax, fMuscleForceStep ) );
            }
        }
#endif


#ifdef ADD_TYPE_MUSCLE_TORQUES
        if ( classifierDataType == ClassifierData::TYPE_MUSCLE_TORQUE_X )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingMuscleTorqueX ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingMuscleTorqueX( (GESTURE_JOINT_INDEX)i );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fMuscleTorqueMin, fMuscleTorqueMax, fMuscleTorqueStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_MUSCLE_TORQUES
        if ( classifierDataType == ClassifierData::TYPE_MUSCLE_TORQUE_Y )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingMuscleTorqueY ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingMuscleTorqueY( (GESTURE_JOINT_INDEX)i );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fMuscleTorqueMin, fMuscleTorqueMax, fMuscleTorqueStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_MUSCLE_TORQUES
        if ( classifierDataType == ClassifierData::TYPE_MUSCLE_TORQUE_Z )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingMuscleTorqueZ ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingMuscleTorqueZ( (GESTURE_JOINT_INDEX)i );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fMuscleTorqueMin, fMuscleTorqueMax, fMuscleTorqueStep ) );
            }
        }
#endif

        GESTURE_JOINT_INDEX positionJoints[] = 
        { 
            GESTURE_JOINT_WRIST_LEFT,
            GESTURE_JOINT_WRIST_RIGHT,
            GESTURE_JOINT_ELBOW_LEFT,
            GESTURE_JOINT_ELBOW_RIGHT,
            GESTURE_JOINT_KNEE_LEFT,
            GESTURE_JOINT_KNEE_RIGHT,
            GESTURE_JOINT_ANKLE_LEFT,
            GESTURE_JOINT_ANKLE_RIGHT 
        };

        nNumClassifierData = ARRAYSIZE( positionJoints );

#ifdef ADD_TYPE_DIFF_POSITION_X
        if ( classifierDataType == ClassifierData::TYPE_DIFF_POSITION_X )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                for ( UINT j = 0; j < nNumClassifierData; j++ )
                {
                    if ( i != (UINT)positionJoints[ j ] )
                    {
                        void* pMem;
                        RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingDiffPositionX ), 4 ) );
                        pClassifierData = new (pMem) ClassifierDataUsingDiffPositionX( positionJoints[ j ], (GESTURE_JOINT_INDEX)i );
                        RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fPositionMin, fPositionMax, fPositionStep ) );
                    }
                }
            }
        }
#endif

#ifdef ADD_TYPE_DIFF_POSITION_Y
        if ( classifierDataType == ClassifierData::TYPE_DIFF_POSITION_Y )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                for ( UINT j = 0; j < nNumClassifierData; j++ )
                {
                    if ( i != (UINT)positionJoints[ j ] )
                    {
                        void* pMem;
                        RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingDiffPositionY ), 4 ) );
                        pClassifierData = new (pMem) ClassifierDataUsingDiffPositionY( positionJoints[ j ], (GESTURE_JOINT_INDEX)i );
                        RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fPositionMin, fPositionMax, fPositionStep ) );
                    }
                }
            }
        }
#endif

#ifdef ADD_TYPE_DIFF_POSITION_Z
        if ( classifierDataType == ClassifierData::TYPE_DIFF_POSITION_Z )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                for ( UINT j = 0; j < nNumClassifierData; j++ )
                {
                    if ( i != (UINT)positionJoints[ j ] )
                    {
                        void* pMem;
                        RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingDiffPositionZ ), 4 ) );
                        pClassifierData = new (pMem) ClassifierDataUsingDiffPositionZ( positionJoints[ j ], (GESTURE_JOINT_INDEX)i );
                        RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fPositionMin, fPositionMax, fPositionStep ) );
                    }
                }
            }
        }
#endif

#ifdef ADD_TYPE_DIFF_MUSCLE_FORCE_X
        if ( classifierDataType == ClassifierData::TYPE_DIFF_MUSCLE_FORCE_X )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                for ( UINT j = 0; j < nNumClassifierData; j++ )
                {
                    if ( i != (UINT)positionJoints[ j ] )
                    {
                        void* pMem;
                        RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingDiffMuscleForceX ), 4 ) );
                        pClassifierData = new (pMem) ClassifierDataUsingDiffMuscleForceX( positionJoints[ j ], (GESTURE_JOINT_INDEX)i );
                        RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fDiffMuscleForceMin, fDiffMuscleForceMax, fDiffMuscleForceStep ) );
                    }
                }
            }
        }
#endif

#ifdef ADD_TYPE_DIFF_MUSCLE_FORCE_Y
        if ( classifierDataType == ClassifierData::TYPE_DIFF_MUSCLE_FORCE_Y )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                for ( UINT j = 0; j < nNumClassifierData; j++ )
                {
                    if ( i != (UINT)positionJoints[ j ] )
                    {
                        void* pMem;
                        RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingDiffMuscleForceY ), 4 ) );
                        pClassifierData = new (pMem) ClassifierDataUsingDiffMuscleForceY( positionJoints[ j ], (GESTURE_JOINT_INDEX)i );
                        RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fDiffMuscleForceMin, fDiffMuscleForceMax, fDiffMuscleForceStep ) );
                    }
                }
            }
        }
#endif

#ifdef ADD_TYPE_DIFF_MUSCLE_FORCE_Z
        if ( classifierDataType == ClassifierData::TYPE_DIFF_MUSCLE_FORCE_Z )
        {
            for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
            {
                for ( UINT j = 0; j < nNumClassifierData; j++ )
                {
                    if ( i != (UINT)positionJoints[ j ] )
                    {
                        void* pMem;
                        RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingDiffMuscleForceZ ), 4 ) );
                        pClassifierData = new (pMem) ClassifierDataUsingDiffMuscleForceZ( positionJoints[ j ], (GESTURE_JOINT_INDEX)i );
                        RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fDiffMuscleForceMin, fDiffMuscleForceMax, fDiffMuscleForceStep ) );
                    }
                }
            }
        }
#endif

        GESTURE_JOINT_INDEX jointVelocities[] = 
        { 
            GESTURE_JOINT_SPINE_SHOULDER,
            GESTURE_JOINT_SHOULDER_LEFT,
            GESTURE_JOINT_ELBOW_LEFT,
            GESTURE_JOINT_WRIST_LEFT,
            GESTURE_JOINT_SHOULDER_RIGHT,
            GESTURE_JOINT_ELBOW_RIGHT,
            GESTURE_JOINT_WRIST_RIGHT,
            GESTURE_JOINT_KNEE_LEFT,
            GESTURE_JOINT_ANKLE_LEFT,
            GESTURE_JOINT_KNEE_RIGHT,
            GESTURE_JOINT_ANKLE_RIGHT 
        };

        nNumClassifierData = ARRAYSIZE( jointVelocities );

#ifdef ADD_TYPE_POSITION_SPEED
        if ( classifierDataType == ClassifierData::TYPE_POSITION_SPEED )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionSpeed ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionSpeed( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fSpeedMin, fSpeedMax, fSpeedStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_SPEED_SQ
        if ( classifierDataType == ClassifierData::TYPE_POSITION_SPEED_SQ )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionSpeedSQ ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionSpeedSQ( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fSpeedSQMin, fSpeedSQMax, fSpeedSQStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_ACCELERATION
        if ( classifierDataType == ClassifierData::TYPE_POSITION_ACCELERATION )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionAcceleration ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionAcceleration( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAccelMin, fAccelMax, fAccelStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_ACCELERATION_X
        if ( classifierDataType == ClassifierData::TYPE_POSITION_ACCELERATION_X )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionAccelerationX ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionAccelerationX( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAccelMin, fAccelMax, fAccelStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_ACCELERATION_Y
        if ( classifierDataType == ClassifierData::TYPE_POSITION_ACCELERATION_Y )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionAccelerationY ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionAccelerationY( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAccelMin, fAccelMax, fAccelStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_ACCELERATION_Z
        if ( classifierDataType == ClassifierData::TYPE_POSITION_ACCELERATION_Z )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionAccelerationZ ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionAccelerationZ( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fAccelMin, fAccelMax, fAccelStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_VELOCITY_X
        if ( classifierDataType == ClassifierData::TYPE_POSITION_VELOCITY_X )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionVelocityX ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionVelocityX( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fVelocityMin, fVelocityMax, fVelocityStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_VELOCITY_Y
        if ( classifierDataType == ClassifierData::TYPE_POSITION_VELOCITY_Y )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionVelocityY ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionVelocityY( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fVelocityMin, fVelocityMax, fVelocityStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_VELOCITY_Z
        if ( classifierDataType == ClassifierData::TYPE_POSITION_VELOCITY_Z )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionVelocityZ ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionVelocityZ( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fVelocityMin, fVelocityMax, fVelocityStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_VELOCITYSQ_X
        if ( classifierDataType == ClassifierData::TYPE_POSITION_VELOCITYSQ_X )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionVelocitySQX ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionVelocitySQX( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fVelocitySQMin, fVelocitySQMax, fVelocitySQStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_VELOCITYSQ_Y
        if ( classifierDataType == ClassifierData::TYPE_POSITION_VELOCITYSQ_Y )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionVelocitySQY ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionVelocitySQY( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fVelocitySQMin, fVelocitySQMax, fVelocitySQStep ) );
            }
        }
#endif

#ifdef ADD_TYPE_POSITION_VELOCITYSQ_Z
        if ( classifierDataType == ClassifierData::TYPE_POSITION_VELOCITYSQ_Z )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingPositionVelocitySQZ ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingPositionVelocitySQZ( jointVelocities[ i ] );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fVelocitySQMin, fVelocitySQMax, fVelocitySQStep ) );
            }
        }
#endif

        struct Bone
        {
            GESTURE_JOINT_INDEX parent;
            GESTURE_JOINT_INDEX child;
        };

        Bone bones[] = 
        { 
            { GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SHOULDER_LEFT },
            { GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_ELBOW_LEFT },
            { GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_WRIST_LEFT },
            { GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_HAND_LEFT },
            { GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SHOULDER_RIGHT },
            { GESTURE_JOINT_SHOULDER_RIGHT, GESTURE_JOINT_ELBOW_RIGHT },
            { GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_WRIST_RIGHT },
            { GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_HAND_RIGHT },
            { GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_HIP_LEFT },
            { GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_KNEE_LEFT },
            { GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_ANKLE_LEFT },
            { GESTURE_JOINT_ANKLE_LEFT, GESTURE_JOINT_FOOT_LEFT },
            { GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_HIP_RIGHT },
            { GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_KNEE_RIGHT },
            { GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_ANKLE_RIGHT },
            { GESTURE_JOINT_ANKLE_RIGHT, GESTURE_JOINT_FOOT_RIGHT },
            { GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SPINE_MID },
            { GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_MID },
            { GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_HEAD },
            { GESTURE_JOINT_HEAD, GESTURE_JOINT_KNEE_LEFT },
            { GESTURE_JOINT_HEAD, GESTURE_JOINT_KNEE_RIGHT },
            { GESTURE_JOINT_HAND_LEFT, GESTURE_JOINT_HAND_RIGHT },
            { GESTURE_JOINT_HAND_LEFT, GESTURE_JOINT_SHOULDER_RIGHT },
            { GESTURE_JOINT_HAND_RIGHT, GESTURE_JOINT_SHOULDER_LEFT } 
        };

        nNumClassifierData = ARRAYSIZE( bones );

#ifdef ADD_TYPE_BONE_LENGTH_CHANGES
        if ( classifierDataType == ClassifierData::TYPE_BONE_LENGTH_CHANGES )
        {
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                void* pMem;
                RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingBoneLengthChanges ), 4 ) );
                pClassifierData = new (pMem) ClassifierDataUsingBoneLengthChanges( bones[ i ].parent, bones[ i ].child );
                RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fBoneChangesMin, fBoneChangesMax, fBoneChangesStep ) );
            }
        }
#endif


#ifdef ADD_TYPE_OPTICAL_FLOW
		if ( classifierDataType == ClassifierData::TYPE_OPTICAL_FLOW_X )
		{
			for ( UINT ij = 0; ij < 3*3; ij++ )
			{
				void* pMem;
				RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingOpticalFlowX ), 4 ) );
				pClassifierData = new (pMem) ClassifierDataUsingOpticalFlowX( ij );
				RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fOpticalFlowMin, fOpticalFlowMax, fOpticalFlowStep, FALSE ) );
			}
		}
		if ( classifierDataType == ClassifierData::TYPE_OPTICAL_FLOW_Y )
		{
			for ( UINT ij = 0; ij < 3*3; ij++ )
			{
				void* pMem;
				RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingOpticalFlowY ), 4 ) );
				pClassifierData = new (pMem) ClassifierDataUsingOpticalFlowY( ij );
				RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fOpticalFlowMin, fOpticalFlowMax, fOpticalFlowStep, FALSE ) );
			}
		}
		if ( classifierDataType == ClassifierData::TYPE_OPTICAL_FLOW_LENGTH_SQ )
		{
			for ( UINT i = 0; i < 3; i++ )
			{
				for ( UINT j = 0; j < 3; j++ )
				{
					void* pMem;
					RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingOpticalFlowLengthSq ), 4 ) );
					pClassifierData = new (pMem) ClassifierDataUsingOpticalFlowLengthSq( i, j );
					RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fOpticalFlowLenSQMin, fOpticalFlowLenSQMax, fOpticalFlowLenSQStep, FALSE ) );
				}
			}
		}
		if ( classifierDataType == ClassifierData::TYPE_OPTICAL_FLOW_TANGENT )
		{
			for ( UINT i = 0; i < 3; i++ )
			{
				for ( UINT j = 0; j < 3; j++ )
				{
					void* pMem;
					RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingOpticalFlowTangent ), 4 ) );
					pClassifierData = new (pMem) ClassifierDataUsingOpticalFlowTangent( i, j );
					RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fOpticalFlowTanMin, fOpticalFlowTanMax, fOpticalFlowTanStep, FALSE ) );
				}
			}
		}
		if ( classifierDataType == ClassifierData::TYPE_OPTICAL_FLOW_X_DIFF )
		{
			for ( UINT a = 0; a < 3*3-1; a++ )
			{
				for ( UINT b = a; b < 3*3; b++ )
				{
					void* pMem;
					RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingOpticalFlowXDiff ), 4 ) );
					pClassifierData = new (pMem) ClassifierDataUsingOpticalFlowXDiff( a, b );
					RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fOpticalFlowDiffMin, fOpticalFlowDiffMax, fOpticalFlowDiffStep, FALSE ) );
				}
			}
		}
		if ( classifierDataType == ClassifierData::TYPE_OPTICAL_FLOW_Y_DIFF )
		{
			for ( UINT a = 0; a < 3*3-1; a++ )
			{
				for ( UINT b = a; b < 3*3; b++ )
				{
					void* pMem;
					RETURN_ON_NULL( pMem = AllocateAligned( sizeof( ClassifierDataUsingOpticalFlowYDiff ), 4 ) );
					pClassifierData = new (pMem) ClassifierDataUsingOpticalFlowYDiff( a, b );
					RETURN_ON_FAIL( GenerateWeakClassifiers( pClassifierData, fOpticalFlowDiffMin, fOpticalFlowDiffMax, fOpticalFlowDiffStep, FALSE ) );
				}
			}
		}
#endif

        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: CombineWeakClassifiers
    // Desc: Combine all weak classifiers in the first pass so that it can be used in the
    //       seconds pass of training for the final strong classifier
    //--------------------------------------------------------------------------------------

    VOID GestureDetectorTrainer::CombineWeakClassifiers( GestureDetectorTrainer* pGestureDetectorTrainers, const UINT nNumTrainers )
    {
        WeakClassifier  weakClassifier;

        for ( UINT i = 0; i < nNumTrainers; i++ )
        {
            GestureDetectorTrainer* pGestureDetectorTrainer = &pGestureDetectorTrainers[ i ];

            for ( UINT j = 0; j < pGestureDetectorTrainer->GetNumWeakClassifiers(); j++ )
            {
                WeakClassifier* pWeakClassifier = pGestureDetectorTrainer->m_StrongClassifier.GetWeakClassifierAt( j );

                // Add the weak classifier
                weakClassifier.SetThreshold( pWeakClassifier->GetThreshold() );
//                weakClassifier.SetData( pWeakClassifier->GetData() );
				weakClassifier.SetDataIndex( pWeakClassifier->GetDataIndex() );
                weakClassifier.SetIsReversed( pWeakClassifier->GetIsReversed() );
                weakClassifier.SetAlpha( 0.0f );
                m_StrongClassifier.Add( weakClassifier );
//                pWeakClassifier->SetData( NULL );
				pWeakClassifier->SetDataIndex( 0 );
            }
        }        
    }


    //--------------------------------------------------------------------------------------
    // Name: TrainStrongClassifier
    // Desc: Implementation of the AdaBoost learning algorithm
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::TrainStrongClassifier( const BOOL bFinalPass )
    {
        if ( bFinalPass )
        {
            printf( "\n\t-Evaluating classifier data for each example skeleton frame..." );
        }

        // Time how long this takes
        DWORD dwStart = GetTickCount();

        // Clear all the data
        ClassifierData::Initialize();
        m_StrongClassifier.Reset( 0 );
        m_uPreviousTimeStamp = 0;
        XMVECTOR vUpVector = XMVectorSet( 0.0f, 1.0f, 0.0f, 0.0f );

        // Cache values from weak classifier data on ground truth so that
        // we don't have to call Update() NxN times, but only N times during training
        const UINT nNumExamples = (UINT)( m_LabeledExamples.m_pExamples.size() );
        const UINT nNumClassifierData = (UINT)( m_ClassifierData.size() );
        const UINT nNumWeakClassifiers = m_StrongClassifier.GetNumWeakClassifiers();  // Number of weak classifiers
        vector<vector<FLOAT>> cachedClassifierDataValues;

        // We're caching lots of values to optimize training time, so we better make sure
        // there aren't any bad memory allocations from STL
        try
        {
            cachedClassifierDataValues.resize( nNumClassifierData );
            for ( UINT i = 0; i < nNumClassifierData; i++ )
            {
                cachedClassifierDataValues[ i ].resize( nNumExamples );
            }
        }
        catch ( bad_alloc& badAllocation )
        {
            printf( "\n\nCould not allocate memory (%s)", badAllocation.what() );
            return E_FAIL;
        }

		if ( !bFinalPass )
		{
			printf(".");
		}

        // Do the work that would normally have been done millions of times in the inner loop of the training
        // and cache the results of the Update() with regards to each classifier data instance
//#define FRAMES_TO_SKIP_AFTER_RESET	(5)
        int nFramesToSkip = m_nFramesToSkip;//FRAMES_TO_SKIP_AFTER_RESET;
		int nFramesSkipped = 0;
		for ( UINT i = 0; i < nNumExamples; i++ )
        {
            GESTURE_SKELETON_TYPE* pSkeletonData = m_LabeledExamples.m_pExamples[ i ];
            UINT64 uTimeStamp = m_LabeledExamples.m_uTimeStamps[ i ];

            LARGE_INTEGER liTimeStamp;
            liTimeStamp.QuadPart = uTimeStamp;
            if (Update( 0, pSkeletonData, liTimeStamp, vUpVector ))
			{
				nFramesToSkip = m_nFramesToSkip;//FRAMES_TO_SKIP_AFTER_RESET;
			}
			if (nFramesToSkip > 0)
			{
				m_LabeledExamples.m_iLabels[ i ] = 0;
				nFramesSkipped++;
				nFramesToSkip--;
			}

            // Let openMP run this loop on multiple threads
#pragma omp parallel for
            for ( INT j = 0; j < (INT)nNumClassifierData; j++ )
            {
                cachedClassifierDataValues[ j ][ i ] =  m_ClassifierData[ j ]->GetValue( 0 );
            }
        }

		if ( !bFinalPass )
		{
//			printf("%d frames skipped", nFramesSkipped);
			printf(".");
		}

        // We're caching lots of values to optimize training time, so we better make sure
        // there aren't any bad memory allocations from STL
        vector<vector<BYTE>> cachedClassificationsError;
        try
        {
            cachedClassificationsError.resize( nNumWeakClassifiers );
            for ( UINT i = 0; i < nNumWeakClassifiers; i++ )
            {
                cachedClassificationsError[ i ].resize( nNumExamples );
            }
        }
        catch ( bad_alloc& badAllocation )
        {
            printf( "\n\nCould not allocate memory (%s)", badAllocation.what() );
            return E_FAIL;
        }

        vector<UINT> cachedClassificationErrorSum;
        try
        {
            cachedClassificationErrorSum.resize( nNumWeakClassifiers );
        }
        catch ( bad_alloc& badAllocation )
        {
            printf( "\n\nCould not allocate memory (%s)", badAllocation.what() );
            return E_FAIL;
        }

		if ( !bFinalPass )
		{
			printf(".");
		}

        // Classify() will get called millions of times in the inner loop of training on already known data.
        // We therefore iIterate through all weak classifiers and cache the results of Classify() for each
        // weak classifier with regards to each example data
#pragma omp parallel for
        for ( INT i = 0; i < (INT)nNumWeakClassifiers; i++ )
        {
            UINT uSum = 0;
            WeakClassifier* pWeakClassifier = m_StrongClassifier.GetWeakClassifierAt( i );

            // Iterate through all ground truth examples
            for ( UINT n = 0; n < nNumExamples; n++ )
            {
				INT8 iGroundTruthLabel = m_LabeledExamples.m_iLabels[ n ];
				if (iGroundTruthLabel==0)
				{//ignore this frame
					cachedClassificationsError[ i ][ n ] = 0;
					continue;
				}

//                UINT uClassifierDataIndex = pWeakClassifier->GetData()->GetID();
				UINT uClassifierDataIndex = pWeakClassifier->GetDataIndex();
                FLOAT fValue = cachedClassifierDataValues[ uClassifierDataIndex ][ n ];
                INT8 iWeakClassifierResult = pWeakClassifier->Classify( fValue );

                // if the weak classifier wrongly classifies this example, then contribute to the error
                cachedClassificationsError[ i ][ n ] = ( iWeakClassifierResult == iGroundTruthLabel ) ? 0 : 1;

                // For optimization, get the sum of the error
                uSum += cachedClassificationsError[ i ][ n ];
            }

            cachedClassificationErrorSum[ i ] = uSum;
        }

        DWORD dwStop = GetTickCount();
        if ( bFinalPass )
        {
            printf( "Done" );
        } else
		{
			printf(".");
		}

        dwStart = GetTickCount();

        // User specified 0, which means use all weak classifiers at runtime, but only the ones that pass the error threshold
        const BOOL bUseAllWeakClassifiers = ( m_nNumWeakClassifiersAtRuntime == 0 );

        // Number of examples in training set. N is normally used in literature.
        const UINT N = nNumExamples;

        // Number of iterations for training loop. T is normally used in literature. If it is an intermediate pass for one of the
        // feature sets, we simply use all the generated weak classifiers and stop when one of them reaches the error threshold. But,
        // if this is for the final pass where the final strong classifier is trained, we use the number of runtime classifiers specified by the user
//        const UINT T = bFinalPass ? ( bUseAllWeakClassifiers ? nNumWeakClassifiers : m_nNumWeakClassifiersAtRuntime ) : nNumWeakClassifiers;
		const UINT T = min( nNumWeakClassifiers, m_nNumWeakClassifiersAtRuntime );

        // The highest error threshold is 0.5 since a weak classifier has to have a better than 50/50 change to be correct, anything higher means
        // that the weak classifier is simply too weak to contribute to solving the problem space
		DOUBLE skipRatio = (DOUBLE)(nNumExamples-nFramesSkipped) / (DOUBLE)nNumExamples;
        const DOUBLE fErrorThreshold = (bFinalPass ? 0.5 : m_fErrorThreshold);

        vector<DOUBLE> d( N, (1.0 / N) * skipRatio );                     // Distributed weight for each example
        vector<BOOL> bClassifierChoosen( nNumWeakClassifiers, FALSE );

        if ( bFinalPass )
        {       
            printf( "\n\t-Running AdaBoost using %d (of %d available) hardware threads...", m_nNumThreadsForTraining, m_nMaxNumThreads );
        }

        // Optimization. Since we're auto generating the classifiers with a min->max range and step, there are most
        // likely lots of overlapping. If we do find that the cached results for classifiers next to each other in
        // the range are exactly the sample for all examples, the we simply remove one of them. Theoretically speaking
        // the removed classifier still has a chance to be boosted by the algorithm, but emperical results show that
        // we can safely remove these from the pool of classifiers.
        if ( !bFinalPass )
        {
            for ( INT i = 1; i < (INT)nNumWeakClassifiers; i++ )
            {
                // if the sum isn't the same, then no need to look through each then no need to compare each example
                if ( cachedClassificationErrorSum[ i ] != cachedClassificationErrorSum[ i - 1 ] )
                {
                    continue;
                }

                // if the sum is the same, then check the classification error on each example for both classifiers
                BOOL bClassifiersTheSame = TRUE;

                for ( INT n = 0; n < (INT)nNumExamples; n++ )
                {
                    if ( cachedClassificationsError[ i ][ n ] != cachedClassificationsError[ i - 1 ][ n ] )
                    {
                        bClassifiersTheSame = FALSE;
                        break;
                    }
                }

                // if two neighbor classifiers give the same results, the remove it from the pool of classifiers that
                // AdaBoost will work with.
                if ( bClassifiersTheSame )
                {
                    bClassifierChoosen[ i ] = TRUE;
                }
            }
        }

		if ( !bFinalPass )
		{
			printf(".");
		}

        // AdaBoost algorithm, iterate T times
        for ( UINT t = 0; t < T; t++ )
        {
            // Find the best weak classifier, i.e. the weak classifier with the smallest error
            UINT uBestWeakClassifierIndex = 0;
            DOUBLE fMinError = FLT_MAX;

            // Let openMP run this loop on multiple threads
#pragma omp parallel for
            // Iterate through all weak classifiers
            for ( INT i = 0; i < (INT)nNumWeakClassifiers; i++ )
            {
                // We could remove a weak classifier from the list and add it to another list, but with
                // so many potential classifers, it's more optimal to just mark it when it's used
                if ( !bClassifierChoosen[ i ] )
                {
                    DOUBLE fError = 0.0;

                    // Iterate through all ground truth examples
                    for ( UINT n = 0; n < N; n++ )
                    {
                        fError += d[ n ] * (DOUBLE)(cachedClassificationsError[ i ][ n ]);
                    }

                    // Let openMP know that this if() operation cannot be parallelized
#pragma omp critical
                    if ( fError < fMinError )
                    {
                        fMinError = fError;
                        uBestWeakClassifierIndex = i;
                    }
                }
            }

            // A weak classifier's error has to be less than 0.5 to contribute successfully, since a weak classifier
            // has to to be better than 50/50 chance of doing a correct classification
            if ( fMinError / skipRatio >= fErrorThreshold )
            {
                break;
            }

            bClassifierChoosen[ uBestWeakClassifierIndex ] = TRUE;
            WeakClassifier* pBestWeakClassifier = m_StrongClassifier.GetWeakClassifierAt( uBestWeakClassifierIndex );

            // Get the confidence of the weak classifier as alpha (lower error => higher alpha)
            DOUBLE fAlpha = ( fMinError == 0.0f ) ? 0.0f : ( 0.5 * log( ( 1.0 - fMinError ) / fMinError ) );
            pBestWeakClassifier->SetAlpha( (FLOAT)fAlpha );

            // Emphasize the training examples that do not agree with the weak classifier h(x)
            DOUBLE Z = 0.0;
#pragma omp parallel for
            for ( INT n = 0; n < (INT)N; n++ )
            {
                INT8 iGroundTruthLabel = m_LabeledExamples.m_iLabels[ n ];
//                UINT uClassifierDataIndex = pBestWeakClassifier->GetData()->GetID();
				UINT uClassifierDataIndex = pBestWeakClassifier->GetDataIndex();
                FLOAT fValue = cachedClassifierDataValues[ uClassifierDataIndex ][ n ];

                d[ n ] *= exp( -fAlpha * iGroundTruthLabel * pBestWeakClassifier->Classify( fValue ) );
            }

            // We need to normalize the distribution, but we want to parallelize it, so first get the sum
            for ( INT n = 0; n < (INT)N; n++ )
            {
                Z += d[ n ];
            }

            // Normalize to a probability distribution and use openMP to do this on multiple threads
#pragma omp parallel for
            for ( INT n = 0; n < (INT)N; n++ )
            {
                d[ n ] /= Z;
            }
        }

        dwStop = GetTickCount();

        if ( bFinalPass )
        {
            printf( "Done" );
        } else
		{
			printf(".");
		}

        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: ByteSwapSkeletonData
    // Desc: Byteswamp skeleton data so that it can be used on PC
    //--------------------------------------------------------------------------------------
//#ifdef TARGET_X360
//    VOID GestureDetectorTrainer::ByteSwapSkeletonDataRead( GESTURE_SKELETON_TYPE* pSkeletonData )
//    {
//		pSkeletonData->m_trackingState = ByteSwap32BitRead( GESTURE_GET_TRACKING(pSkeletonData) );
//		pSkeletonData->m_trackingID = ByteSwap32BitRead( GESTURE_GET_TRACKING_ID(pSkeletonData) );
//        //pSkeletonData->dwEnrollmentIndex = ByteSwap32BitRead( pSkeletonData->dwEnrollmentIndex );
//        //pSkeletonData->dwUserIndex = ByteSwap32BitRead( pSkeletonData->dwUserIndex );
//
//        for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
//        {
//            pSkeletonData->m_jointPositions[ i ].m128_f32[ 0 ] = ByteSwap32BitRead( pSkeletonData->m_jointPositions[ i ].m128_f32[ 0 ] );
//            pSkeletonData->m_jointPositions[ i ].m128_f32[ 1 ] = ByteSwap32BitRead( pSkeletonData->m_jointPositions[ i ].m128_f32[ 1 ] );
//            pSkeletonData->m_jointPositions[ i ].m128_f32[ 2 ] = ByteSwap32BitRead( pSkeletonData->m_jointPositions[ i ].m128_f32[ 2 ] );
//            pSkeletonData->m_jointPositions[ i ].m128_f32[ 3 ] = ByteSwap32BitRead( pSkeletonData->m_jointPositions[ i ].m128_f32[ 3 ] );
//        }
//
//        for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
//        {
//            pSkeletonData->m_trackingState[ i ] = ByteSwap32BitRead( pSkeletonData->m_trackingState[ i ] );
//        }
//
//        pSkeletonData->dwQualityFlags = ByteSwap32BitRead( pSkeletonData->dwQualityFlags );
//    }
//#endif
    //--------------------------------------------------------------------------------------
    // Name: SwapPositions
    // Desc: Used with MirroSkeletonData(), this simply swaps two vectors
    //--------------------------------------------------------------------------------------

    static inline VOID SwapPositions( XMVECTOR& vPosition0, XMVECTOR& vPosition1 )
    {
        XMVECTOR vTemp = vPosition0;
        vPosition0 = vPosition1;
        vPosition1 = vTemp;
    }


    //--------------------------------------------------------------------------------------
    // Name: SwapTrackingStatus
    // Desc: Used with MirroSkeletonData(), this simply swaps two states
    //--------------------------------------------------------------------------------------

    static inline VOID SwapTrackingStatus( GESTURE_SKELETON_TYPE* pSkeletonData, const GESTURE_JOINT_INDEX left, const GESTURE_JOINT_INDEX right )
    {
        GESTURE_JOINT_TRACKING_STATE temp = GESTURE_GET_JOINT_TRACKING( pSkeletonData, left );
        GESTURE_GET_JOINT_TRACKING( pSkeletonData, left ) = GESTURE_GET_JOINT_TRACKING( pSkeletonData, right );
        GESTURE_GET_JOINT_TRACKING( pSkeletonData, right ) = temp;
    }


    //--------------------------------------------------------------------------------------
    // Name: SwapJointData
    // Desc: Used with MirroSkeletonData(), this swaps data from two joints
    //--------------------------------------------------------------------------------------

    VOID SwapJointData( GESTURE_SKELETON_TYPE* pSkeletonData, const GESTURE_JOINT_INDEX left, const GESTURE_JOINT_INDEX right )
    {
        SwapPositions( GESTURE_GET_JOINT_POS( pSkeletonData, left ), GESTURE_GET_JOINT_POS( pSkeletonData, right ) );
        SwapTrackingStatus( pSkeletonData, left, right );
    }


    //--------------------------------------------------------------------------------------
    // Name: MirrorSkeletonData
    // Desc: Mirror the skeleton data
    //--------------------------------------------------------------------------------------

    VOID GestureDetectorTrainer::MirrorSkeletonData( GESTURE_SKELETON_TYPE* pSkeletonData )
    {
        SwapJointData( pSkeletonData, GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_SHOULDER_RIGHT );
        SwapJointData( pSkeletonData, GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_ELBOW_RIGHT );
        SwapJointData( pSkeletonData, GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_WRIST_RIGHT );
        SwapJointData( pSkeletonData, GESTURE_JOINT_HAND_LEFT, GESTURE_JOINT_HAND_RIGHT );
        SwapJointData( pSkeletonData, GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_HIP_RIGHT );
        SwapJointData( pSkeletonData, GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_KNEE_RIGHT );
        SwapJointData( pSkeletonData, GESTURE_JOINT_ANKLE_LEFT, GESTURE_JOINT_ANKLE_RIGHT );
        SwapJointData( pSkeletonData, GESTURE_JOINT_FOOT_LEFT, GESTURE_JOINT_FOOT_RIGHT );

        const XMVECTOR vMirror = XMVectorSet( -1.0f, 1.0f, 1.0f, 1.0f );
        for ( UINT i = 0; i < GESTURE_JOINT_COUNT; i++ )
        {
            GESTURE_GET_JOINT_POS( pSkeletonData, i ) *= vMirror;
        }
#ifdef TARGET_X360  // durango target uses a hack here - dont want to swap the hips position twice!
        GESTURE_GET_POS( pSkeletonData ) *= vMirror;
#endif
    }

    //--------------------------------------------------------------------------------------
    // Name: GenerateLabeledExamples
    // Desc: Used tagged xed files to generate labeled training/testing examples as ground truth
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::GenerateLabeledExamples( const CHAR* szGestureName,                                                                
                                                             CHAR* const* szExampleFiles,
                                                             const UINT uiExampleFileCount, 
                                                             const CHAR* szDestPath,
                                                             const BOOL bUseRawSkeletonData,
                                                             const INT iSkeletonIndexToProcess,
															 INT8 iLabels[],
															 CHAR* const* szOutputFiles )
    {
        Reset();

        for ( UINT i = 0; i < uiExampleFileCount; i++ )
        {
			if( iLabels == NULL )
			{
				RETURN_ON_FAIL( GenerateLabeledExamplesFromRawSkeleton( szGestureName, szExampleFiles[ i ], iSkeletonIndexToProcess ) );
			}
			else
			{
				RETURN_ON_FAIL( GenerateLabeledExamplesFromRawSkeleton( szGestureName, szExampleFiles[ i ], iSkeletonIndexToProcess, iLabels[ i ] ) );
			}

			if( szOutputFiles )
			{
				RETURN_ON_FAIL( SaveLabeledExamples( szOutputFiles[ i ] ) );
				// Since we're saving to separate files we must reset between them
				Reset();
			}
        }

		if( !szOutputFiles )
		{
			RETURN_ON_FAIL( SaveLabeledExamples( szDestPath ) );
		}

        return S_OK;
    }


    //--------------------------------------------------------------------------------------
    // Name: GenerateLabeledExamplesFromRawSkeleton
    // Desc: Generate labeled examples using raw skeleton data in the xed files
    //--------------------------------------------------------------------------------------

    HRESULT GestureDetectorTrainer::GenerateLabeledExamplesFromRawSkeleton( const CHAR* szGestureName,
                                                                            const CHAR* szExamplesFileName,
                                                                            INT iSkeletonIndexToProcess,
																			INT8 iExampleLabel )
    {
		wchar_t wideFilename[MAX_PATH];
		memset( wideFilename, 0, MAX_PATH*sizeof(wchar_t) );
		MultiByteToWideChar( CP_UTF8, 0, szExamplesFileName, strlen(szExamplesFileName), wideFilename, MAX_PATH );

		IExample* ex = FExample::Create(wideFilename);
		Error::EExamplesError err = ex->Load(wideFilename);
		if( err != Error::Success )
		{
			printf( "\n\nError: Unable to read %s. %i\n\n", szExamplesFileName, (int)err );
			delete ex;
			return E_FAIL;
		}
		// TODO: only read and unpack skeleton frames
		err = ex->ReadAllFrames();
		if( err != Error::Success )
		{
			printf( "\n\nError: Unable to load frames from %s. %i\n\n", szExamplesFileName, (int)err );
			delete ex;
			return E_FAIL;
		}
		err = ex->UnpackAllFrames();
		if( err != Error::Success )
		{
			printf( "\n\nError: Unable to unpack frames in %s. %i\n\n", szExamplesFileName, (int)err );
			delete ex;
			return E_FAIL;
		}

        // We potentially open multiple files from multiple folders and we want to add all the labeled examples into one list
        const UINT uOffsetIndex = (UINT)m_LabeledExamples.m_pExamples.size();

		printf( "\n  %s\n\t-Reading skeleton frames from .e file and labeling...\n", szExamplesFileName );

		BOOL bToDoSkeleton[GESTURE_SKELETON_COUNT];
		memset( &bToDoSkeleton, 0, sizeof( BOOL ) * GESTURE_SKELETON_COUNT );
		INT iCurrentSkeleton =  iSkeletonIndexToProcess;
		BOOL bScanningSkeletons = ( iSkeletonIndexToProcess == -1 );

		while ( iCurrentSkeleton < (INT)GESTURE_SKELETON_COUNT && ( iCurrentSkeleton == -1 || bToDoSkeleton[iCurrentSkeleton]) )
		{
			BOOL bTrackedSkeleton = FALSE;
			BOOL bReset = FALSE;

			for( UINT64 frameIndex = 0; frameIndex < ex->GetFrameCount(); ++frameIndex )
            {      
				Examples::Frame frame;
				if(!ex->FetchFrame(frameIndex, frame))
				{
					printf( "\n\nError: Unable to read %s. %i\n\n", szExamplesFileName, (int)err );
					delete ex;
					return E_FAIL;
				}
				GESTURE_FRAME_TYPE& skeletonFrame = frame.m_skeletonFrame;
			    INT8 iLabel = g_iClassificationLabelIncorrect;
			
			    // If there's a valid label passed for this entire file just use it and continue
			    if( iExampleLabel != 0 && (iExampleLabel == g_iClassificationLabelIncorrect || iExampleLabel == g_iClassificationLabelCorrect) )
			    {
				    iLabel = iExampleLabel;
				    m_nTotalNumGestures++;
				    m_nNumTrainingGestures++;
			    }

			    //find which skeletons are active ever in the whole timeline
			    if ( bScanningSkeletons )
			    {
				    for ( INT iSkeletonIndex = 0; iSkeletonIndex < GESTURE_SKELETON_COUNT; iSkeletonIndex++ )
				    {
						GESTURE_SKELETON_TYPE& skeletonData = GESTURE_GET_SKEL( skeletonFrame, iSkeletonIndex );

					    // If not tracked, then ignore
					    if ( GESTURE_GET_TRACKING( &skeletonData ) != GESTURE_SKELETON_NOT_TRACKED )
					    {
						    bToDoSkeleton[iSkeletonIndex] = TRUE;
						    if ( iCurrentSkeleton == -1 )
						    {
							    iCurrentSkeleton = iSkeletonIndex;
						    }
						    continue;
					    }
				    }
			    }

			    //not a real loop, just to simplify the refactoring ;)
			    if (iCurrentSkeleton >= 0)
			    for( INT iSkeletonIndex = iCurrentSkeleton; iSkeletonIndex<=iCurrentSkeleton; iSkeletonIndex++)
			    {
					GESTURE_SKELETON_TYPE& skeletonData = GESTURE_GET_SKEL( skeletonFrame, iSkeletonIndex );

				    if ( GESTURE_GET_TRACKING( &skeletonData ) != GESTURE_SKELETON_TRACKED )
				    {
					    bTrackedSkeleton = FALSE;
					    continue;
				    }

				    if( !bTrackedSkeleton )
				    {
					    bTrackedSkeleton = TRUE;
					    bReset = TRUE;
				    }

                    // Apply tilt correction on the data
                    ApplyTiltCorrection( iSkeletonIndex, &GESTURE_GET_SKEL( skeletonFrame, iSkeletonIndex ), &GESTURE_GET_SKEL( skeletonFrame, iSkeletonIndex ), GESTURE_GET_NORMAL( skeletonFrame ), bReset );

                    // Add the skeleton data as a training example with the NUI_SKELETON_FRAME timestamp
                    GESTURE_SKELETON_TYPE* pSkeletonData = (GESTURE_SKELETON_TYPE*)AllocateAligned( sizeof( GESTURE_SKELETON_TYPE ), 16 );
                    memcpy( pSkeletonData, &GESTURE_GET_SKEL( skeletonFrame, iSkeletonIndex ), sizeof( GESTURE_SKELETON_TYPE ) );
                    m_LabeledExamples.m_pExamples.push_back( pSkeletonData );
                    m_LabeledExamples.m_iLabels.push_back( iLabel );
					m_LabeledExamples.m_uTimeStamps.push_back( frame.m_timestamp );
                }

			    //printf("%.1f%%...   \r", ((float)i/(float)nNumDepthEvents)*100.0f );
            }

			if ( iCurrentSkeleton < 0 )
				break;

			bScanningSkeletons = false;
			bToDoSkeleton[iCurrentSkeleton] = FALSE;

			iCurrentSkeleton++;
			for ( INT iSkeletonIndex = iCurrentSkeleton; iSkeletonIndex < GESTURE_SKELETON_COUNT; iSkeletonIndex++ )
			{
				if ( bToDoSkeleton[iSkeletonIndex] )
				{
					iCurrentSkeleton = iSkeletonIndex;
					break;
				}
			}
		}

        printf( "\nDone" );

		delete ex;

        return S_OK;
    }
}

// for ITF zlib usage, we need to define these functions for memory allocation
#undef free
#undef malloc
namespace ITF
{

struct MemoryId
{
	enum ITF_ALLOCATOR_IDS
	{
		ALLOCATOR_ID_FAKE,
	};
};

struct Memory
{
	static void* mallocCategory( size_t size, MemoryId::ITF_ALLOCATOR_IDS id );
	static void free(void* ptr);
};

void* Memory::mallocCategory( size_t size, MemoryId::ITF_ALLOCATOR_IDS id )
{
	UNREFERENCED_PARAMETER( id );
	return malloc( size );
}

void Memory::free(void* ptr)
{
	::free( ptr );
}

}


#endif