/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "_cvaux.h"

#define _CV_NORM_L2(a) (float)(icvSqrt32f(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]))
#define _CV_NORM_L22(a) (float)(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])

/****************************************************************************************\

   find region where hand is   (for gesture recognition)
   flag = 0 (use left bucket)  flag = 1 (use right bucket) 

\****************************************************************************************/

static CvStatus CV_STDCALL
icvFindHandRegion( CvPoint3D32f * points, int count,
                   CvSeq * indexs,
                   float *line, CvSize2D32f size, int flag,
                   CvPoint3D32f * center,
                   CvMemStorage * storage, CvSeq ** numbers )
{

/*    IppmVect32f sub, cros;   */
    float *sub, *cros;
    CvSeqWriter writer;
    CvSeqReader reader;

    CvStatus status;
    int nbins = 20, i, l, i_point, left, right;
    int *bin_counts = 0;        //  pointer to the point's counter in the bickets
    int low_count;              //  low threshold  

    CvPoint *tmp_number = 0, *pt;
    float value, vmin, vmax, vl, bsize, vc;
    float hand_length, hand_length2, hand_left, hand_right;
    float threshold, threshold2;
    float *vv = 0;
    float a[3];

    status = CV_OK;

    hand_length = size.width;
    hand_length2 = hand_length / 2;

    threshold = (float) (size.height * 3 / 5.);
    threshold2 = threshold * threshold;

/*    low_count = count/nbins;     */
    low_count = (int) (count / 60.);

    assert( points != NULL && line != NULL );
    if( points == NULL || line == NULL )
        return CV_NULLPTR_ERR;

    assert( count > 5 );
    if( count < 5 )
        return CV_BADFLAG_ERR;

    assert( flag == 0 || flag == 1 );
    if( flag != 0 && flag != 1 )
        return CV_BADFLAG_ERR;

/*  create vectors         */
    sub = icvCreateVector_32f( 3 );
    cros = icvCreateVector_32f( 3 );
    if( sub == NULL || cros == NULL )
        return CV_OUTOFMEM_ERR;

/*  alloc memory for the point's projections on the line    */
    vv = (float *) cvAlloc( count * sizeof( float ));

    if( vv == NULL )
        return CV_OUTOFMEM_ERR;

/*  alloc memory for the point's counter in the bickets     */
    bin_counts = (int *) cvAlloc( nbins * sizeof( int ));

    if( bin_counts == NULL )
    {
        status = CV_OUTOFMEM_ERR;
        goto M_END;
    }
    memset( bin_counts, 0, nbins * sizeof( int ));

    cvStartReadSeq( indexs, &reader, 0 );

/*  alloc memory for the temporale point's numbers      */
    tmp_number = (CvPoint *) cvAlloc( count * sizeof( CvPoint ));
    if( tmp_number == NULL )
    {
        status = CV_OUTOFMEM_ERR;
        goto M_END;
    }

/*  find min and max point's projection on the line     */
    vmin = 1000;
    vmax = -1000;
    i_point = 0;
    for( i = 0; i < count; i++ )
    {
/*    
        icvSubVector_32f ((IppmVect32f )&points[i], (IppmVect32f )&line[3], sub, 3);

        icvCrossProduct2L_32f ((IppmVect32f )&line[0], sub, cros);
*/

        sub[0] = points[i].x - line[3];
        sub[1] = points[i].y - line[4];
        sub[2] = points[i].z - line[5];
        a[0] = sub[0] * line[1] - sub[1] * line[0];
        a[1] = sub[1] * line[2] - sub[2] * line[1];
        a[2] = sub[2] * line[0] - sub[0] * line[2];

/*      if(IPPI_NORM_L22 ( cros ) < threshold2)    */
        if( _CV_NORM_L22( a ) < threshold2 )
        {
            value = (float)icvDotProduct_32f( sub, &line[0], 3 );
            if( value > vmax )
                vmax = value;
            if( value < vmin )
                vmin = value;

            vv[i_point] = value;

            pt = (CvPoint*)cvGetSeqElem( indexs, i );
            tmp_number[i_point] = *pt;
            i_point++;
        }
    }

/*  compute the length of one bucket             */
    vl = vmax - vmin;
    bsize = vl / nbins;

/*  compute the number of points in each bucket   */
    for( i = 0; i < i_point; i++ )
    {
        l = cvRound( (vv[i] - vmin) / bsize );
        bin_counts[l]++;
    }

    *numbers = cvCreateSeq( CV_SEQ_POINT_SET, sizeof( CvSeq ), sizeof( CvPoint ), storage );
    assert( numbers != 0 );
    if( numbers == NULL )
    {
        status = CV_OUTOFMEM_ERR;
        goto M_END;
    }

    cvStartAppendToSeq( *numbers, &writer );

    if( flag == 0 )
    {
/*  find the leftmost bucket           */
        for( l = 0; l < nbins; l++ )
        {
            if( bin_counts[l] > low_count )
                break;
        }
        left = l;

/*  compute center point of the left hand     */
        hand_left = vmin + left * bsize;
        vc = hand_left + hand_length2;
        hand_right = hand_left + hand_length;
    }
    else
    {
/*  find the rightmost bucket                */
        for( l = nbins - 1; l >= 0; l-- )
        {
            if( bin_counts[l] > low_count )
                break;
        }
        right = l;

/*  compute center point of the right hand    */
        hand_right = vmax - (nbins - right - 1) * bsize;
        vc = hand_right - hand_length2;
        hand_left = hand_right - hand_length;
    }

    icvScaleVector_32f( &line[0], sub, 3, vc );
    icvAddVector_32f( &line[3], sub, (float *) center, 3 );

/*  select hand's points and calculate mean value     */

    //ss.x = ss.y = ss.z = 0;
    for( l = 0; l < i_point; l++ )
    {
        if( vv[l] >= hand_left && vv[l] <= hand_right )
        {
            CV_WRITE_SEQ_ELEM( tmp_number[l], writer );

        }
    }

    cvEndWriteSeq( &writer );

  M_END:
    if( tmp_number != NULL )
        cvFree( &tmp_number );
    if( bin_counts != NULL )
        cvFree( &bin_counts );
    if( vv != NULL )
        cvFree( &vv );
    if( sub != NULL ) icvDeleteVector (sub);
    if( cros != NULL ) icvDeleteVector (cros);

    return status;

}


//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////


#define _CV_NORM_L31(a) (float)(icvSqrt32f(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]))
#define _CV_NORM_L32(a) (float)(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])

/****************************************************************************************\

   find region where hand is   (for gesture recognition)
   flag = 0 (use left bucket)  flag = 1 (use right bucket)

\****************************************************************************************/

static CvStatus CV_STDCALL
icvFindHandRegionA( CvPoint3D32f * points, int count,
                    CvSeq * indexs,
                    float *line, CvSize2D32f size, int jc,
                    CvPoint3D32f * center,
                    CvMemStorage * storage, CvSeq ** numbers )
{

/*    IppmVect32f sub, cros;   */
    float *sub, *cros;
    float eps = (float) 0.01;
    CvSeqWriter writer;
    CvSeqReader reader;

    CvStatus status;
    float gor[3] = { 1, 0, 0 };
    float ver[3] = { 0, 1, 0 };

    int nbins = 20, i, l, i_point, left, right, jmin, jmax, jl;
    int j_left, j_right;
    int *bin_counts = 0;        //  pointer to the point's counter in the bickets

//    int *bin_countsj = 0;   //  pointer to the index's counter in the bickets
    int low_count;              //  low threshold  

    CvPoint *tmp_number = 0, *pt;
    float value, vmin, vmax, vl, bsize, bsizej, vc, vcl, vcr;
    double v_ver, v_gor;
    float hand_length, hand_length2, hand_left, hand_right;
    float threshold, threshold2;
    float *vv = 0;
    float a[3];
    char log;

    status = CV_OK;

    hand_length = size.width;
    hand_length2 = hand_length / 2;

    threshold = (float) (size.height * 3 / 5.);
    threshold2 = threshold * threshold;

/*    low_count = count/nbins;     */
    low_count = (int) (count / 60.);

    assert( points != NULL && line != NULL );
    if( points == NULL || line == NULL )
        return CV_NULLPTR_ERR;

    assert( count > 5 );
    if( count < 5 )
        return CV_BADFLAG_ERR;

/*  create vectors         */
    sub = icvCreateVector_32f( 3 );
    cros = icvCreateVector_32f( 3 );
    if( sub == NULL || cros == NULL )
        return CV_OUTOFMEM_ERR;

/*  alloc memory for the point's projections on the line    */
    vv = (float *) cvAlloc( count * sizeof( float ));

    if( vv == NULL )
        return CV_OUTOFMEM_ERR;

/*  alloc memory for the point's counter in the bickets     */
    bin_counts = (int *) cvAlloc( nbins * sizeof( int ));

    if( bin_counts == NULL )
    {
        status = CV_OUTOFMEM_ERR;
        goto M_END;
    }
    memset( bin_counts, 0, nbins * sizeof( int ));

/*  alloc memory for the point's counter in the bickets     */
//    bin_countsj = (int*) icvAlloc(nbins*sizeof(int));  
//    if(bin_countsj == NULL) {status = CV_OUTOFMEM_ERR; goto M_END;}
//    memset(bin_countsj,0,nbins*sizeof(int));

    cvStartReadSeq( indexs, &reader, 0 );

/*  alloc memory for the temporale point's numbers      */
    tmp_number = (CvPoint *) cvAlloc( count * sizeof( CvPoint ));
    if( tmp_number == NULL )
    {
        status = CV_OUTOFMEM_ERR;
        goto M_END;
    }

/*  find min and max point's projection on the line     */
    vmin = 1000;
    vmax = -1000;
    jmin = 1000;
    jmax = -1000;
    i_point = 0;
    for( i = 0; i < count; i++ )
    {
/*
        icvSubVector_32f ((IppmVect32f )&points[i], (IppmVect32f )&line[3], sub, 3);

        icvCrossProduct2L_32f ((IppmVect32f )&line[0], sub, cros);
*/

        sub[0] = points[i].x - line[3];
        sub[1] = points[i].y - line[4];
        sub[2] = points[i].z - line[5];

//      if(fabs(sub[0])<eps||fabs(sub[1])<eps||fabs(sub[2])<eps) continue;

        a[0] = sub[0] * line[1] - sub[1] * line[0];
        a[1] = sub[1] * line[2] - sub[2] * line[1];
        a[2] = sub[2] * line[0] - sub[0] * line[2];

        v_gor = icvDotProduct_32f( gor, &line[0], 3 );
        v_ver = icvDotProduct_32f( ver, &line[0], 3 );

        if( v_ver > v_gor )
            log = true;
        else
            log = false;


/*      if(IPPI_NORM_L22 ( cros ) < threshold2)    */
/*
        if(fabs(a[0])<eps && fabs(a[1])<eps && fabs(a[2])<eps)
        {
            icvDotProduct_32f( sub, &line[0], 3, &value);
            if(value > vmax) vmax = value;
            if(value < vmin) vmin = value;
            
            vv[i_point] = value;

            pt = (CvPoint* )icvGetSeqElem ( indexs, i, 0);

            if(pt->x > jmax) jmax = pt->x;
            if(pt->x < jmin) jmin = pt->x;

            tmp_number[i_point] = *pt;
            i_point++;
        }
        else 
*/
        {
            if( _CV_NORM_L32( a ) < threshold2 )
            {
                value = (float)icvDotProduct_32f( sub, &line[0], 3 );
                if( value > vmax )
                    vmax = value;
                if( value < vmin )
                    vmin = value;

                vv[i_point] = value;

                pt = (CvPoint*)cvGetSeqElem( indexs, i );

                if( !log )
                {
                    if( pt->x > jmax )
                        jmax = pt->x;
                    if( pt->x < jmin )
                        jmin = pt->x;
                }
                else
                {
                    if( pt->y > jmax )
                        jmax = pt->y;
                    if( pt->y < jmin )
                        jmin = pt->y;
                }


                tmp_number[i_point] = *pt;
                i_point++;
            }
        }
    }

/*  compute the length of one bucket along the line        */
    vl = vmax - vmin;

/*  examining on the arm's existence  */
    if( vl < eps )
    {
        *numbers = NULL;
        status = CV_OK;
        goto M_END;
    }

    bsize = vl / nbins;

/*  compute the number of points in each bucket along the line  */
    for( i = 0; i < i_point; i++ )
    {
        l = cvRound( (vv[i] - vmin) / bsize );
        bin_counts[l]++;
    }

    /*  compute the length of one bucket along the X axe        */
    jl = jmax - jmin;
    if( jl <= 1 )
    {
        *numbers = NULL;
        status = CV_OK;
        goto M_END;
    }

    bsizej = (float) (jl / (nbins + 0.));

/*  compute the number of points in each bucket along the X axe */
//    for(i=0;i<i_point;i++)
//    {
//        l = cvRound((tmp_number[i].x - jmin)/bsizej);
//        bin_countsj[l]++;
//    }


    left = right = -1;

/*  find the leftmost and the rightmost buckets           */
    for( l = 0; l < nbins; l++ )
    {
        if( bin_counts[l] > low_count && left == -1 )
            left = l;
        else if( bin_counts[l] > low_count && left >= 0 )
            right = l;

    }

/*  compute center point of the left hand     */
    if( left == -1 && right == -1 )
    {
        *numbers = NULL;
        status = CV_OK;
        goto M_END;
    }

    hand_left = vmin + left * bsize;
    j_left = (int) (jmin + left * bsizej);

    vcl = hand_left + hand_length2;

/*  compute center point of the right hand    */
    hand_right = vmax - (nbins - right - 1) * bsize;
    vcr = hand_right - hand_length2;

    j_right = (int) (jmax - (nbins - right - 1) * bsizej);

    j_left = abs( j_left - jc );
    j_right = abs( j_right - jc );

    if( j_left <= j_right )
    {
        hand_right = hand_left + hand_length;
        vc = vcl;
    }
    else
    {
        hand_left = hand_right - hand_length;
        vc = vcr;
    }

    icvScaleVector_32f( &line[0], sub, 3, vc );
    icvAddVector_32f( &line[3], sub, (float *) center, 3 );

/*  select hand's points and calculate mean value     */
    *numbers = cvCreateSeq( CV_SEQ_POINT_SET, sizeof( CvSeq ), sizeof( CvPoint ), storage );
    assert( *numbers != 0 );
    if( *numbers == NULL )
    {
        status = CV_OUTOFMEM_ERR;
        goto M_END;
    }

    cvStartAppendToSeq( *numbers, &writer );

    for( l = 0; l < i_point; l++ )
    {
        if( vv[l] >= hand_left && vv[l] <= hand_right )
        {
            CV_WRITE_SEQ_ELEM( tmp_number[l], writer );

        }
    }

    cvEndWriteSeq( &writer );

  M_END:
    if( tmp_number != NULL )
        cvFree( &tmp_number );
//    if(bin_countsj != NULL) cvFree( &bin_countsj );
    if( bin_counts != NULL )
        cvFree( &bin_counts );

    if( vv != NULL )
        cvFree( &vv );

    if( sub != NULL ) icvDeleteVector (sub);
    if( cros != NULL ) icvDeleteVector (cros);

    return status;
}


/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name:     cvFindHandRegion
//    Purpose:  finds hand region in range image data
//    Context:   
//    Parameters: 
//      points - pointer to the input point's set.
//      count  - the number of the input points.
//      indexs - pointer to the input sequence of the point's indexes
//      line   - pointer to the 3D-line
//      size   - size of the hand in meters 
//      flag   - hand direction's flag (0 - left, -1 - right, 
//               otherwise j-index of the initial image center)
//      center - pointer to the output hand center
//      storage - pointer to the memory storage  
//      numbers - pointer to the output sequence of the point's indexes inside
//                hand region                
//      
//    Notes:
//F*/
CV_IMPL void
cvFindHandRegion( CvPoint3D32f * points, int count,
                  CvSeq * indexs,
                  float *line, CvSize2D32f size, int flag,
                  CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers )
{
    CV_FUNCNAME( "cvFindHandRegion" );
    __BEGIN__;

    if(flag == 0 || flag == -1)
	{
		IPPI_CALL( icvFindHandRegion( points, count, indexs, line, size, -flag,
			                           center, storage, numbers ));
	}
	else 
		IPPI_CALL( icvFindHandRegionA( points, count, indexs, line, size, flag,
			                            center, storage, numbers ));

    __CLEANUP__;
    __END__;
}

/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name:     cvFindHandRegionA
//    Purpose:  finds hand region in range image data
//    Context:   
//    Parameters: 
//      points - pointer to the input point's set.
//      count  - the number of the input points.
//      indexs - pointer to the input sequence of the point's indexes
//      line   - pointer to the 3D-line
//      size   - size of the hand in meters 
//      jc     - j-index of the initial image center
//      center - pointer to the output hand center
//      storage - pointer to the memory storage  
//      numbers - pointer to the output sequence of the point's indexes inside
//                hand region                
//      
//    Notes:
//F*/
CV_IMPL void
cvFindHandRegionA( CvPoint3D32f * points, int count,
                   CvSeq * indexs,
                   float *line, CvSize2D32f size, int jc,
                   CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers )
{
    CV_FUNCNAME( "cvFindHandRegionA" );
    __BEGIN__;

    IPPI_CALL( icvFindHandRegionA( points, count, indexs, line, size, jc,
                                    center, storage, numbers ));
    __CLEANUP__;
    __END__;
}