
/************************************************************
**  ExtendedSourceTracker.c
**
**  Object name :
**    SourceTracker
**
**  History :
**     2013.10.31 M.Takigahira IFINDEX関連(Source構造体の変更)に対応
**     2013.09.12 M.Takigahira SourceTrackerをベースに新規作成
**
**  Description
**  -----------
**  音源定位のトレース
**
**  Copyright (c) 2012-2013
**  System In Frontier,Inc.
**  All rights reserved.
**
**  当社の許可なしに複製および転載を禁ず
**
*/

/*=========================================================*/
/*                   Include Files                         */
/*=========================================================*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "HA_log.h"
#include "android_hark.h"

#include "HABuffer.h"
#include "ExtendedSourceTracker.h"

/*=========================================================*/
/*                   Macro Definition                      */
/*=========================================================*/
#ifdef DEBUG
#define DEBUG_PRINT_LM LOGI
#else
#define DEBUG_PRINT_LM(...)
#endif

#include "build_modules.h"
#if !defined BUILD_MODULE_EXTENDED_SOURCE_TRACKER
/******************************************************************************
**
**  History
**  -----------
**  2013.09.12 M.Takigahira 新規作成
**
**  Description
**  -----------
**  全ての関数をダミー定義
**
*/
static float SourceCosTheta(float *x, float *y){ return 0.0f; }
static float SourceAngle(float *x, float *y){ return 0.0f; }
static inline int IABS(int a){ return 0; }
static int SourceTfindexDistance(SOURCE_t *x, SOURCE_t *y){ return 0; }
int HASetParamExtendedSourceTracker(Param_SourceTracker_t *param){ return 0; }
int HADynamicParamUpdateExtendedSourceTracker(Param_ExtendedSourceTracker_t *param){ return 0; }
int HAInitExtendedSourceTracker(void){ return 0; }
int ExtendedSourceTracker(int frame_count, SOURCE_DATA_ARY_t *sources){ return 0; }
int ExtendedSourceTrackerWithFilter(int frame_count, SOURCE_DATA_ARY_t *sources, SOURCE_DATA_ARY_t *target){ return 0; }
//int HASetParamSourceIntervalExtender(Param_SourceIntervalExtender_t *param){ return 0; }
//int SourceIntervalExtender(int frame_count, SOURCE_DATA_ARY_t *sources){ return 0; }
#else // #if !defined BUILD_MODULE_SOURCE_TRACKER
/*=========================================================*/
/*                 Structure Declaration                   */
/*=========================================================*/

/*=========================================================*/
/*                    Private Valiable                     */
/*=========================================================*/
static Param_ExtendedSourceTracker_t extended_source_tracker_info;
SOURCE_DATA_ARY_t last_sources;
SOURCE_DATA_ARY_t target_sources;
static int next_source_id;

/*=========================================================*/
/*                    Public Valiable                      */
/*=========================================================*/

/************************************************************
**
**  Private : 
**
**  History :
**            2012.12.14 M.Takigahira TFINDEX方式の実装
**            2013.05.09 M.Takigahira 内積によるcos(theta)導出関数SourceCosTheta()（ただし、-1<=theta<=1の範囲外を許す）と
**                                    theta導出関数SourceAngle()（ただし、-1<=theta<=1への制限処理を含む）に分割した
**                                    最小角検出にはacosを行わないSourceCosTheta()を用いて行う
**            2012.12.14 K.Takagi 新規作成
**
**  Description
**  -----------
**  二つの音源の角度を算出
**
*/
static float SourceCosTheta(float *x, float *y) // input is SOURCE_t x, y; func(x.x, y.x)
{
    float prod;
    //prod = (x[0]*y[0]+x[1]*y[1]+x[2]*y[2]) / sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]) / sqrt(y[0]*y[0]+y[1]*y[1]+y[2]*y[2]);
    prod = (x[0]*y[0]+x[1]*y[1]+x[2]*y[2]) / sqrtf((x[0]*x[0]+x[1]*x[1]+x[2]*x[2]) * (y[0]*y[0]+y[1]*y[1]+y[2]*y[2]));
    return prod;
}

static float SourceAngle(float *x, float *y) // input is SOURCE_t x, y; func(x.x, y.x)
{
    float cos_theta;
    cos_theta = SourceCosTheta(x, y);
    return acosf((cos_theta < -1.0f ? -1.0f : (cos_theta > 1.0f ? 1.0f : cos_theta))); // acos(theta)のために-1<=theta<=1に収める
}

static inline int IABS(int a){ // (-INT_MIN == INT_MIN) case supported fast abs()
    return ((a < 0) ? ((-a < 0) ? -(a+1) : -a) : a);
}

static int SourceTfindexDistance(SOURCE_t *x, SOURCE_t *y) const // input is SOURCE_t x, y; func(x, y)
{
    int maxdist, tmpdist, i;
    for(i=0, maxdist=0; i<3 i++){
        if(x->tfindex[i] > y->tfindex[i]){
            tmpdist = min(IABS(x->tfindex[i] - y->tfindex[i]), IABS(x->tfindex[i] - x->tfindex_max[i]) + IABS(y->tfindex[i] - x->tfindex_min[i]));
        }
        else{
            tmpdist = min(IABS(x->tfindex[i] - y->tfindex[i]), IABS(x->tfindex[i] - x->tfindex_min[i]) + IABS(y->tfindex[i] - x->tfindex_max[i]));
        }
        maxdist = ((maxdist < tmpdist) ? tmpdist : maxdist);
    }
    return maxdist;
}

/************************************************************
**
**  Public  : 
**
**  History : 2013.09.12 M.Takigahira 新規作成
**
**  Description
**  -----------
**  パラメータ設定
**
*/
int HASetParamExtendedSourceTracker(Param_ExtendedSourceTracker_t *param)
{
    if((param->remove_length < 0) || (param->remove_length > param->remove_check_length) || (param->pause_length < param->remove_check_length)){ // 0 <= remove_length <= remove_check_length <= pause_length
        return -1;
    }
    if((param->compare_mode < 0) || (param->compare_mode > 1)){
        return -1;
    }
    if((param->remove_source_algorithm < 0) || (param->remove_source_algorithm > 1)){
        return -1;
    }
    if(param->min_id < 0){
        return -1;
    }
    
    extended_source_tracker_info.thresh                  = param->thresh;
    extended_source_tracker_info.pause_length            = param->pause_length;
    extended_source_tracker_info.remove_length           = param->remove_length;
    extended_source_tracker_info.remove_check_length     = param->remove_check_length;
    extended_source_tracker_info.compare_mode            = param->compare_mode;
    extended_source_tracker_info.min_src_interval        = param->min_src_interval;
    extended_source_tracker_info.min_tfindex_interval    = param->min_tfindex_interval;
    extended_source_tracker_info.preroll_length          = param->preroll_length;
    extended_source_tracker_info.max_num_source          = param->max_num_source;
    extended_source_tracker_info.remove_source_algorithm = param->remove_source_algorithm;
    extended_source_tracker_info.min_id                  = param->min_id;
    
    return 0;
}

/************************************************************
**
**  Public  : 
**
**  History : 2013.09.12 M.Takigahira 新規作成
**
**  Description
**  -----------
**  パラメータ設定
**
*/
int HADynamicParamUpdateExtendedSourceTracker(Param_ExtendedSourceTracker_t *param)
{
    if((param->remove_length < 0) || (param->remove_length > param->remove_check_length) || (param->pause_length < param->remove_check_length)){ // 0 <= remove_length <= remove_check_length <= pause_length
        return -1;
    }
    if((param->compare_mode < 0) || (param->compare_mode > 1)){
        return -1;
    }
    if((param->remove_source_algorithm < 0) || (param->remove_source_algorithm > 1)){
        return -1;
    }
    //if(param->min_id < 0){
    //    return -1;
    //}
    
    extended_source_tracker_info.thresh                  = param->thresh;
    extended_source_tracker_info.pause_length            = param->pause_length;
    extended_source_tracker_info.remove_length           = param->remove_length;
    extended_source_tracker_info.remove_check_length     = param->remove_check_length;
    extended_source_tracker_info.compare_mode            = param->compare_mode;
    extended_source_tracker_info.min_src_interval        = param->min_src_interval;
    extended_source_tracker_info.min_tfindex_interval    = param->min_tfindex_interval;
    extended_source_tracker_info.preroll_length          = param->preroll_length;
    extended_source_tracker_info.max_num_source          = param->max_num_source;
    extended_source_tracker_info.remove_source_algorithm = param->remove_source_algorithm;
    //extended_source_tracker_info.min_id                  = param->min_id;
    
    return 0;
}

/************************************************************
**
**  Public  : 
**
**  History : 
**            2013.09.12 M.Takigahira 新規作成
**
**  Description
**  -----------
**  初期化
**
*/
int HAInitExtendedSourceTracker(void)
{
    last_sources.source_num = 0;
    target_sources.source_num = 0;
    
    next_source_id = extended_source_tracker_info.min_id;
    
    return 0;
}

/************************************************************
**
**  Public  : 
**
**  History : 2013.09.12 M.Takigahira 新規作成
**
**  Description
**  -----------
**  トレース処理
**
*/
int ExtendedSourceTracker(int frame_count, SOURCE_DATA_ARY_t *sources)
{
    int i, j;
    int index;
    int sub_frame_count, ahead_frame_length, ahead_frame_count, extender_frame_count;
    int min_id;
    
    SOURCE_DATA_ARY_t new_sources;
    SOURCE_DATA_ARY_t *cur_sources;
    ////SOURCE_DATA_ARY_t remaining_sources;
    
    // for SourceTracker algorithm
    SOURCE_t *last_src;
    SOURCE_t *cur_src;
    SOURCE_t *new_src;
    // for SourceIntervalExtender algorithm
    SOURCE_t *ecur_src;
    SOURCE_t *enew_src;
    SOURCE_t *enew_src;


    float power_tracking[18*2]; //remaining_sources.data[18*2].power
    float cos_theta, max_cos_theta, near_angle_cos_theta; //, angle, min_angle;
    int   distance, min_distance; // for TFINDEX
    float Thresh;
    int   PauseLength;
    int   RemoveLength;
    int   RemoveCheckLength;
    int   CompareMode;
    float MinSourceInterval;
    int   MinTFIndexInterval;
    int   PrerollLength;
    int   MaxNumSource;
    int   RemoveSourceAlgorithm;
    
    /* 設定されているパラメータをロード */
    Thresh                = extended_source_tracker_info.thresh;
    PauseLength           = extended_source_tracker_info.pause_length;
    RemoveLength          = extended_source_tracker_info.remove_length;
    RemoveCheckLength     = extended_source_tracker_info.remove_check_length;
    CompareMode           = extended_source_tracker_info.compare_mode;
    PrerollLength         = extended_source_tracker_info.preroll_length;
    MaxNumSource          = extended_source_tracker_info.max_num_source;
    RemoveSourceAlgorithm = extended_source_tracker_info.remove_source_algorithm;

    //デバック
    //flowdesignerのカウントと合わせるため-50(定位の間隔)して表示
    //LOGI("[%d] last_sources.source_num = %d, cur_source_num = %d\n", (frame_count-50), last_sources.source_num, sources->source_num);
    //LOGI("[%d]\n", (frame_count-50));
    
    sub_frame_count    = sources->count;
    ahead_frame_length = PrerollLength + RemoveCheckLength;
    ahead_frame_count  = sub_frame_count + ahead_frame_length;
    cur_sources = sources;
    
    ////for(i=0; i<cur_sources->source_num; i++){
    ////    remaining_sources.data[i].x[0]  = cur_sources->data[i].x[0];
    ////    remaining_sources.data[i].x[1]  = cur_sources->data[i].x[1];
    ////    remaining_sources.data[i].x[2]  = cur_sources->data[i].x[2];
    ////    remaining_sources.data[i].power = cur_sources->data[i].power;
    ////    remaining_sources.data[i].id    = cur_sources->data[i].id; //no need
    ////    remaining_sources.data[i].remaining = 0; //no need
    ////}
    ////remaining_sources.source_num = cur_sources->source_num;
    for(i=cur_sources->source_num - 1; i>=0; i--){ power_tracking[i] = cur_sources->data[i].power; }

    if(CompareMode == 0){ // DEG
        MinSourceInterval  = source_tracker_info.min_src_interval;
        near_angle_cos_theta = cos(MinSourceInterval * M_PI / 180.0);
        
        index = 0; //index is new_sources.source_num
        for(i=0; i<last_sources.source_num; i++){
            last_src = &last_sources.data[i];
            if(last_src->remaining < 0){ /*LOGI("[%d] Source %d is removed.\n", frame_count, last_src->id);*/ continue; }
            ////min_angle = 100;
            max_cos_theta = -1.0f; // max acos(-1)==180[deg] //// << DEG diff >>
            min_id = 0;
            ////for(j=0; j<remaining_sources.source_num; j++){
            for(j=0; j<cur_sources->source_num; j++){
                ////cur_src = &remaining_sources.data[j];
                ////if(cur_src->power < Thresh){ continue; }
                if(power_tracking[j] < Thresh){ continue; }
                cur_src = &cur_sources->data[j];
                //// << DEG diff >> start
                ////angle = SourceAngle(cur_src->x, last_src->x);
                ////if(angle < min_angle){ min_angle = angle; min_id = j; }
                cos_theta = SourceCosTheta(cur_src->x, last_src->x);
                if(cos_theta > max_cos_theta){ max_cos_theta = cos_theta; min_id = j; }
                //// << DEG diff >> end
            }
            ////if(max_cos_theta > 1.0f){ max_cos_theta = 1.0f; } //no need
            
            ////index = new_sources.source_num;
            ////if(min_angle < near_angle){ // near_angle=(MinSourceInterval*M_PI/180.0) /* 角度が近ければ統合 */ //// << DEG diff >>
            if(max_cos_theta > near_angle_cos_theta){ // near_angle_cos_theta=cos(MinSourceInterval*M_PI/180.0) /* 角度が近ければ統合 */ //// << DEG diff >>
                ////new_sources.data[index].x[0]      = remaining_sources.data[min_id].x[0];
                ////new_sources.data[index].x[1]      = remaining_sources.data[min_id].x[1];
                ////new_sources.data[index].x[2]      = remaining_sources.data[min_id].x[2];
                ////new_sources.data[index].power     = remaining_sources.data[min_id].power;
                new_sources.data[index].x[0]             = cur_sources->data[min_id].x[0];
                new_sources.data[index].x[1]             = cur_sources->data[min_id].x[1];
                new_sources.data[index].x[2]             = cur_sources->data[min_id].x[2];
                new_sources.data[index].power            = cur_sources->data[min_id].power;
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = last_src->count;
                new_sources.data[index].last_found_count = ahead_frame_count;
                new_sources.data[index].tfindex[0]       = cur_sources->data[min_id].tfindex[0];
                new_sources.data[index].tfindex[1]       = cur_sources->data[min_id].tfindex[1];
                new_sources.data[index].tfindex[2]       = cur_sources->data[min_id].tfindex[2];
                new_sources.data[index].tfindex_max[0]   = cur_sources->data[min_id].tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = cur_sources->data[min_id].tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = cur_sources->data[min_id].tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = cur_sources->data[min_id].tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = cur_sources->data[min_id].tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = cur_sources->data[min_id].tfindex_min[2];
                /* 統合したので現在のリストからは削除（powerを0にすることで次の処理で扱われない） */
                ////remaining_sources.data[min_id].power = 0.0f;
                power_tracking[min_id] = 0.0f;
                cur_sources->data[min_id].power = 0.0f;
            }
            else{
                /* 統合する音源がないのでremainingを減らす */
                new_sources.data[index].x[0]             = last_src->x[0];
                new_sources.data[index].x[1]             = last_src->x[1];
                new_sources.data[index].x[2]             = last_src->x[2];
                new_sources.data[index].power            = last_src->power;
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = last_src->remaining - 1;
                new_sources.data[index].count            = last_src->count;
                new_sources.data[index].last_found_count = last_src->last_found_count;
                new_sources.data[index].tfindex[0]       = last_src->tfindex[0];
                new_sources.data[index].tfindex[1]       = last_src->tfindex[1];
                new_sources.data[index].tfindex[2]       = last_src->tfindex[2];
                new_sources.data[index].tfindex_max[0]   = last_src->tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = last_src->tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = last_src->tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = last_src->tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = last_src->tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = last_src->tfindex_min[2];
            }
            //LOGI("id=%d, remaining=%g\n", new_sources.data[index].id, new_sources.data[index].remaining);
            index++; //index is new_sources.source_num
        }
        
        for(i=0; i<cur_sources->source_num; i++){
            ////cur_src = &remaining_sources.data[i];
            //LOGI("i=%d : pow=%g source_num=%d\n", i, cur_src->power, new_sources.source_num);
            ////if(cur_src->power < Thresh){ continue; }
            if(power_tracking[i] < Thresh){ continue; }
            cur_src = &cur_sources->data[i];
            ////min_angle = 100; //// << DEG diff >>
            max_cos_theta = -1.0f; // max cos(-1)==180[deg] //// << DEG diff >>
            for(j=0; j<index; j++){ //index is new_sources.source_num
                new_src = &new_sources.data[j];
                //// << DEG diff >> start
                ////angle = SourceAngle(new_src->x, cur_src->x);
                ////if(angle < min_angle){ min_angle = angle; min_id = j; }
                cos_theta = SourceCosTheta(new_src->x, cur_src->x);
                if(cos_theta > max_cos_theta){ max_cos_theta = cos_theta; }
                //// << DEG diff >> end
            }
            ////if(max_cos_theta > 1.0f){ max_cos_theta = 1.0f; } //no need
            
            ////index = new_sources.source_num;
            ////if(min_angle >= near_angle){ // near_angle=(MinSourceInterval*M_PI/180.0) //// << DEG diff >>
            if(max_cos_theta < near_angle_cos_theta){ // near_angle_cos_theta=cos(MinSourceInterval*M_PI/180.0) //// << DEG diff >>
                //LOGI("[%d] Source %d is created.\n", frame_count, next_source_id);
                new_sources.data[index].x[0]             = cur_src->x[0];
                new_sources.data[index].x[1]             = cur_src->x[1];
                new_sources.data[index].x[2]             = cur_src->x[2];
                new_sources.data[index].power            = cur_src->power;
                new_sources.data[index].id               = next_source_id++;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = ahead_frame_count;
                new_sources.data[index].last_found_count = ahead_frame_count;
                new_sources.data[index].tfindex[0]       = cur_src->tfindex[0];
                new_sources.data[index].tfindex[1]       = cur_src->tfindex[1];
                new_sources.data[index].tfindex[2]       = cur_src->tfindex[2];
                new_sources.data[index].tfindex_max[0]   = cur_src->tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = cur_src->tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = cur_src->tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = cur_src->tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = cur_src->tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = cur_src->tfindex_min[2];
                index++; //index is new_sources.source_num
            }
        }
    }
    else{ // CompareMode == 1 : TFINDEX
        MinTFIndexInterval = source_tracker_info.min_tfindex_interval;
        
        index = 0; //index is new_sources.source_num
        for(i=0; i<last_sources.source_num; i++){
            last_src = &last_sources.data[i];
            if(last_src->remaining < 0){ /*LOGI("[%d] Source %d is removed.\n", frame_count, last_src->id);*/ continue; }
            min_distance = 0x7fffffff; //// << TFINDEX diff >>
            min_id = 0;
            ////for(j=0; j<remaining_sources.source_num; j++){
            for(j=0; j<cur_sources->source_num; j++){
                ////cur_src = &remaining_sources.data[j];
                ////if(cur_src->power < Thresh){ continue; }
                if(power_tracking[j] < Thresh){ continue; }
                cur_src = &cur_sources->data[j];
                //// << TFINDEX diff >> start
                distance = SourceTfindexDistance(cur_src, last_src);
                if(distance < min_distance){ min_distance = distance; min_id = j; }
                //// << TFINDEX diff >> end
            }
            
            ////index = new_sources.source_num;
            if(min_distance < MinTFInterval){ // /* TFINDEXが近ければ統合 */ //// << TFINDEX diff >>
                ////new_sources.data[index].x[0]      = remaining_sources.data[min_id].x[0];
                ////new_sources.data[index].x[1]      = remaining_sources.data[min_id].x[1];
                ////new_sources.data[index].x[2]      = remaining_sources.data[min_id].x[2];
                ////new_sources.data[index].power     = remaining_sources.data[min_id].power;
                new_sources.data[index].x[0]             = cur_sources->data[min_id].x[0];
                new_sources.data[index].x[1]             = cur_sources->data[min_id].x[1];
                new_sources.data[index].x[2]             = cur_sources->data[min_id].x[2];
                new_sources.data[index].power            = cur_sources->data[min_id].power;
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = last_src->count;
                new_sources.data[index].last_found_count = ahead_frame_count;
                new_sources.data[index].tfindex[0]       = cur_sources->data[min_id].tfindex[0];
                new_sources.data[index].tfindex[1]       = cur_sources->data[min_id].tfindex[1];
                new_sources.data[index].tfindex[2]       = cur_sources->data[min_id].tfindex[2];
                new_sources.data[index].tfindex_max[0]   = cur_sources->data[min_id].tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = cur_sources->data[min_id].tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = cur_sources->data[min_id].tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = cur_sources->data[min_id].tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = cur_sources->data[min_id].tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = cur_sources->data[min_id].tfindex_min[2];
                /* 統合したので現在のリストからは削除（powerを0にすることで次の処理で扱われない） */
                ////remaining_sources.data[min_id].power = 0.0f;
                power_tracking[min_id] = 0.0f;
                cur_sources->data[min_id].power = 0.0f;
            }
            else{
                /* 統合する音源がないのでremainingを減らす */
                new_sources.data[index].x[0]             = last_src->x[0];
                new_sources.data[index].x[1]             = last_src->x[1];
                new_sources.data[index].x[2]             = last_src->x[2];
                new_sources.data[index].power            = last_src->power;
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = last_src->remaining - 1;
                new_sources.data[index].count            = last_src->count;
                new_sources.data[index].last_found_count = last_src->last_found_count;
                new_sources.data[index].tfindex[0]       = last_src->data[min_id].tfindex[0];
                new_sources.data[index].tfindex[1]       = last_src->data[min_id].tfindex[1];
                new_sources.data[index].tfindex[2]       = last_src->data[min_id].tfindex[2];
                new_sources.data[index].tfindex_max[0]   = last_src->data[min_id].tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = last_src->data[min_id].tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = last_src->data[min_id].tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = last_src->data[min_id].tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = last_src->data[min_id].tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = last_src->data[min_id].tfindex_min[2];
            }
            //LOGI("id=%d, remaining=%g\n", new_sources.data[index].id, new_sources.data[index].remaining);
            index++; //index is new_sources.source_num
        }
        
        for(i=0; i<cur_sources->source_num; i++){
            ////cur_src = &remaining_sources.data[i];
            //LOGI("i=%d : pow=%g source_num=%d\n", i, cur_src->power, new_sources.source_num);
            ////if(cur_src->power < Thresh){ continue; }
            if(power_tracking[i] < Thresh){ continue; }
            cur_src = &cur_sources->data[i];
            min_distance = 0x7fffffff; //// << TFINDEX diff >>
            for(j=0; j<index; j++){ //index is new_sources.source_num
                new_src = &new_sources.data[j];
                //// << TFINDEX diff >> start
                distance = SourceTfindexDistance(new_src, cur_src);
                if(distance < min_distance){ min_distance = distance; }
                //// << TFINDEX diff >> end
            }
            
            ////index = new_sources.source_num;
            if(min_distance >= MinTFIndexInterval){ //// << TFINDEX diff >>
                //LOGI("[%d] Source %d is created.\n", frame_count, next_source_id);
                new_sources.data[index].x[0]             = cur_src->x[0];
                new_sources.data[index].x[1]             = cur_src->x[1];
                new_sources.data[index].x[2]             = cur_src->x[2];
                new_sources.data[index].power            = cur_src->power;
                new_sources.data[index].id               = next_source_id++;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = ahead_frame_count;
                new_sources.data[index].last_found_count = ahead_frame_count;
                new_sources.data[index].tfindex[0]       = cur_src->tfindex[0];
                new_sources.data[index].tfindex[1]       = cur_src->tfindex[1];
                new_sources.data[index].tfindex[2]       = cur_src->tfindex[2];
                new_sources.data[index].tfindex_max[0]   = cur_src->tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = cur_src->tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = cur_src->tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = cur_src->tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = cur_src->tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = cur_src->tfindex_min[2];
                index++; //index is new_sources.source_num
            }
        }
    }
    
    // DEG/TFINDEX 共通
    new_sources.source_num = index;
    
    for(i=0; i<new_sources.source_num; i++){
        //保存
        last_sources.data[i].x[0]             = new_sources.data[i].x[0];
        last_sources.data[i].x[1]             = new_sources.data[i].x[1];
        last_sources.data[i].x[2]             = new_sources.data[i].x[2];
        last_sources.data[i].power            = new_sources.data[i].power;
        last_sources.data[i].id               = new_sources.data[i].id;
        last_sources.data[i].remaining        = new_sources.data[i].remaining;
        last_sources.data[i].count            = new_sources.data[i].count;
        last_sources.data[i].last_found_count = new_sources.data[i].last_found_count;
        last_sources.data[i].tfindex[0]       = new_sources.data[i].tfindex[0];
        last_sources.data[i].tfindex[1]       = new_sources.data[i].tfindex[1];
        last_sources.data[i].tfindex[2]       = new_sources.data[i].tfindex[2];
        last_sources.data[i].tfindex_max[0]   = new_sources.data[i].tfindex_max[0];
        last_sources.data[i].tfindex_max[1]   = new_sources.data[i].tfindex_max[1];
        last_sources.data[i].tfindex_max[2]   = new_sources.data[i].tfindex_max[2];
        last_sources.data[i].tfindex_min[0]   = new_sources.data[i].tfindex_min[0];
        last_sources.data[i].tfindex_min[1]   = new_sources.data[i].tfindex_min[1];
        last_sources.data[i].tfindex_min[2]   = new_sources.data[i].tfindex_min[2];
        last_sources.data[i].mask             = 0xffff; //dummy flag of all enable
    }
    last_sources.source_num = new_sources.source_num;
    //last_sources.count      = frame_count;
    last_sources.count      = sub_frame_count;
    
    //////// SourceTracker's new_sources ==> last_sources
    
    int extender_count;
    int add_flag;
    
    frame_count = sub_frame_count;
    if(frame_count < 0){
        return -1;
    }
    
    /* 現在位置に追加 */
    HABufferSetSourceData(sub_frame_count, &last_sources);
    
    extender_frame_count = sub_frame_count - ahead_frame_count;
    if(extender_frame_count < 0){
        return -1;
    }
    
    if(PrerollLength != 0){
        /* 遡った位置に追加 */
        if(last_sources.source_num > 0){
            cur_sources = HABufferGetSourceData(extender_frame_count);
            
            for(i=0; i<cur_sources->source_num; i++){
                new_sources.data[i] = cur_sources->data[i];
            }
            new_sources.source_num = cur_sources->source_num;
            
            for(i=0; i<last_sources.source_num; i++){
                add_flag = 1;
                for(j=0; j<cur_sources->source_num; j++){
                    if(last_sources.data[i].id == cur_sources->data[j].id){
                        add_flag = 0; break; // TAKIGAHIRA : break追加
                    }
                }
                if(add_flag){
                    new_sources.data[new_sources.source_num] = last_sources.data[i];
                    new_sources.source_num++;
                }
            }
            HABufferSetSourceData(extender_frame_count, &new_sources);
        }
        
        /* 改めて遡った位置のデータを返す */
        cur_sources = HABufferGetSourceData(extender_frame_count);
        for(i=0; i<cur_sources->source_num; i++){
            sources->data[i] = cur_sources->data[i];
        }
        sources->count = cur_sources->count;
        sources->source_num = cur_sources->source_num;
        //sources->count -= source_interval_extender_info.preroll_length;
    }
    
    if(RemoveCheckLength){
    
    
    
    
    for(i=0; i<new_sources.source_num; i++){
        //現在情報を更新（コピーバック）
        sources->data[i].x[0]             = new_sources.data[i].x[0];
        sources->data[i].x[1]             = new_sources.data[i].x[1];
        sources->data[i].x[2]             = new_sources.data[i].x[2];
        sources->data[i].power            = new_sources.data[i].power;
        sources->data[i].id               = new_sources.data[i].id;
        sources->data[i].remaining        = new_sources.data[i].remaining;
        sources->data[i].count            = new_sources.data[i].count;
        sources->data[i].last_found_count = new_sources.data[i].last_found_count;
        sources->data[i].tfindex[0]       = new_sources.data[i].tfindex[0];
        sources->data[i].tfindex[1]       = new_sources.data[i].tfindex[1];
        sources->data[i].tfindex[2]       = new_sources.data[i].tfindex[2];
        sources->data[i].tfindex_max[0]   = new_sources.data[i].tfindex_max[0];
        sources->data[i].tfindex_max[1]   = new_sources.data[i].tfindex_max[1];
        sources->data[i].tfindex_max[2]   = new_sources.data[i].tfindex_max[2];
        sources->data[i].tfindex_min[0]   = new_sources.data[i].tfindex_min[0];
        sources->data[i].tfindex_min[1]   = new_sources.data[i].tfindex_min[1];
        sources->data[i].tfindex_min[2]   = new_sources.data[i].tfindex_min[2];
        sources->data[i].mask             = 0xffff; //dummy flag of all enable
        
        //保存
        last_sources.data[i].x[0]             = new_sources.data[i].x[0];
        last_sources.data[i].x[1]             = new_sources.data[i].x[1];
        last_sources.data[i].x[2]             = new_sources.data[i].x[2];
        last_sources.data[i].power            = new_sources.data[i].power;
        last_sources.data[i].id               = new_sources.data[i].id;
        last_sources.data[i].remaining        = new_sources.data[i].remaining;
        last_sources.data[i].count            = new_sources.data[i].count;
        last_sources.data[i].last_found_count = new_sources.data[i].last_found_count;
        last_sources.data[i].tfindex[0]       = new_sources.data[i].tfindex[0];
        last_sources.data[i].tfindex[1]       = new_sources.data[i].tfindex[1];
        last_sources.data[i].tfindex[2]       = new_sources.data[i].tfindex[2];
        last_sources.data[i].tfindex_max[0]   = new_sources.data[i].tfindex_max[0];
        last_sources.data[i].tfindex_max[1]   = new_sources.data[i].tfindex_max[1];
        last_sources.data[i].tfindex_max[2]   = new_sources.data[i].tfindex_max[2];
        last_sources.data[i].tfindex_min[0]   = new_sources.data[i].tfindex_min[0];
        last_sources.data[i].tfindex_min[1]   = new_sources.data[i].tfindex_min[1];
        last_sources.data[i].tfindex_min[2]   = new_sources.data[i].tfindex_min[2];
        last_sources.data[i].mask             = 0xffff; //dummy flag of all enable
    }
    sources->source_num     = new_sources.source_num;
    //sources->count          = frame_count;
    sources->count          = sub_frame_count;
    last_sources.source_num = new_sources.source_num;
    //last_sources.count      = frame_count;
    last_sources.count      = sub_frame_count;
    
#if 0
    {
        SOURCE_t *src;
        for(i=0; i<new_sources.source_num; i++){
            src = &new_sources.data[i];
            fprintf(stdout, "ID: %d\n power: %g x: %g %g %g, remining = %g\n",
                    src->id, src->power, src->x[0], src->x[1], src->x[2], src->remaining);
        }
        fprintf(stdout, "\n");
    }
#endif
    return 0;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/************************************************************
**
**  Public  :
**
**  History : 
**            2013.03.29 K.Takagi 新規追加（マージ）
**
**  Description
**  -----------
**  トレース処理
**
*/
int SourceTrackerWithFilter(int frame_count, SOURCE_DATA_ARY_t *sources, SOURCE_DATA_ARY_t *target)
{
    int i, j;
    int index;
    int sub_frame_count, ahead_frame_length, ahead_frame_count;
    int min_id;

    SOURCE_DATA_ARY_t new_sources;
    SOURCE_DATA_ARY_t *cur_sources;
    ////SOURCE_DATA_ARY_t remaining_sources;

    // for SourceTracker algorithm
    SOURCE_t *last_src;
    SOURCE_t *cur_src;
    SOURCE_t *new_src;
    SOURCE_t *target_src;
    // for SourceIntervalExtender algorithm
    SOURCE_t *ecur_src;
    SOURCE_t *enew_src;
    SOURCE_t *enew_src;

    float power_tracking[18*2]; //remaining_sources.data[18*2].power
    float cos_theta, max_cos_theta, near_angle_cos_theta; //, angle, min_angle;
    int   distance, min_distance; // for TFINDEX
    float Thresh;
    int   PauseLength;
    int   RemoveLength;
    int   RemoveCheckLength;
    int   CompareMode;
    float MinSourceInterval;
    int   MinTFIndexInterval;
    int   PrerollLength;
    int   MaxNumSource;
    int   RemoveSourceAlgorithm;

    /* 設定されているパラメータをロード */
    Thresh                = extended_source_tracker_info.thresh;
    PauseLength           = extended_source_tracker_info.pause_length;
    RemoveLength          = extended_source_tracker_info.remove_length;
    RemoveCheckLength     = extended_source_tracker_info.remove_check_length;
    CompareMode           = extended_source_tracker_info.compare_mode;
    PrerollLength         = extended_source_tracker_info.preroll_length;
    MaxNumSource          = extended_source_tracker_info.max_num_source;
    RemoveSourceAlgorithm = extended_source_tracker_info.remove_source_algorithm;

    //デバック
    //flowdesignerのカウントと合わせるため-50(定位の間隔)して表示
    //LOGI("[%d] last_sources.source_num = %d, cur_source_num = %d\n", (frame_count-50), last_sources.source_num, sources->source_num);
    //LOGI("[%d]\n", (frame_count-50));
    
    sub_frame_count = sources->count;
    ahead_frame_length = PrerollLength + RemoveCheckLength;
    ahead_frame_count  = sub_frame_count + ahead_frame_length;
    cur_sources = sources;
    
    ////for(i=0; i<cur_sources->source_num; i++){
    ////    remaining_sources.data[i].x[0]  = cur_sources->data[i].x[0];
    ////    remaining_sources.data[i].x[1]  = cur_sources->data[i].x[1];
    ////    remaining_sources.data[i].x[2]  = cur_sources->data[i].x[2];
    ////    remaining_sources.data[i].power = cur_sources->data[i].power;
    ////    remaining_sources.data[i].id    = cur_sources->data[i].id; //no need
    ////    remaining_sources.data[i].remaining = 0; //no need
    ////}
    ////remaining_sources.source_num = cur_sources->source_num;
    for(i=cur_sources->source_num - 1; i>=0; i--){ power_tracking[i] = cur_sources->data[i].power; }

    if(CompareMode == 0){ // DEG
        MinSourceInterval = source_tracker_info.min_src_interval;
        near_angle_cos_theta = cos(MinSourceInterval * M_PI / 180.0);
        
        if(target_sources.source_num == 0){
            for(i=0; i<target->source_num; i++){
                target_sources.data[i].x[0]             = target->data[i].x[0];
                target_sources.data[i].x[1]             = target->data[i].x[1];
                target_sources.data[i].x[2]             = target->data[i].x[2];
                //target_sources.data[i].power            = target->data[i].power;            // xしか使わない
                //target_sources.data[i].id               = target->data[i].id;               // xしか使わない
                //target_sources.data[i].remaining        = 0;                                // xしか使わない
                //target_sources.data[i].count            = target->data[i].count;            // xしか使わない
                //target_sources.data[i].last_found_count = target->data[i].last_found_count; // xしか使わない
                //target_sources.data[i].tfindex[0]       = target->data[i].tfindex[0];       // xしか使わない
                //target_sources.data[i].tfindex[1]       = target->data[i].tfindex[1];       // xしか使わない
                //target_sources.data[i].tfindex[2]       = target->data[i].tfindex[2];       // xしか使わない
                //target_sources.data[i].tfindex_max[0]   = target->data[i].tfindex_max[0];   // xしか使わない
                //target_sources.data[i].tfindex_max[1]   = target->data[i].tfindex_max[1];   // xしか使わない
                //target_sources.data[i].tfindex_max[2]   = target->data[i].tfindex_max[2];   // xしか使わない
                //target_sources.data[i].tfindex_min[0]   = target->data[i].tfindex_min[0];   // xしか使わない
                //target_sources.data[i].tfindex_min[1]   = target->data[i].tfindex_min[1];   // xしか使わない
                //target_sources.data[i].tfindex_min[2]   = target->data[i].tfindex_min[2];   // xしか使わない
                target_sources.data[i].mask             = target->data[i].mask;
            }
            target_sources.source_num = target->source_num;
        }
        
        index = 0; //index is new_sources.source_num
        for(i=0; i<last_sources.source_num; i++){
            last_src = &last_sources.data[i];
            if(last_src->remaining < 0){ /*LOGI("[%d] Source %d is removed.\n", frame_count, last_src->id);*/ continue; }
            ////min_angle = 100;
            max_cos_theta = -1.0f; // max cos(-1)==180[deg] //// << DEG diff >>
            min_id = 0;
            ////for(j=0; j<remaining_sources.source_num; j++){
            for(j=0; j<cur_sources->source_num; j++){
                ////cur_src = &remaining_sources.data[j];
                ////if(cur_src->power < Thresh){ continue; }
                if(power_tracking[j] < Thresh){ continue; }
                cur_src = &cur_sources->data[j];
                //// << DEG diff >> start
                ////angle = SourceAngle(cur_src->x, last_src->x);
                ////if(angle < min_angle){ min_angle = angle; min_id = j; }
                cos_theta = SourceCosTheta(cur_src->x, last_src->x);
                if(cos_theta > max_cos_theta){ max_cos_theta = cos_theta; min_id = j; }
                //// << DEG diff >> end
            }
            ////if(max_cos_theta > 1.0f){ max_cos_theta = 1.0f; } //no need
            
            ////index = new_sources.source_num;
            ////if(min_angle < near_angle){ // near_angle=(MinSourceInterval*M_PI/180.0) /* 角度が近ければ統合 */ //// << DEG diff >>
            if(max_cos_theta > near_angle_cos_theta){ // near_angle_cos_theta=cos(MinSourceInterval*M_PI/180.0) /* 角度が近ければ統合 */ //// << DEG diff >>
                ////new_sources.data[index].x[0]      = remaining_sources.data[min_id].x[0];
                ////new_sources.data[index].x[1]      = remaining_sources.data[min_id].x[1];
                ////new_sources.data[index].x[2]      = remaining_sources.data[min_id].x[2];
                ////new_sources.data[index].power     = remaining_sources.data[min_id].power;
                new_sources.data[index].x[0]             = cur_sources->data[min_id].x[0];
                new_sources.data[index].x[1]             = cur_sources->data[min_id].x[1];
                new_sources.data[index].x[2]             = cur_sources->data[min_id].x[2];
                new_sources.data[index].power            = cur_sources->data[min_id].power;
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = last_src->count;
                new_sources.data[index].last_found_count = ahead_frame_count;
                new_sources.data[index].tfindex[0]       = cur_sources->data[min_id].tfindex[0];
                new_sources.data[index].tfindex[1]       = cur_sources->data[min_id].tfindex[1];
                new_sources.data[index].tfindex[2]       = cur_sources->data[min_id].tfindex[2];
                new_sources.data[index].tfindex_max[0]   = cur_sources->data[min_id].tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = cur_sources->data[min_id].tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = cur_sources->data[min_id].tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = cur_sources->data[min_id].tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = cur_sources->data[min_id].tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = cur_sources->data[min_id].tfindex_min[2];
                new_sources.data[index].mask             = last_src->mask;
                /* 統合したので現在のリストからは削除（powerを0にすることで次の処理で扱われない） */
                ////remaining_sources.data[min_id].power = 0.0f;
                power_tracking[min_id] = 0.0f;
                cur_sources->data[min_id].power = 0.0f;
            }
            else{
                /* 統合する音源がないのでremainingを減らす */
                new_sources.data[index].x[0]             = last_src->x[0];
                new_sources.data[index].x[1]             = last_src->x[1];
                new_sources.data[index].x[2]             = last_src->x[2];
                new_sources.data[index].power            = last_src->power;
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = last_src->remaining - 1;
                new_sources.data[index].count            = last_src->count;
                new_sources.data[index].last_found_count = last_src->last_found_count;
                new_sources.data[index].tfindex[0]       = last_src->tfindex[0];
                new_sources.data[index].tfindex[1]       = last_src->tfindex[1];
                new_sources.data[index].tfindex[2]       = last_src->tfindex[2];
                new_sources.data[index].tfindex_max[0]   = last_src->tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = last_src->tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = last_src->tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = last_src->tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = last_src->tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = last_src->tfindex_min[2];
                new_sources.data[index].mask             = last_src->mask;
            }
            //LOGI("id=%d, remaining=%g\n", new_sources.data[index].id, new_sources.data[index].remaining);
            index++; //index is new_sources.source_num
        }
        
        for(i=0; i<cur_sources->source_num; i++){
            ////cur_src = &remaining_sources.data[i];
            //LOGI("i=%d : pow=%g source_num=%d\n", i, cur_src->power, new_sources.source_num);
            ////if(cur_src->power < Thresh){ continue; }
            if(power_tracking[i] < Thresh){ continue; }
            cur_src = &cur_sources->data[i];
            ////min_angle = 100; //// << DEG diff >>
            max_cos_theta = -1.0f; // max acos(-1)==180[deg] //// << DEG diff >>
            min_id = 0; //for use to backup mask
            for(j=0; j<target_sources.source_num; j++){
                target_src = &target_sources.data[j];
                //// << DEG diff >> start
                cos_theta = SourceCosTheta(cur_src->x, target_src->x);
                if(cos_theta > max_cos_theta){ max_cos_theta = cos_theta; min_id = target_src->mask; }
                //// << DEG diff >> end
            }
            if(min_id == 0 || max_cos_theta < near_angle_cos_theta){ continue; } //out of target //// << DEG diff >>
            max_cos_theta = -1.0f; // max acos(-1)==180[deg] //// << DEG diff >>
            for(j=0; j<index; j++){ //index is new_sources.source_num
                new_src = &new_sources.data[j];
                //// << DEG diff >> start
                ////angle = SourceAngle(new_src->x, cur_src->x);
                ////if(angle < min_angle){ min_angle = angle; min_id = j; }
                cos_theta = SourceCosTheta(new_src->x, cur_src->x);
                if(cos_theta > max_cos_theta){ max_cos_theta = cos_theta; }
                //// << DEG diff >> end
            }
            ////if(max_cos_theta > 1.0f){ max_cos_theta = 1.0f; } //no need
            
            ////index = new_sources.source_num;
            ////if(min_angle > near_angle){ // near_angle=(MinSourceInterval*M_PI/180.0) //// << DEG diff >>
            if(max_cos_theta < near_angle_cos_theta){ // near_angle_cos_theta=cos(MinSourceInterval*M_PI/180.0) //// << DEG diff >>
                //LOGI("[%d] Source %d is created.\n", frame_count, next_source_id);
                //角度がフィルタ対象角近傍かつ継続中のIDから離れている場合は対象角のmask情報を付与して新規IDで定位を追加
                //new_sources.data[index].x[0]             = target_src->x[0]; // Overwrite x[0] for filter direction
                //new_sources.data[index].x[1]             = target_src->x[1]; // Overwrite x[1] for filter direction
                //new_sources.data[index].x[2]             = target_src->x[2]; // Overwrite x[2] for filter direction
                new_sources.data[index].x[0]             = cur_src->x[0];
                new_sources.data[index].x[1]             = cur_src->x[1];
                new_sources.data[index].x[2]             = cur_src->x[2];
                new_sources.data[index].power            = cur_src->power;
                new_sources.data[index].id               = next_source_id++;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = ahead_frame_count;
                new_sources.data[index].last_found_count = ahead_frame_count;
                new_sources.data[index].tfindex[0]       = cur_src->tfindex[0];
                new_sources.data[index].tfindex[1]       = cur_src->tfindex[1];
                new_sources.data[index].tfindex[2]       = cur_src->tfindex[2];
                new_sources.data[index].tfindex_max[0]   = cur_src->tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = cur_src->tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = cur_src->tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = cur_src->tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = cur_src->tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = cur_src->tfindex_min[2];
                new_sources.data[index].mask             = min_id; //target_src->mask;
                index++; //index is new_sources.source_num
            }
        }
    }
    else{ // CompareMode == 1 : TFINDEX
        MinTFIndexInterval = source_tracker_info.min_tfindex_interval;
        
        if(target_sources.source_num == 0){
            for(i=0; i<target->source_num; i++){
                //target_sources.data[i].x[0]             = target->data[i].x[0];             // tfindexしか使わない
                //target_sources.data[i].x[1]             = target->data[i].x[1];             // tfindexしか使わない
                //target_sources.data[i].x[2]             = target->data[i].x[2];             // tfindexしか使わない
                //target_sources.data[i].power            = target->data[i].power;            // tfindexしか使わない
                //target_sources.data[i].id               = target->data[i].id;               // tfindexしか使わない
                //target_sources.data[i].remaining        = 0;                                // tfindexしか使わない
                //target_sources.data[i].count            = target->data[i].count;            // tfindexしか使わない
                //target_sources.data[i].last_found_count = target->data[i].last_found_count; // tfindexしか使わない
                target_sources.data[i].tfindex[0]       = target->data[i].tfindex[0];
                target_sources.data[i].tfindex[1]       = target->data[i].tfindex[1];
                target_sources.data[i].tfindex[2]       = target->data[i].tfindex[2];
                target_sources.data[i].tfindex_max[0]   = target->data[i].tfindex_max[0];
                target_sources.data[i].tfindex_max[1]   = target->data[i].tfindex_max[1];
                target_sources.data[i].tfindex_max[2]   = target->data[i].tfindex_max[2];
                target_sources.data[i].tfindex_min[0]   = target->data[i].tfindex_min[0];
                target_sources.data[i].tfindex_min[1]   = target->data[i].tfindex_min[1];
                target_sources.data[i].tfindex_min[2]   = target->data[i].tfindex_min[2];
                target_sources.data[i].mask             = target->data[i].mask;
            }
            target_sources.source_num = target->source_num;
        }
        
        index = 0; //index is new_sources.source_num
        for(i=0; i<last_sources.source_num; i++){
            last_src = &last_sources.data[i];
            if(last_src->remaining < 0){ /*LOGI("[%d] Source %d is removed.\n", frame_count, last_src->id);*/ continue; }
            min_distance = 0x7fffffff; //// << TFINDEX diff >>
            min_id = 0;
            ////for(j=0; j<remaining_sources.source_num; j++){
            for(j=0; j<cur_sources->source_num; j++){
                ////cur_src = &remaining_sources.data[j];
                ////if(cur_src->power < Thresh){ continue; }
                if(power_tracking[j] < Thresh){ continue; }
                cur_src = &cur_sources->data[j];
                //// << TFINDEX diff >> start
                distance = SourceTfindexDistance(cur_src, last_src);
                if(distance < min_distance){ min_distance = distance; min_id = j; }
                //// << TFINDEX diff >> end
            }
            
            ////index = new_sources.source_num;
            if(min_distance < MinTFInterval){ // /* TFINDEXが近ければ統合 */ //// << TFINDEX diff >>
                ////new_sources.data[index].x[0]      = remaining_sources.data[min_id].x[0];
                ////new_sources.data[index].x[1]      = remaining_sources.data[min_id].x[1];
                ////new_sources.data[index].x[2]      = remaining_sources.data[min_id].x[2];
                ////new_sources.data[index].power     = remaining_sources.data[min_id].power;
                new_sources.data[index].x[0]             = cur_sources->data[min_id].x[0];
                new_sources.data[index].x[1]             = cur_sources->data[min_id].x[1];
                new_sources.data[index].x[2]             = cur_sources->data[min_id].x[2];
                new_sources.data[index].power            = cur_sources->data[min_id].power;
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = last_src->count;
                new_sources.data[index].last_found_count = ahead_frame_count;
                new_sources.data[index].tfindex[0]       = cur_sources->data[min_id].tfindex[0];
                new_sources.data[index].tfindex[1]       = cur_sources->data[min_id].tfindex[1];
                new_sources.data[index].tfindex[2]       = cur_sources->data[min_id].tfindex[2];
                new_sources.data[index].tfindex_max[0]   = cur_sources->data[min_id].tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = cur_sources->data[min_id].tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = cur_sources->data[min_id].tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = cur_sources->data[min_id].tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = cur_sources->data[min_id].tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = cur_sources->data[min_id].tfindex_min[2];
                new_sources.data[index].mask             = last_src->mask;
                /* 統合したので現在のリストからは削除（powerを0にすることで次の処理で扱われない） */
                ////remaining_sources.data[min_id].power = 0.0f;
                power_tracking[min_id] = 0.0f;
                cur_sources->data[min_id].power = 0.0f;
            }
            else{
                /* 統合する音源がないのでremainingを減らす */
                new_sources.data[index].x[0]             = last_src->x[0];
                new_sources.data[index].x[1]             = last_src->x[1];
                new_sources.data[index].x[2]             = last_src->x[2];
                new_sources.data[index].power            = last_src->power;
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = last_src->remaining - 1;
                new_sources.data[index].count            = last_src->count;
                new_sources.data[index].last_found_count = last_src->last_found_count;
                new_sources.data[index].tfindex[0]       = last_src->data[min_id].tfindex[0];
                new_sources.data[index].tfindex[1]       = last_src->data[min_id].tfindex[1];
                new_sources.data[index].tfindex[2]       = last_src->data[min_id].tfindex[2];
                new_sources.data[index].tfindex_max[0]   = last_src->data[min_id].tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = last_src->data[min_id].tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = last_src->data[min_id].tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = last_src->data[min_id].tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = last_src->data[min_id].tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = last_src->data[min_id].tfindex_min[2];
                new_sources.data[index].mask             = last_src->mask;
            }
            //LOGI("id=%d, remaining=%g\n", new_sources.data[index].id, new_sources.data[index].remaining);
            index++; //index is new_sources.source_num
        }
        
        for(i=0; i<cur_sources->source_num; i++){
            ////cur_src = &remaining_sources.data[i];
            //LOGI("i=%d : pow=%g source_num=%d\n", i, cur_src->power, new_sources.source_num);
            ////if(cur_src->power < Thresh){ continue; }
            if(power_tracking[i] < Thresh){ continue; }
            cur_src = &cur_sources->data[i];
            min_distance = 0x7fffffff; //// << TFINDEX diff >>
            min_id = 0; //for use to backup mask
            for(j=0; j<target_sources.source_num; j++){
                target_src = &target_sources.data[j];
                //// << TFINDEX diff >> start
                distance = SourceTfindexDistance(cur_src, target_src);
                if(distance < min_distance){ min_distance = distance; min_id = target_src->mask; }
                //// << TFINDEX diff >> end
            }
            if(min_id == 0 || min_distance >= MinTFIndexInterval){ continue; } //out of target //// << TFINDEX diff >>
            min_distance = 0x7fffffff; //// << TFINDEX diff >>
            for(j=0; j<index; j++){ //index is new_sources.source_num
                new_src = &new_sources.data[j];
                //// << TFINDEX diff >> start
                distance = SourceTfindexDistance(new_src, cur_src);
                if(distance < min_distance){ min_distance = distance; }
                //// << TFINDEX diff >> end
            }
            
            ////index = new_sources.source_num;
            if(min_distance >= MinTFIndexInterval){ //// << TFINDEX diff >>
                //LOGI("[%d] Source %d is created.\n", frame_count, next_source_id);
                //角度がフィルタ対象角近傍かつ継続中のIDから離れている場合は対象角のmask情報を付与して新規IDで定位を追加
                //new_sources.data[index].x[0]             = target_src->x[0]; // Overwrite x[0] for filter direction
                //new_sources.data[index].x[1]             = target_src->x[1]; // Overwrite x[1] for filter direction
                //new_sources.data[index].x[2]             = target_src->x[2]; // Overwrite x[2] for filter direction
                new_sources.data[index].x[0]             = cur_src->x[0];
                new_sources.data[index].x[1]             = cur_src->x[1];
                new_sources.data[index].x[2]             = cur_src->x[2];
                new_sources.data[index].power            = cur_src->power;
                new_sources.data[index].id               = next_source_id++;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = ahead_frame_count;
                new_sources.data[index].last_found_count = ahead_frame_count;
                new_sources.data[index].tfindex[0]       = cur_src->tfindex[0];
                new_sources.data[index].tfindex[1]       = cur_src->tfindex[1];
                new_sources.data[index].tfindex[2]       = cur_src->tfindex[2];
                new_sources.data[index].tfindex_max[0]   = cur_src->tfindex_max[0];
                new_sources.data[index].tfindex_max[1]   = cur_src->tfindex_max[1];
                new_sources.data[index].tfindex_max[2]   = cur_src->tfindex_max[2];
                new_sources.data[index].tfindex_min[0]   = cur_src->tfindex_min[0];
                new_sources.data[index].tfindex_min[1]   = cur_src->tfindex_min[1];
                new_sources.data[index].tfindex_min[2]   = cur_src->tfindex_min[2];
                new_sources.data[index].mask             = min_id; //target_src->mask;
                index++; //index is new_sources.source_num
            }
        }
    }
    
    // DEG/TFINDEX 共通
    new_sources.source_num = index;
    
    for(i=0; i<new_sources.source_num; i++){
        //現在情報を更新（コピーバック）
        sources->data[i].x[0]             = new_sources.data[i].x[0];
        sources->data[i].x[1]             = new_sources.data[i].x[1];
        sources->data[i].x[2]             = new_sources.data[i].x[2];
        sources->data[i].power            = new_sources.data[i].power;
        sources->data[i].id               = new_sources.data[i].id;
        sources->data[i].remaining        = new_sources.data[i].remaining;
        sources->data[i].count            = new_sources.data[i].count;
        sources->data[i].last_found_count = new_sources.data[i].last_found_count;
        sources->data[i].tfindex[0]       = new_sources.data[i].tfindex[0];
        sources->data[i].tfindex[1]       = new_sources.data[i].tfindex[1];
        sources->data[i].tfindex[2]       = new_sources.data[i].tfindex[2];
        sources->data[i].tfindex_max[0]   = new_sources.data[i].tfindex_max[0];
        sources->data[i].tfindex_max[1]   = new_sources.data[i].tfindex_max[1];
        sources->data[i].tfindex_max[2]   = new_sources.data[i].tfindex_max[2];
        sources->data[i].tfindex_min[0]   = new_sources.data[i].tfindex_min[0];
        sources->data[i].tfindex_min[1]   = new_sources.data[i].tfindex_min[1];
        sources->data[i].tfindex_min[2]   = new_sources.data[i].tfindex_min[2];
        sources->data[i].mask             = new_sources.data[i].mask;
    
        //保存
        last_sources.data[i].x[0]             = new_sources.data[i].x[0];
        last_sources.data[i].x[1]             = new_sources.data[i].x[1];
        last_sources.data[i].x[2]             = new_sources.data[i].x[2];
        last_sources.data[i].power            = new_sources.data[i].power;
        last_sources.data[i].id               = new_sources.data[i].id;
        last_sources.data[i].remaining        = new_sources.data[i].remaining;
        last_sources.data[i].count            = new_sources.data[i].count;
        last_sources.data[i].last_found_count = new_sources.data[i].last_found_count;
        last_sources.data[i].tfindex[0]       = new_sources.data[i].tfindex[0];
        last_sources.data[i].tfindex[1]       = new_sources.data[i].tfindex[1];
        last_sources.data[i].tfindex[2]       = new_sources.data[i].tfindex[2];
        last_sources.data[i].tfindex_max[0]   = new_sources.data[i].tfindex_max[0];
        last_sources.data[i].tfindex_max[1]   = new_sources.data[i].tfindex_max[1];
        last_sources.data[i].tfindex_max[2]   = new_sources.data[i].tfindex_max[2];
        last_sources.data[i].tfindex_min[0]   = new_sources.data[i].tfindex_min[0];
        last_sources.data[i].tfindex_min[1]   = new_sources.data[i].tfindex_min[1];
        last_sources.data[i].tfindex_min[2]   = new_sources.data[i].tfindex_min[2];
        last_sources.data[i].mask             = new_sources.data[i].mask;
    }
    sources->source_num     = new_sources.source_num;
    //sources->count          = frame_count;
    sources->count          = sub_frame_count;
    last_sources.source_num = new_sources.source_num;
    //last_sources.count      = frame_count;
    last_sources.count      = sub_frame_count;
    
#if 0
    {
        SOURCE_t *src;
        for(i=0; i<new_sources.source_num; i++){
            src = &new_sources.data[i];
            fprintf(stdout, "ID: %d\n power: %g x: %g %g %g, remining = %g\n",
                    src->id, src->power, src->x[0], src->x[1], src->x[2], src->remaining);
        }
        fprintf(stdout, "\n");
    }
#endif
    return 0;
}
/************* 以下はSourceIntervalExtender *************/


/************************************************************
**
**  Public  : 
**
**  History : 2013.02.13 K.Takagi フレームカウントをsourcesに入っているもを利用するように修正
**            2013.02.05 K.Takagi 指定したフレーム数分前のキューに追加するように処理を修正
**                                引数のsourcesの内容を遡った位置の情報に更新するように修正
**            2012.12.15 K.Takagi 新規作成
**
**  Description
**  -----------
**  実際の処理
**
*/

int SourceIntervalExtender(int frame_count, SOURCE_DATA_ARY_t *sources)
{
    int sub_count;
    int add_flag;
    int index;
    int index2;
    SOURCE_DATA_ARY_t *src_ptr;
    SOURCE_DATA_ARY_t new_sources;
    
    frame_count = sources->count;
    if(frame_count < 0){
        return -1;
    }
    
    /* 現在位置に追加 */
    HABufferSetSourceData(sources->count, sources);
    
    sub_count = frame_count - source_interval_extender_info.preroll_length;
    if(sub_count < 0){
        return -1;
    }
    
    /* 遡った位置に追加 */
    if(sources->source_num > 0){
        src_ptr = HABufferGetSourceData(sub_count);
        
        for(index=0; index<src_ptr->source_num; index++){
            new_sources.data[index] = src_ptr->data[index];
        }
        new_sources.source_num = src_ptr->source_num;
        
        for(index=0; index<sources->source_num; index++){
            add_flag = 1;
            for(index2=0; index2<src_ptr->source_num; index2++){
                if(sources->data[index].id == src_ptr->data[index2].id){
                    add_flag = 0; break; // TAKIGAHIRA : break追加
                }
            }
            if(add_flag){
                new_sources.data[new_sources.source_num] = sources->data[index];
                new_sources.source_num++;
            }
        }
        HABufferSetSourceData(sub_count, &new_sources);
    }
    
    /* 改めて遡った位置のデータを返す */
    src_ptr = HABufferGetSourceData(sub_count);
    for(index=0; index<src_ptr->source_num; index++){
        sources->data[index] = src_ptr->data[index];
    }
    sources->count = src_ptr->count;
    sources->source_num = src_ptr->source_num;
    //sources->count -= source_interval_extender_info.preroll_length;
    return 0;
}
#endif // BUILD_MODULE_EXTENDED_SOURCE_TRACKER

/* EOF */
