
/************************************************************
**  SourceTracker.c
**
**  Object name :
**    SourceTracker
**
**  History :
**     2013.10.31 M.Takigahira IFINDEX関連(Source構造体の変更)に対応
**     2013.05.14 M.Takigahira Build有無指定に対応
**     2013.03.29 K.Takagi SourceTrackerWithFilterをマージ
**     2013.02.13 K.Takagi フレームカウントの処理を修正
**     2013.02.05 K.Takagi SourceIntervalExtender修正
**     2012.12.15 K.Takagi SourceIntervalExtender追加
**     2012.12.14 K.Takagi 新規作成
**
**  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 "header.h"
#include "HA_log.h"
#include "android_hark.h"

#include "HABuffer.h"
#include "SourceTracker.h"

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

#include "build_modules.h"
#if !defined BUILD_MODULE_SOURCE_TRACKER
/******************************************************************************
**
**  History
**  -----------
**  2013.05.14 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 HAGetParamSourceTracker(Param_SourceTracker_t *param){ return 0; }
int HASetParamSourceTracker(Param_SourceTracker_t *param){ return 0; }
int HADynamicParamUpdateSourceTracker(Param_SourceTracker_t *param){ return 0; }
int HAInitSourceTracker(void){ return 0; }
int SourceTracker(int frame_count, SOURCE_DATA_ARY_t *sources){ return 0; }
int SourceTrackerWithFilter(int frame_count, SOURCE_DATA_ARY_t *sources, SOURCE_DATA_ARY_t *target){ return 0; }
int HAGetParamSourceIntervalExtender(Param_SourceIntervalExtender_t *param){ return 0; }
int HASetParamSourceIntervalExtender(Param_SourceIntervalExtender_t *param){ return 0; }
int HADynamicParamUpdateSourceIntervalExtender(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_SourceTracker_t source_tracker_info;
SOURCE_DATA_ARY_t last_sources;
SOURCE_DATA_ARY_t target_sources;
static int next_source_id;

Param_SourceIntervalExtender_t source_interval_extender_info;

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

/************************************************************
**
**  Private : 
**
**  History :
**            2013.05.09 M.Takigahira 内積によるcos(theta)導出関数SourceCosTheta()（ただし、-1<=theta<=1の範囲外を許す）と
**                                    theta導出関数SourceAngle()（ただし、-1<=theta<=1への制限処理を含む）に分割した
**                                    最小角検出にはacosを行わないSourceCosTheta()を用いて行う
**            2012.12.14 K.Takagi 新規作成
**
**  Description
**  -----------
**  二つの音源の角度を算出
**
*/
/*
// SourceCosTheta() ==> src_cos_theta() in android_hark.h
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;
}

// SourceAngle() ==> coordinates_angle() in android_hark.h
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に収める
}

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

// min() ==> _ha_imin() in android_hark.h
static inline int min(int a, int b){
    return ((a < b) ? a : b);
}

// SourceTfindexDistance() ==> tfindex_distance() in android_hark.h
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 : 2015.08.31 M.Takigahira 新規作成
**
**  Description
**  -----------
**  パラメータ取得
**
*/
int HAGetParamSourceTracker(Param_SourceTracker_t *param)
{
    (*param) = source_tracker_info;
    return 0;
}

/************************************************************
**
**  Public  : 
**
**  History : 2012.12.14 K.Takagi 新規作成
**
**  Description
**  -----------
**  パラメータ設定
**
*/
int HASetParamSourceTracker(Param_SourceTracker_t *param)
{
    if(param->min_id < 0){
        return -1;
    }
    
    source_tracker_info.thresh               = param->thresh;
    source_tracker_info.pause_length         = param->pause_length;
    source_tracker_info.compare_mode         = param->compare_mode;
    source_tracker_info.min_src_interval     = param->min_src_interval;
    source_tracker_info.min_tfindex_interval = param->min_tfindex_interval;
    source_tracker_info.min_id               = param->min_id;
    
    return 0;
}

/************************************************************
**
**  Public  : 
**
**  History : 2013.10.31 M.Takigahira 新規作成
**
**  Description
**  -----------
**  パラメータ設定
**
*/
int HADynamicParamUpdateSourceTracker(Param_SourceTracker_t *param)
{
    if(param->min_id < 0){
        return -1;
    }
    
    source_tracker_info.thresh               = param->thresh;
    source_tracker_info.pause_length         = param->pause_length;
    source_tracker_info.compare_mode         = param->compare_mode;
    source_tracker_info.min_src_interval     = param->min_src_interval;
    source_tracker_info.min_tfindex_interval = param->min_tfindex_interval;
    //source_tracker_info.min_id               = param->min_id;
    
    return 0;
}

/************************************************************
**
**  Public  : 
**
**  History : 
**            2013.03.29 K.Takagi マージ
**            2012.12.14 K.Takagi 新規作成
**
**  Description
**  -----------
**  初期化
**
*/
int HAInitSourceTracker(void)
{
    last_sources.source_num = 0;
    target_sources.source_num = 0;
    
    next_source_id = source_tracker_info.min_id;
    
    return 0;
}

/************************************************************
**
**  Public  : 
**
**  History : 2013.02.13 K.Takagi フレームカウントをsourcesに入っているもを利用するように修正
**            2013.02.05 K.Takagi 音源の検出と削除時にフレーム番号を表示
**            2012.12.14 K.Takagi 新規作成
**
**  Description
**  -----------
**  トレース処理
**
*/
int SourceTracker(int frame_count, SOURCE_DATA_ARY_t *sources)
{
    int i, j;
    int index;
    int sub_frame_count;
    int min_id;
    
    SOURCE_DATA_ARY_t new_sources;
    SOURCE_DATA_ARY_t *cur_sources;
    ////SOURCE_DATA_ARY_t remaining_sources;
    
    SOURCE_t *last_src;
    SOURCE_t *cur_src;
    SOURCE_t *new_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;
    float PauseLength;
    int   CompareMode;
    float MinSourceInterval;
    int   MinTFIndexInterval;
    
    /* 設定されているパラメータをロード */
    Thresh             = source_tracker_info.thresh;
    PauseLength        = source_tracker_info.pause_length;
    //CompareMode        = source_tracker_info.compare_mode;
    CompareMode        = 0;
    
    //デバック
    //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;
    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);
                cos_theta = src_cos_theta(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].dir_id           = cur_sources->data[min_id].dir_id; // 2017.08.18 K.Takagi UART
                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 = sub_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].dir_id           = last_src->dir_id; // 2017.08.18 K.Takagi UART
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = last_src->remaining - 10.0f;
                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);
                cos_theta = src_cos_theta(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].dir_id           = cur_src->dir_id; // 2017.08.18 K.Takagi UART
                new_sources.data[index].id               = next_source_id++;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = sub_frame_count;
                new_sources.data[index].last_found_count = sub_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);
                distance = tfindex_distance(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 < MinTFIndexInterval){ // /* 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].dir_id           = cur_sources->data[min_id].dir_id; // 2017.08.18 K.Takagi UART
                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 = sub_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].dir_id           = last_src->dir_id; // 2017.08.18 K.Takagi UART
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = last_src->remaining - 10.0f;
                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_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);
                distance = tfindex_distance(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].dir_id           = cur_src->dir_id; // 2017.08.18 K.Takagi UART
                new_sources.data[index].id               = next_source_id++;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = sub_frame_count;
                new_sources.data[index].last_found_count = sub_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++){
        //現在情報を更新（コピーバック）
        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].dir_id           = new_sources.data[i].dir_id; // 2017.08.18 K.Takagi UART
        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].dir_id           = new_sources.data[i].dir_id; // 2017.08.18 K.Takagi UART
        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;
//
//    SOURCE_DATA_ARY_t new_sources;
//    SOURCE_DATA_ARY_t *cur_sources;
//    SOURCE_DATA_ARY_t remaining_sources;
//
//    SOURCE_t *cur_src;
//    SOURCE_t *new_src;
//	SOURCE_t *target_src;
//
//    float cos_theta, max_cos_theta, angle, min_angle;
//    int min_id;
//
//    float Thresh;
//    float PauseLength;
//    float MinSourceInterval;
//    float near_angle;
//
//    /* 設定されているパラメータをロード */
//    Thresh            = source_tracker_info.thresh;
//    PauseLength       = source_tracker_info.pause_length;
//    MinSourceInterval = source_tracker_info.min_src_interval;
//
//    near_angle = MinSourceInterval * M_PI / 180.0;
//
//    //デバック
//    //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));
//
//    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].mask  = target->data[i].mask;
//        }
//        target_sources.source_num = target->source_num;
//    }
//
//    sub_frame_count = sources->count;
//    cur_sources = sources;
//    for(i=0; i<cur_sources->source_num; i++){
//    	/* 角度が対象外ならばPower 0となるように初期化（以降の処理で扱われない） */
//        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 = 0; // 処理対象外
//        remaining_sources.data[i].id    = cur_sources->data[i].id;
//        remaining_sources.data[i].remaining = 0;
//    	for(j=0; j<target_sources.source_num; j++){
//            target_src = &target_sources.data[j];
//            cur_src = &cur_sources->data[i];
//            if(remaining_sources.data[i].power != 0){ continue; }
//            angle = SourceAngle(cur_src->x, target_src->x);
//            if(angle < near_angle){ /* 角度が対象角に近ければ対象角で上書き（統合処理はSourceTrackerのアルゴリズムに任せる） */
//                remaining_sources.data[i].x[0]  = target_src->x[0]; // Overwrite x[0] for filter direction
//                remaining_sources.data[i].x[1]  = target_src->x[1]; // Overwrite x[1] for filter direction
//                remaining_sources.data[i].x[2]  = target_src->x[2]; // Overwrite x[2] for filter direction
//                remaining_sources.data[i].power = cur_sources->data[i].power; // 処理対象
//                //remaining_sources.data[i].id    = cur_sources->data[i].id;
//                //remaining_sources.data[i].remaining = 0;
//            }
//    	}
//    }
//    remaining_sources.source_num = cur_sources->source_num;
//
//    new_sources.source_num = 0;
//    for(i=0; i<last_sources.source_num; i++){
//        SOURCE_t *last_src;
//        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]
//        min_id = 0;
//        for(j=0; j<remaining_sources.source_num; j++){
//            cur_src = &remaining_sources.data[j];
//            if(cur_src->power < Thresh){ continue; }
//            //cos_theta = SourceCosTheta(cur_src->x, last_src->x); //angle = SourceAngle(cur_src->x, last_src->x);
//            cos_theta = src_cos_theta(cur_src->x, last_src->x); //angle = SourceAngle(cur_src->x, last_src->x);
//            ////if(angle < min_angle){
//            ////    min_angle = angle;
//            ////    min_id = j;
//            ////}
//            if(cos_theta > max_cos_theta){
//            	max_cos_theta = cos_theta;
//                min_id = j;
//            }
//        }
//        min_angle = acos((max_cos_theta > 1.0f ? 1.0f : max_cos_theta));
//
//        index = new_sources.source_num;
//        if(min_angle < near_angle){ // near_angle=(MinSourceInterval*M_PI/180.0) /* 角度が近ければ統合 */
//            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].id        = last_src->id;
//            new_sources.data[index].remaining = PauseLength;
//
//            /* 統合したので現在のリストからは削除（powerを0にすることで次の処理で扱われない） */
//            remaining_sources.data[min_id].power = 0;
//            cur_sources->data[min_id].power = 0;
//        }
//        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 - 10;
//        }
//        //LOGI("id=%d, remaining=%g\n", new_sources.data[index].id, new_sources.data[index].remaining);
//        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; }
//        ////min_angle = 100;
//        max_cos_theta = -1.0f; // max cos(-1)==180[deg]
//        min_id = 0;
//        for(j=0; j<new_sources.source_num; j++){
//            new_src = &new_sources.data[j];
//            //cos_theta = SourceCosTheta(new_src->x, cur_src->x); //angle = SourceAngle(new_src->x, cur_src->x);
//            cos_theta = src_cos_theta(new_src->x, cur_src->x); //angle = SourceAngle(new_src->x, cur_src->x);
//            ////if(angle < min_angle){
//            ////    min_angle = angle;
//            ////    min_id = j;
//            ////}
//            if(cos_theta > max_cos_theta){
//            	max_cos_theta = cos_theta;
//                min_id = j;
//            }
//        }
//        min_angle = acos((max_cos_theta > 1.0f ? 1.0f : max_cos_theta));
//
//	    index = new_sources.source_num;
//        if(min_angle > near_angle){ // near_angle=(MinSourceInterval*M_PI/180.0)
//            //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.source_num++;
//        }
//    }
//
//    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;
//
//        //保存
//        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;
//    }
//    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;
//}

int SourceTrackerWithFilter(int frame_count, SOURCE_DATA_ARY_t *sources, SOURCE_DATA_ARY_t *target)
{
    int i, j;
    int index;
    int sub_frame_count;
    int min_id;

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

    SOURCE_t *last_src;
    SOURCE_t *cur_src;
    SOURCE_t *new_src;
    SOURCE_t *target_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;
    float PauseLength;
    int   CompareMode;
    float MinSourceInterval;
    int   MinTFIndexInterval;

    /* 設定されているパラメータをロード */
    Thresh            = source_tracker_info.thresh;
    PauseLength       = source_tracker_info.pause_length;
    CompareMode       = source_tracker_info.compare_mode;
    
    //デバック
    //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;
    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);
                cos_theta = src_cos_theta(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].dir_id           = cur_sources->data[min_id].dir_id; // 2017.08.18 K.Takagi UART
                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 = sub_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].dir_id           = last_src->dir_id; // 2017.08.18 K.Takagi UART
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = last_src->remaining - 10.0f;
                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);
                cos_theta = src_cos_theta(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);
                cos_theta = src_cos_theta(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].dir_id           = cur_src->dir_id; // 2017.08.18 K.Takagi UART
                new_sources.data[index].id               = next_source_id++;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = sub_frame_count;
                new_sources.data[index].last_found_count = sub_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);
                distance = tfindex_distance(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 < MinTFIndexInterval){ // /* 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].dir_id           = cur_sources->data[min_id].dir_id; // 2017.08.18 K.Takagi UART
                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 = sub_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].dir_id           = last_src->dir_id; // 2017.08.18 K.Takagi UART
                new_sources.data[index].id               = last_src->id;
                new_sources.data[index].remaining        = last_src->remaining - 10.0f;
                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_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);
                distance = tfindex_distance(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);
                distance = tfindex_distance(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].dir_id           = cur_src->dir_id; // 2017.08.18 K.Takagi UART
                new_sources.data[index].id               = next_source_id++;
                new_sources.data[index].remaining        = PauseLength;
                new_sources.data[index].count            = sub_frame_count;
                new_sources.data[index].last_found_count = sub_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].dir_id           = new_sources.data[i].dir_id; // 2017.08.18 K.Takagi UART
        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].dir_id           = new_sources.data[i].dir_id; // 2017.08.18 K.Takagi UART
        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 : 2015.08.31 M.Takigahira 新規作成
**
**  Description
**  -----------
**  パラメータ取得
**
*/
int HAGetParamSourceIntervalExtender(Param_SourceIntervalExtender_t *param)
{
    (*param) = source_interval_extender_info;
    return 0;
}

/************************************************************
**
**  Public  : 
**
**  History : 2012.12.15 K.Takagi 新規作成
**
**  Description
**  -----------
**  パラメータ設定
**
*/
int HASetParamSourceIntervalExtender(Param_SourceIntervalExtender_t *param)
{
    if(param->preroll_length < 0){
        return -1;
    }
    source_interval_extender_info.preroll_length = param->preroll_length;
    return 0;
}


int HADynamicParamUpdateSourceIntervalExtender(Param_SourceIntervalExtender_t *param){
    if(param->preroll_length < 0){
        return -1;
    }
    source_interval_extender_info.preroll_length = param->preroll_length;
    return 0;
}


/************************************************************
**
**  Public  : 
**
**  History : 2013.02.13 K.Takagi フレームカウントをsourcesに入っているもを利用するように修正
**            2013.02.05 K.Takagi 指定したフレーム数分前のキューに追加するように処理を修正
**                                引数のsourcesの内容を遡った位置の情報に更新するように修正
**            2012.12.15 K.Takagi 新規作成
**
**  Description
**  -----------
**  実際の処理
**
*/
#if 0
int SourceIntervalExtender(int frame_count, SOURCE_DATA_ARY_t *sources)
{
    if(frame_count < source_interval_extender_info.preroll_length){
        return -1;
    }
    sources->count -= source_interval_extender_info.preroll_length;
    
    return 0;
}
#endif

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_SOURCE_TRACKER

/* EOF */
