
/************************************************************
**  LocalizeMUSIC.c
**
**  Object name :
**     LocalizeMUSIC
**
**  History :
**     2013.05.13 M.Takigahira Build有無指定に対応
**     2013.04.16 K.Takagi tmp[16]、FoundDirection[16]を拡張
**                         SearchPeakのif文を修正
**     2013.04.03 K.Takagi マージ
**     2013.03.29 K.Takagi マージ
**     2013.03.06 K.Takagi 平滑化のためのスペクトル加算は各フレームで処理するように修正
**     2013.02.26 K.Takagi RxxとRxxNをバッファ確保時にクリア
**     2013.02.13 K.Takagi sourcesに入れるフレームカウントを修正
**     2013.01.17 K.Takagi メモリチェック追加
**     2013.01.08 K.Takagi GSVDとSEVDで同じ方向が得られることを確認
**     2013.01.07 K.Takagi GSVD対応
**     2012.12.15 K.Takagi バッファの解放処理追加
**     2012.12.14 K.Takagi 定位結果を上位に返すように引数を追加
**     2012.12.13 K.Takagi 定位の結果が合うことを確認
**     2012.12.05 K.Takagi オリジナルをベースに計算用のバッファを定義
**     2012.12.02 K.Takagi 伝達関数の読み出し動作を確認
**     2012.10.22 K.Takagi 新規作成
**
**  Description
**  -----------
**     MUSIC法による音源定位
**
**     ステアリングベクトルの格納順に注意
**       ファイル上の並びと格納バッファの並びが異なる
**
**     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_config.h"
#include "build_modules.h"
#include "HA_log.h"
#include "android_hark.h"

#include "HABuffer.h"
#include "LocalizeMUSIC.h"
#include "MatrixMemoryAllocate.h"
/*
#ifdef ENABLE_HARKIO3
#include "libharkio3.h"
#endif // ENABLE_HARKIO3
*/
///////////////////////////////////////////////////////////////
//  Define Macros
#ifdef DEBUG
#define DEBUG_PRINT_LM LOGI
#else
#define DEBUG_PRINT_LM(...)
#endif

#define USE_CALC_ACCELERATION

#ifdef USE_CALC_ACCELERATION
#define TAKIGAHIRA
#define COMMENT_OUT
#else
#endif

#undef EXPORT_DIRECTION_POWER
#ifdef ENABLE_ANDROID_PATH
#define OUT_FILE_NAME_DIRECTION_POWER "/mnt/sdcard/hark2/direc_pow"
#else
#define OUT_FILE_NAME_DIRECTION_POWER "/media/SD/hark2/direc_pow"
#endif

#if !defined BUILD_MODULE_LOCALIZE_MUSIC
///////////////////////////////////////////////////////////////
//  Dummy Functions for build empty methods.
static int ReadTransferFile(char *file_name, int *ch, int channels){ return 0; }
int HASetParamLocalizeMUSIC(Param_LocalizeMUSIC_t *param){ return 0; }
int HAInitLocalizeMUSIC(void){ return 0; }
int HAReleaseLocalizeMUSIC(void){ return 0; }
static void AddCorrelation(int pslength, int nb_channel){ }
static void AddNormalizeCorrelation(int pslength, int nb_channel, int sum_count){ }
static int SearchPeak(float *power, int power_num, int *peak_cand, float *SDirection, int mindeg, int maxdeg){ return 0; }
static int SearchDirectionPeak(float *power, int power_num, int *peak_cand, float *SDirection, int mindeg, int maxdeg){ return 0; }
static int SearchDifferentialPeak(float *power, int power_num, int *peak_cand, float *SDirection, int mindeg, int maxdeg){ return 0; }
static void Partial_Sort(int *candidate, float *Pavg_sum, int source_num, int *direction){ }
static void Evd(COMPLEX_FLOAT **a, float *d, COMPLEX_FLOAT **v){ }
float ABS(float a){ return 0.0f; }
static void Svd(COMPLEX_FLOAT **a, float *s, COMPLEX_FLOAT **u, COMPLEX_FLOAT **v){ }
void SortEigFvecAscending(float *ai, COMPLEX_FLOAT **bi, float *af, COMPLEX_FLOAT **bf){ }
int LocalizeMUSIC(int frame_count, SOURCE_DATA_ARY_t *sources){ return 0; }
#ifdef STANDALON_TEST
int main(int argc, char **argv){ return 0; }
#endif // STANDALON_TEST
#else // #if defined BUILD_MODULE_LOCALIZE_MUSIC
///////////////////////////////////////////////////////////////
//  Parameter

/*=========================================================*/
/*                 Structure Declaration                   */
/*=========================================================*/
typedef struct _AMATRIX_INFO_t_
{
/*
    int length;
    int sampling_rate;
    float elevation;
    int window;
    int period;
    int num_source;
    int min_deg;
    int max_deg;
    int lower_band;
    int upper_band;
*/
    int fft_length;
    int mic_num;
    int direction_num;
    int range_num;
    
}AMATRIX_INFO_t;

typedef struct _POS_3D_t_
{
    float x;
    float y;
    float z;
} POS_3D_t;

/*=========================================================*/
/*                    Private Valiable                     */
/*=========================================================*/
static int period_count;

static Param_LocalizeMUSIC_t localize_music_info;
static AMATRIX_INFO_t amatrix_info;
static COMPLEX_FLOAT ****AMatrix;
static COMPLEX_FLOAT ****AMatrixTmp;
static FFT_DATA_ARY_t *fft_data_ary_ptr;

#define SEARCH_DIRECTION_MAX_NUM 360
#define SEARCH_RANGE_MAX_NUM     4

// ファイルから取得したマイクの座標
static POS_3D_t m2pg_mic_pos[16];

// ファイルから取得した音源の方向情報(5度刻みで72要素)
static POS_3D_t m2pg_sound_pos[SEARCH_DIRECTION_MAX_NUM][SEARCH_RANGE_MAX_NUM];

// 検出された音源数
static int FoundSourceCount;

// 音源の方向情報(5度刻みで72要素)
static int   SearchDirection[SEARCH_DIRECTION_MAX_NUM];
static float fSearchDirection[SEARCH_DIRECTION_MAX_NUM];

// 音源の距離情報(通常は1つ)
static int SearchRange[SEARCH_RANGE_MAX_NUM];

static float Pavg[SEARCH_DIRECTION_MAX_NUM][SEARCH_RANGE_MAX_NUM];
static float Pavg_sum[SEARCH_DIRECTION_MAX_NUM];
#ifndef COMMENT_OUT
static float **Pavg_bin;
#endif

// 最大サイズは構造体のmanual_weight_splineと同じにする
static float SSW[2][32];

// スペクトルの長さと同じ（デフォルトは512/2+1=257）
static float MSSW[512+1];

static int UsesFreq[512+1];

static COMPLEX_FLOAT **X;
static COMPLEX_FLOAT ***Rxx;
static COMPLEX_FLOAT ***RxxN;

static float **EigenValue;
//static COMPLEX_FLOAT CEigenValue[16];
//static float REigenValue[16];
static float *REigenValue;
static COMPLEX_FLOAT ***E;
static COMPLEX_FLOAT ***EN;

static int FoundDirection[(SEARCH_DIRECTION_MAX_NUM/2)];
static int FoundRange[16];

//static COMPLEX_FLOAT R[16][16];
static COMPLEX_FLOAT **R;
static COMPLEX_FLOAT RN[16][16];
//static COMPLEX_FLOAT V[16][16];
static COMPLEX_FLOAT **V;
static COMPLEX_FLOAT VN[16][16];

//static COMPLEX_FLOAT TempCalc1[16][16];
//static COMPLEX_FLOAT TempCalc2[16][16];
//static COMPLEX_FLOAT TempCalc3[16][16];
static COMPLEX_FLOAT **TempCalc3;
//static COMPLEX_FLOAT TempCalc4[16][16];
static COMPLEX_FLOAT **TempCalc4;
//static COMPLEX_FLOAT TempCalc5[16][16];
//static COMPLEX_FLOAT TempCalc6[16][16];
//static COMPLEX_FLOAT TempCalc7[16][16];
//static COMPLEX_FLOAT TempCalc8[16][16];
//static COMPLEX_FLOAT TempCalc9[16][16];

static float DirecInfo[32][4];
static int DirecNum;

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

static void loadTable(){
  FILE* fp = NULL;
  int i, j;
  char buf[8192];
  char *end;
  if((fp=fopen("direc_info", "r"))==NULL){
    if((fp=fopen("direc_info", "r"))==NULL){
      printf("direc_info file not found.¥nwas cannot found the default file.¥n");
    }
    else{
      printf("direc_info file not found.¥nuse the default value.¥n");
    }
  }
  if(!fp){
    perror("direc_info file open");
    exit(1);
  }
  i = 0; j = 0;
  while(fgets(buf, sizeof(buf), (FILE*)fp)!=NULL){
    //printf("[debug] %s¥n", buf);
    if(j==4){ j=0; i++; }
    DirecInfo[i][j] = strtof(buf, &end);
    j++;
    DirecNum = i;
  }
  if(feof(fp)==0){
    printf("not eof error");
  }
  if(fp){ fclose(fp); }


}

///////////////////////////////////////////////////////////////
//  MAIN
/************************************************************
**  History :
**    2013.12.** M.Takigahira libharkio3対応を追加（する前に3D対応を行う必要がある）
**    2013.01.17 K.Takagi     AMatrixバッファのチェック追加
**    2012.12.02 K.Takagi     実ファイルからの読み出し動作を確認
**    2012.10.22 K.Takagi     新規作成
*/
// Public : 指定したファイルに含まれる情報よりバッファを確保し、伝達関数を読み込む
static int ReadTransferFile(char *file_name, int *ch, int channels)
{
/*
#ifdef ENABLE_HARKIO3
    int ct, f, s, m;
    float coord_tmp[3]; //if pos->sys is Polar
    harkio_TransferFunction *tfp = harkio_TransferFunction_fromFile(tf_file_name);
    int num_tf_pos      = tfp->poses->size;
    int num_mic     = tfp->mics->size;
    amatrix_info.mic_num = mic_num;
    int num_freq    = tfp->cfg->nfft / 2 + 1;
    ct=0;
    if(tfp->poses->pos[0]->sys == Cartesian){
        //LOGD("Source positions is Cartesian");
        for(s=0; s<num_tf_pos; s++){
            locaTFPos[s].ha_fX = tfp->poses->pos[s]->coord[ct++];
            locaTFPos[s].ha_fY = tfp->poses->pos[s]->coord[ct++];
            locaTFPos[s].ha_fZ = tfp->poses->pos[s]->coord[ct++];
            //LOGI("POS : sourceLocations[%d] = (%.6f, %.6f, %.6f) ", s, sourceLocations[s].x[0], sourceLocations[s].x[1], sourceLocations[s].x[2]);
        }
    }
    else{
        //LOGD("Source positions is Polar");
        for(s=0; s<num_tf_pos; s++){
            harkio_Position_Convert(tfp->poses->pos[s], coord_tmp);
            locaTFPos[s].ha_fX = coord_tmp[0];
            locaTFPos[s].ha_fY = coord_tmp[1];
            locaTFPos[s].ha_fZ = coord_tmp[2];
            //LOGI("POS : sourceLocations[%d] = (%.6f, %.6f, %.6f) ", s, sourceLocations[s].x[0], sourceLocations[s].x[1], sourceLocations[s].x[2]);
        }
    }
    ct=0;
    if(tfp->mics->pos[0]->sys == Cartesian){
        //LOGD("Mic positions is Cartesian");
        for(m=0; m<num_mic; m++){
            locaMicPos[m].ha_fX = tfp->mics->pos[m]->coord[ct++];
            locaMicPos[m].ha_fY = tfp->mics->pos[m]->coord[ct++];
            locaMicPos[m].ha_fZ = tfp->mics->pos[m]->coord[ct++];
            //LOGI("MIC : micLocations[%d] = (%.6f, %.6f, %.6f) ", m, micLocations[m].x[0], micLocations[m].x[1], micLocations[m].x[2]);
        }
    }
    else{
        //LOGD("Mic positions is Polar");
        for(m=0; m<num_mic; m++){
        	harkio_Position_Convert(tfp->mics->pos[m], coord_tmp);
            locaMicPos[m].ha_fX = coord_tmp[0];
            locaMicPos[m].ha_fY = coord_tmp[1];
            locaMicPos[m].ha_fZ = coord_tmp[2];
            //LOGI("MIC : micLocations[%d] = (%.6f, %.6f, %.6f) ", m, micLocations[m].x[0], micLocations[m].x[1], micLocations[m].x[2]);
        }
    }
    ct=0;
    for(s=0; s<num_tf_pos; s++){
        if(tfp->septfs[s]->rows == num_mic && tfp->septfs[s]->cols == num_freq){
            for(m=0; m<num_mic; m++){
                for(f=0; f<num_freq; f++){
                    piaFreqLine[f][m][s].re = crealf(tfp->septfs[s]->data.complexdata[num_freq * m + f]);
                    piaFreqLine[f][m][s].im = cimagf(tfp->septfs[s]->data.complexdata[num_freq * m + f]);
                }
            }
        }
    }
    //int num_nbrs;
    //for(s=0; s<num_tf_pos; s++){
    //    harkio_Neighbors_getNeighbors(tfp->nbrs, s, neighborsInformations[s], &num_nbrs);
    //    neighborsInformations[s][num_nbrs] = -1; // terminate
    //}
    harkio_TransferFunction_delete(&tfp);
    //new_tf_format = 1;
#else // ENABLE_HARKIO3
*/
    FILE *fp;
    
    int rsize;
    int total_size;
    int chunk_size;
    short format_ver[2];
    int format_id;
    int m;
    int mic_num;
    int f;
    int fft_length;
    int impluse_response_length;
    int sampling_freq;
    float pos[3];
    
    int id;
    int ir;
    int direction_num;
    int range_num;
    
    int index;
    char key_word[5];
    
    float f_real;
    float f_imag;
    
    fp = fopen(file_name, "rb");
    if(fp == NULL){
        return -1;
    }
    
    /* 識別子 HARK */
    rsize = fread(key_word, sizeof(char), 4, fp);
    if(rsize != 4){
        return -1;
    }
    key_word[4] = '\0';
    DEBUG_PRINT_LM("%s\n", key_word);
    
    if(strcmp(key_word, "HARK") != 0){
        DEBUG_PRINT_LM("HARK Key not found.\n");
        return -1;
    }
    
    /* 以降のサイズを取得 */
    rsize = fread(&total_size, sizeof(int), 1, fp);
    if(rsize != 1){
        return -1;
    }
    DEBUG_PRINT_LM("Total Size = %d\n", total_size);
    
    /* 識別子 HGTF */
    rsize = fread(key_word, sizeof(char), 4, fp);
    if(rsize != 4){
        return -1;
    }
    key_word[4] = '\0';
    DEBUG_PRINT_LM("%s\n", key_word);
    
    if(strcmp(key_word, "HGTF") != 0){
        DEBUG_PRINT_LM("HGTF Key not found.\n");
        return -1;
    }
    
    /* 識別子 fmt */
    rsize = fread(key_word, sizeof(char), 4, fp);
    if(rsize != 4){
        return -1;
    }
    key_word[4] = '\0';
    DEBUG_PRINT_LM("%s\n", key_word);
    
    if(strcmp(key_word, "fmt ") != 0){
        DEBUG_PRINT_LM("fmt  Key not found.\n");
        return -1;
    }
    
    /* チャンクサイズを取得 */
    rsize = fread(&chunk_size, sizeof(int), 1, fp);
    if(rsize != 1){
        return -1;
    }
    DEBUG_PRINT_LM("Chunk Size = %d\n", chunk_size);
    
    /* フォーマットバージョンを取得 */
    rsize = fread(format_ver, sizeof(short), 2, fp);
    if(rsize != 2){
        return -1;
    }
    DEBUG_PRINT_LM("Major Ver = %d, Minor Ver = %d\n", format_ver[0], format_ver[1]);
    
    /* フォーマットIDを取得 */
    rsize = fread(&format_id, sizeof(int), 1, fp);
    if((rsize != 1) || (format_id != 1)){
        return -1;
    }
    DEBUG_PRINT_LM("Format ID = %d\n", format_id);

    /* マイク数を取得 */
    rsize = fread(&mic_num, sizeof(int), 1, fp);
    if(rsize != 1){
        return -1;
    }
    DEBUG_PRINT_LM("Mic Num = %d\n", mic_num);
    amatrix_info.mic_num = mic_num;

    for(m=0; m<channels; m++){
        if(!((ch[m] >= 0) && (ch[m] < mic_num))){
            //LOGI("Mic CH Error %d : %d\n", m, ch[m]);
            return -1;
        }
    }

    /* FFT長を取得 */
    rsize = fread(&fft_length, sizeof(int), 1, fp);
    if(rsize != 1){
        return -1;
    }
    DEBUG_PRINT_LM("FFT Length = %d\n", fft_length);
    amatrix_info.fft_length = fft_length;
    const int fft_div2_plus1 = fft_length/2+1;
    
    /* インパルスレスポンス長を取得 */
    rsize = fread(&impluse_response_length, sizeof(int), 1, fp);
    if(rsize != 1){
        return -1;
    }
    DEBUG_PRINT_LM("Impulse Response Length = %d\n", impluse_response_length);

    /* サンプリング周波数を取得 */
    rsize = fread(&sampling_freq, sizeof(int), 1, fp);
    if(rsize != 1){
        return -1;
    }
    DEBUG_PRINT_LM("Sampling Freq[Hz] = %d\n", sampling_freq);
    
    /* マイク座標を取得 */
    for(index=0; index<mic_num; index++){
        rsize = fread(pos, sizeof(float), 3, fp);
        if(rsize != 3){
            return -1;
        }
        m2pg_mic_pos[index].x = pos[0];
        m2pg_mic_pos[index].y = pos[1];
        m2pg_mic_pos[index].z = pos[2];
        
        DEBUG_PRINT_LM("Mic Pos[%d] = %f, %f, %f\n", index, pos[0], pos[1], pos[2]);
    }
    
    /*===================== M2PG部の解析 =====================*/
    
    /* 識別子 M2PG */
    rsize = fread(key_word, sizeof(char), 4, fp);
    if(rsize != 4){
        return -1;
    }
    key_word[4] = '\0';
    DEBUG_PRINT_LM("%s\n", key_word);
    
    if(strcmp(key_word, "M2PG") != 0){
        DEBUG_PRINT_LM("M2PG Key not found.\n");
        return -1;
    }
    
    /* チャンクサイズを取得 */
    rsize = fread(&chunk_size, sizeof(int), 1, fp);
    if(rsize != 1){
        return -1;
    }
    DEBUG_PRINT_LM("Chunk Size = %d\n", chunk_size);
    
    /* フォーマットバージョンを取得 */
    rsize = fread(format_ver, sizeof(short), 2, fp);
    if(rsize != 2){
        return -1;
    }
    DEBUG_PRINT_LM("Major Ver = %d, Minor Ver = %d\n", format_ver[0], format_ver[1]);
    
    /* 水平方向の要素数を取得 */
    rsize = fread(&direction_num, sizeof(int), 1, fp);
    if(rsize != 1){
        return -1;
    }
    DEBUG_PRINT_LM("Direction Num = %d\n", direction_num);
    amatrix_info.direction_num = direction_num;
    
    /* 距離の要素数を取得 */
    rsize = fread(&range_num, sizeof(int), 1, fp);
    if(rsize != 1){
        return -1;
    }
    DEBUG_PRINT_LM("Range Num = %d\n", range_num);
    amatrix_info.range_num = range_num;
    
    /* 音源位置の座標を取得 */
    for(id=0; id<direction_num; id++){
        for(ir=0; ir<range_num; ir++){
            rsize = fread(pos, sizeof(float), 3, fp);
            if(rsize != 3){
                return -1;
            }
            m2pg_sound_pos[id][ir].x = pos[0];
            m2pg_sound_pos[id][ir].y = pos[1];
            m2pg_sound_pos[id][ir].z = pos[2];
        }
        SearchDirection[id]  = (int)(round(atan2(pos[1], pos[0]) * 180.0 / M_PI));
        DEBUG_PRINT_LM("Search Direction [%d][%d] = %f, %f, %f, %d\n", id, 0, pos[0], pos[1], pos[2], SearchDirection[id]);
    }

    for(ir=0; ir<range_num; ir++){
        SearchRange[ir] = (int)sqrt( (m2pg_sound_pos[0][ir].x * m2pg_sound_pos[0][ir].x) +
                                     (m2pg_sound_pos[0][ir].y * m2pg_sound_pos[0][ir].y) +
                                     (m2pg_sound_pos[0][ir].z * m2pg_sound_pos[0][ir].z) );
        DEBUG_PRINT_LM("Search Range [%d][%d] = %d\n", 0, ir, SearchRange[ir]);
    }
    
    /* インパルス応答のバッファを確保 complex<float> [direction_num][range_num][fft_length/2+1][mic_num] */
    if(AMatrix != NULL){
        free(AMatrix[0][0][0]);
        free(AMatrix[0][0]);
        free(AMatrix[0]);
        free(AMatrix);
    }
    //AMatrix = (COMPLEX_FLOAT ****)MemoryAllocate4D(direction_num, range_num, (fft_length/2+1), mic_num, sizeof(COMPLEX_FLOAT));
    AMatrix = (COMPLEX_FLOAT ****)MemoryAllocate4D(direction_num, range_num, fft_div2_plus1, mic_num, sizeof(COMPLEX_FLOAT));
    if(AMatrix == NULL){
        return -1;
    }
    
    if(AMatrixTmp != NULL){
        free(AMatrixTmp[0][0][0]);
        free(AMatrixTmp[0][0]);
        free(AMatrixTmp[0]);
        free(AMatrixTmp);
    }
    //AMatrixTmp = (COMPLEX_FLOAT ****)MemoryAllocate4D(direction_num, range_num, (fft_length/2+1), mic_num, sizeof(COMPLEX_FLOAT));
    AMatrixTmp = (COMPLEX_FLOAT ****)MemoryAllocate4D(direction_num, range_num, fft_div2_plus1, mic_num, sizeof(COMPLEX_FLOAT));
    if(AMatrixTmp == NULL){
        return -1;
    }
    
    /* インパルス応答の読み込み
    **
    ** ファイルには [direction_num][range_num][mic_num][fft_length/2+1] の順で格納されている
    ** バッファには [direction_num][range_num][fft_length/2+1][mic_num] の順で格納する
    **
    */
    for(id=0; id<direction_num; id++){
        for(ir=0; ir<range_num; ir++){
            for(m=0; m<mic_num; m++){
                //for(f=0; f<(fft_length/2+1); f++){
                for(f=0; f<fft_div2_plus1; f++){
                    rsize = fread(&f_real, sizeof(float), 1, fp); // 実部
                    if(rsize != 1){
                        return -1;
                    }
                    AMatrixTmp[id][ir][f][m].real = f_real;
                    
                    rsize = fread(&f_imag, sizeof(float), 1, fp); // 虚部
                    if(rsize != 1){
                        return -1;
                    }
                    AMatrixTmp[id][ir][f][m].imag = f_imag;
                }
            }
        }
    }
    fclose(fp);
/*
#endif // ENABLE_HARKIO3
*/
    
    /* 実際に必要なCHのデータのみを抽出 */
    for(id=0; id<direction_num; id++){
        for(ir=0; ir<range_num; ir++){
            //for(m=0; m<channels; m++){
                //for(f=0; f<(fft_length/2+1); f++){
            for(f=0; f<fft_div2_plus1; f++){
                for(m=0; m<channels; m++){
                    AMatrix[id][ir][f][m].real = AMatrixTmp[id][ir][f][ch[m]].real; // 実部
                    AMatrix[id][ir][f][m].imag = AMatrixTmp[id][ir][f][ch[m]].imag; // 虚部
                }
            }
        }
    }
    free(AMatrixTmp[0][0][0]);
    free(AMatrixTmp[0][0]);
    free(AMatrixTmp[0]);
    free(AMatrixTmp);
    AMatrixTmp = NULL;
    
    //LOGI("nd, nr, m, f = %d, %d, %d, %d\n", nd, nr, m, f);
    
    /* 計算用のバッファをクリア */
    bzero(&Pavg, sizeof(Pavg));
    bzero(&Pavg_sum, sizeof(Pavg_sum));
    
#if 0 // COMMENT_OUT
    if(Pavg_bin != NULL){
        free(Pavg_bin[0]);
        free(Pavg_bin);
    }
    Pavg_bin = (float **)MemoryAllocate2D((fft_length/2+1), direction_num, sizeof(float));
#endif
    return 0;
}

/************************************************************
**  History :
**    2012.12.04 K.Takagi デバック表示を追加
**    2012.12.05 K.Takagi 新規作成
*/
// Public : パラメータ設定
int HASetParamLocalizeMUSIC(Param_LocalizeMUSIC_t *param)
{
    int index;
    int ch;
    int channel_num;
    
    channel_num = param->tf_channel_num;
    if((channel_num <= 0) || (channel_num > 16)){
        return -1;
    }

    localize_music_info.music_algorithm = param->music_algorithm;
    localize_music_info.tf_channel_num  = channel_num;
    
    for(ch=0; ch<channel_num; ch++){
        localize_music_info.tf_channel_selection[ch] = param->tf_channel_selection[ch];
    }
    
    localize_music_info.length = param->length;
    localize_music_info.sampling_rate = param->sampling_rate;
    
    strncpy(localize_music_info.a_matrix, param->a_matrix, 256);
    
    localize_music_info.elevation = param->elevation;
    localize_music_info.window = param->window;
    localize_music_info.period = param->period;
    localize_music_info.num_source = param->num_source;
    localize_music_info.export_num_source = param->export_num_source;
    localize_music_info.min_deg = param->min_deg;
    localize_music_info.max_deg = param->max_deg;
    localize_music_info.low_bound_frequency = param->low_bound_frequency;
    localize_music_info.upper_bound_frequency = param->upper_bound_frequency;
    localize_music_info.spectrum_weight_type = param->spectrum_weight_type;
    localize_music_info.enable_eigenvalue_weight = param->enable_eigenvalue_weight;
    
    /* spectrum_weight_type = A_Characteristic */
    localize_music_info.a_char_scaling = param->a_char_scaling;
    
    /* spectrum_weight_type = Manual_Spline */
    localize_music_info.spline_num = param->spline_num;
    for(index=0; index<32; index++){
        localize_music_info.manual_weight_spline[0][index] = param->manual_weight_spline[0][index];
        localize_music_info.manual_weight_spline[1][index] = param->manual_weight_spline[1][index];
    }
    
    /* spectrum_weight_type = Manual_Square */
    localize_music_info.square_num = param->square_num;
    for(index=0; index<32; index++){
        localize_music_info.manual_weight_square[index] = param->manual_weight_square[index];
    }
    
    // デバック表示
    /*
    LOGI("music_algorithm          = %d\n", localize_music_info.music_algorithm);
    LOGI("tf_channel_num           = %d\n", localize_music_info.tf_channel_num);
    LOGI("length                   = %d\n", localize_music_info.length);
    LOGI("sampling_rate            = %d\n", localize_music_info.sampling_rate);
    LOGI("a_matrix                 = %s\n", localize_music_info.a_matrix);
    LOGI("elevation                = %f\n", localize_music_info.elevation);
    LOGI("window                   = %d\n", localize_music_info.window);
    LOGI("period                   = %d\n", localize_music_info.period);
    LOGI("num_source               = %d\n", localize_music_info.num_source);
    LOGI("min_deg                  = %d\n", localize_music_info.min_deg);
    LOGI("max_deg                  = %d\n", localize_music_info.max_deg);
    LOGI("low_bound_frequency      = %d\n", localize_music_info.low_bound_frequency);
    LOGI("upper_bound_frequency    = %d\n", localize_music_info.upper_bound_frequency);
    LOGI("spectrum_weight_type     = %d\n", localize_music_info.spectrum_weight_type);
    LOGI("enable_eigenvalue_weight = %d\n", localize_music_info.enable_eigenvalue_weight);
    LOGI("a_char_scaling           = %f\n", localize_music_info.a_char_scaling);
    LOGI("spline_num               = %d\n", localize_music_info.spline_num);
    LOGI("square_num               = %d\n", localize_music_info.square_num);
    */
    return 0;
}

/************************************************************
**
**  Public  : 
**
**  History : 2013.02.26 K.Takagi Rxx、RxxNのクリアを追加
**            2013.01.07 K.Takagi REigenValue、TempCalcを追加
**            2012.12.05 K.Takagi バッファクリア等を追加
**            2012.12.04 K.Takagi 新規作成
**
**  Description
**  -----------
**  初期化
**
**
*/
int HAInitLocalizeMUSIC(void)
{
    int i,j,k;
    int stat;
    int index;
    int channel_num;
    int sampling_rate;
    int length;
    int pslength;
    
    float t_s;
    
    int ssw_length;
    float KEY_X[32];
    float KEY_Y[32];
    
    float SPLINE_X[512+1];
    float SPLINE_Y[512+1];
    
    channel_num   = localize_music_info.tf_channel_num,
    length        = localize_music_info.length;
    pslength      = length / 2 + 1;
    sampling_rate = localize_music_info.sampling_rate;
    
    period_count = 0;
    
    /* 周波数軸方向の重みの計算 */
    if(localize_music_info.spectrum_weight_type == Manual_Square){
    }
    else
    {
        if(localize_music_info.spectrum_weight_type == Manual_Spline){
            ssw_length = localize_music_info.spline_num;
            for(index=0; index<ssw_length; index++){
                SSW[0][index] = localize_music_info.manual_weight_spline[0][index];
                SSW[1][index] = localize_music_info.manual_weight_spline[1][index];
            }
        }
        else if(localize_music_info.spectrum_weight_type == Uniform){
            /* Unifomがデフォルト */
            ssw_length = 5;
            
            SSW[0][0] = 0.0 / 4.0 / 2.0 * (float)sampling_rate;
            SSW[0][1] = 1.0 / 4.0 / 2.0 * (float)sampling_rate;
            SSW[0][2] = 2.0 / 4.0 / 2.0 * (float)sampling_rate;
            SSW[0][3] = 3.0 / 4.0 / 2.0 * (float)sampling_rate;
            SSW[0][4] = 4.0 / 4.0 / 2.0 * (float)sampling_rate;
            
            SSW[1][0] = 1.0;
            SSW[1][1] = 1.0;
            SSW[1][2] = 1.0;
            SSW[1][3] = 1.0;
            SSW[1][4] = 1.0;
        }
        else if(localize_music_info.spectrum_weight_type == A_Characteristic){
            ssw_length = 16;
            t_s = localize_music_info.a_char_scaling;
            SSW[0][0]  = 10.0   * t_s;  SSW[0][1]  = 12.5    * t_s;     SSW[0][2]  = 16.0    * t_s;     SSW[0][3]  = 20.0    * t_s;
            SSW[0][4]  = 31.5   * t_s;  SSW[0][5]  = 63.0    * t_s;     SSW[0][6]  = 125.0   * t_s;     SSW[0][7]  = 250.0   * t_s;
            SSW[0][8]  = 500.0  * t_s;  SSW[0][9]  = 1000.0  * t_s;     SSW[0][10] = 2000.0  * t_s;     SSW[0][11] = 4000.0  * t_s;
            SSW[0][12] = 8000.0 * t_s;  SSW[0][13] = 12500.0 * t_s;     SSW[0][14] = 16000.0 * t_s;     SSW[0][15] = 20000.0 * t_s;
            
            SSW[1][0]  = pow(10.0,-70.4/20.0);  SSW[1][1]  = pow(10.0,-63.4/20.0);  SSW[1][2]  = pow(10.0,-56.7/20.0);   
            SSW[1][3]  = pow(10.0,-50.5/20.0);  SSW[1][4]  = pow(10.0,-39.4/20.0);  SSW[1][5]  = pow(10.0,-26.2/20.0); 
            SSW[1][6]  = pow(10.0,-16.1/20.0);  SSW[1][7]  = pow(10.0,-8.6/20.0);   SSW[1][8]  = pow(10.0,-3.2/20.0);    
            SSW[1][9]  = pow(10.0,0.0/20.0);    SSW[1][10] = pow(10.0,1.2/20.0);    SSW[1][11] = pow(10.0,1.0/20.0);  
            SSW[1][12] = pow(10.0,-1.1/20.0);   SSW[1][13] = pow(10.0,-4.3/20.0);   SSW[1][14] = pow(10.0,-6.6/20.0);    
            SSW[1][15] = pow(10.0,-9.3/20.0);       
        }
        else{
            return -1;
        }
        
        for(index=0; index<ssw_length; index++){
            KEY_X[index] = SSW[0][index];
            KEY_Y[index] = SSW[1][index];
        }
        
        for(index=0; index<pslength; index++){
            SPLINE_X[index] = sampling_rate * (float)index / (float)length;
            if((SPLINE_X[index] < SSW[0][0]) || (SPLINE_X[index] > SSW[0][ssw_length-1])){
                SPLINE_Y[index] = 0.0;
                continue;
            }
            //SPLINE_Y[index] = cubic_spline();
            
            // デフォルトのUniformならすべて1.0（ひとまずこれで進める）
            SPLINE_Y[index] = 1.0;
        }
        
        for(index=0; index<pslength; index++){
            MSSW[index] = SPLINE_Y[index];
        }
    }
    
    // localizer.reset()
    
    // バッファ初期化
    bzero(RN, sizeof(RN));
    bzero(VN, sizeof(VN));
    /*
    bzero(TempCalc1, sizeof(TempCalc1));
    bzero(TempCalc2, sizeof(TempCalc2));
    bzero(TempCalc3, sizeof(TempCalc3));
    bzero(TempCalc4, sizeof(TempCalc4));
    bzero(TempCalc5, sizeof(TempCalc5));
    bzero(TempCalc6, sizeof(TempCalc6));
    bzero(TempCalc7, sizeof(TempCalc7));
    bzero(TempCalc8, sizeof(TempCalc8));
    bzero(TempCalc9, sizeof(TempCalc9));
    */
    
    if(REigenValue != NULL){
        free(REigenValue);
    }
    REigenValue = (float *)malloc(sizeof(float) * channel_num);
    
    if(TempCalc3 != NULL){
        free(TempCalc3[0]);
        free(TempCalc3);
    }
    TempCalc3 = (COMPLEX_FLOAT **)MemoryAllocate2D(channel_num, channel_num, sizeof(COMPLEX_FLOAT));
    
    if(TempCalc4 != NULL){
        free(TempCalc4[0]);
        free(TempCalc4);
    }
    TempCalc4 = (COMPLEX_FLOAT **)MemoryAllocate2D(channel_num, channel_num, sizeof(COMPLEX_FLOAT));
    
    if(R != NULL){
        free(R[0]);
        free(R);
    }
    R = (COMPLEX_FLOAT **)MemoryAllocate2D(channel_num, channel_num, sizeof(COMPLEX_FLOAT));

    if(V != NULL){
        free(V[0]);
        free(V);
    }
    V = (COMPLEX_FLOAT **)MemoryAllocate2D(channel_num, channel_num, sizeof(COMPLEX_FLOAT));

    if(EigenValue != NULL){
        free(EigenValue[0]);
        free(EigenValue);
    }
    EigenValue = (float **)MemoryAllocate2D(pslength, channel_num, sizeof(float));
    
    if(E != NULL){
        free(E[0][0]);
        free(E[0]);
        free(E);
    }
    E = (COMPLEX_FLOAT ***)MemoryAllocate3D(pslength, channel_num, channel_num, sizeof(COMPLEX_FLOAT));
    
    if(EN != NULL){
        free(EN[0][0]);
        free(EN[0]);
        free(EN);
    }
    EN = (COMPLEX_FLOAT ***)MemoryAllocate3D(pslength, channel_num, channel_num, sizeof(COMPLEX_FLOAT));
    
    if(Rxx != NULL){
        free(Rxx[0][0]);
        free(Rxx[0]);
        free(Rxx);
    }
    Rxx = (COMPLEX_FLOAT ***)MemoryAllocate3D(pslength, channel_num, channel_num, sizeof(COMPLEX_FLOAT));
    
    if(RxxN != NULL){
        free(RxxN[0][0]);
        free(RxxN[0]);
        free(RxxN);
    }
    RxxN = (COMPLEX_FLOAT ***)MemoryAllocate3D(pslength, channel_num, channel_num, sizeof(COMPLEX_FLOAT));
    
    for(k=0; k<pslength; k++){
        for(i=0; i<channel_num; i++){
            for(j=0; j<channel_num; j++){
                Rxx[k][i][j].real = 0.0f;
                Rxx[k][i][j].imag = 0.0f;
                
                RxxN[k][i][j].real = 0.0f;
                RxxN[k][i][j].imag = 0.0f;
            }
        }
    }

    // localize->ReadTransferFunction
    
    /* ステアリングベクトルの読み込み */
    stat = ReadTransferFile(localize_music_info.a_matrix, localize_music_info.tf_channel_selection, channel_num);
    //LOGI("ReadTransferFile : %d\n", stat);
    
    // localize->SetNumsource 構造体の値をそのまま利用
    // localize->SetUsedFrequency
    {
        int lower;
        int upper;
        
        lower = (int)((float)localize_music_info.low_bound_frequency * length / sampling_rate);
        upper = (int)((float)localize_music_info.upper_bound_frequency * length / sampling_rate);
        
        bzero(UsesFreq, sizeof(UsesFreq));
        for(index=lower; index<upper; index++){
            UsesFreq[index] = 1;
        } 
    }

    //if(DirecInfo != NULL){
    //    free(DirecInfo[0]);
    //    free(DirecInfo);
    //}
    //DirecInfo = (float **)MemoryAllocate2D(4, 32, sizeof(float));
    loadTable();
    return stat;
}

/************************************************************
**
**  Public  : 
**
**  History : 2013.01.07 K.Takagi REigenValue、TempCalcを追加
**            2012.12.15 K.Takagi 新規作成
**
**  Description
**  -----------
**  バッファクリアなどの解放処理
**
**
*/
int HAReleaseLocalizeMUSIC(void)
{
    if(REigenValue != NULL){
        free(REigenValue);
        REigenValue = NULL;
    }
    
    if(TempCalc3 != NULL){
        free(TempCalc3[0]);
        free(TempCalc3);
        TempCalc3 = NULL;
    }

    if(TempCalc4 != NULL){
        free(TempCalc4[0]);
        free(TempCalc4);
        TempCalc4 = NULL;
    }

    if(R != NULL){
        free(R[0]);
        free(R);
        R = NULL;
    }

    if(V != NULL){
        free(V[0]);
        free(V);
        V = NULL;
    }

    if(EigenValue != NULL){
        free(EigenValue[0]);
        free(EigenValue);
        EigenValue = NULL;
    }
    
    if(E != NULL){
        free(E[0][0]);
        free(E[0]);
        free(E);
        E = NULL;
    }
    
    if(EN != NULL){
        free(EN[0][0]);
        free(EN[0]);
        free(EN);
        EN = NULL;
    }
    
    if(Rxx != NULL){
        free(Rxx[0][0]);
        free(Rxx[0]);
        free(Rxx);
        Rxx = NULL;
    }
    
    if(RxxN != NULL){
        free(RxxN[0][0]);
        free(RxxN[0]);
        free(RxxN);
        RxxN = NULL;
    }
    
    if(AMatrix != NULL){
        free(AMatrix[0][0][0]);
        free(AMatrix[0][0]);
        free(AMatrix[0]);
        free(AMatrix);
        AMatrix = NULL;
    }
    
#if 0 // COMMENT_OUT
    if(Pavg_bin != NULL){
        free(Pavg_bin[0]);
        free(Pavg_bin);
        Pavg_bin = NULL;
    }
#endif
    //if(DirecInfo != NULL){
    //    free(DirecInfo[0]);
    //    free(DirecInfo);
    //    DirecInfo = NULL;
    //}
    
    return 0;    
}

/************************************************************
**
**  Public  : 
**
**  History : 2012.12.09 K.Takagi outer_prodの計算を修正
**            2012.12.05 K.Takagi 新規作成
**
**  Description
**  -----------
**  入力スペクトルの加算
**
*/
static void AddCorrelation(int pslength, int nb_channel)
{
    int i,j,k;
    //float f_real;
    //float f_imag;
    float tmp;
    COMPLEX_FLOAT x_i,x_j;
    
    for(k=0; k<pslength; k++){
        if(UsesFreq[k]){
            //for(j=0; j<nb_channel; j++){
                //for(i=0; i<nb_channel; i++){
            for(i=0; i<nb_channel; i++){
            	x_i = X[i][k];
                for(j=0; j<nb_channel; j++){
                    // 直積（outer_prod）の計算
                    // Rxx[k][i][j] += X[i][k] * X[j][k];
                    //f_real =  X[i][k].real * X[j][k].real + X[i][k].imag * X[j][k].imag;
                    //f_imag = -X[i][k].real * X[j][k].imag + X[i][k].imag * X[j][k].real;
                    //Rxx[k][i][j].real += f_real;
                    //Rxx[k][i][j].imag += f_imag;
                	x_j = X[j][k];
                    tmp  =  x_i.real * x_j.real;
                    tmp +=  x_i.imag * x_j.imag;
                    Rxx[k][i][j].real += tmp;
                    tmp  = -x_i.real * x_j.imag;
                    tmp +=  x_i.imag * x_j.real;
                    Rxx[k][i][j].imag += tmp;
                }
            }
            //LOGI("k=%d\n", k); k=16から88
        }
    }
}

static void AddNormalizeCorrelation(int pslength, int nb_channel, int sum_count)
{
    int i,j,k;
    //float f_real;
    //float f_imag;
    float tmp;
    COMPLEX_FLOAT x_i,x_j;

    const float sum_count_1 = 1.0f / (float)sum_count;
    for(k=0; k<pslength; k++){
        if(UsesFreq[k]){
            //for(j=0; j<nb_channel; j++){
                //for(i=0; i<nb_channel; i++){
            for(i=0; i<nb_channel; i++){
            	x_i = X[i][k];
                for(j=0; j<nb_channel; j++){
                    // 直積（outer_prod）の計算
                    // Rxx[k][i][j] += X[i][k] * X[j][k];
                    //f_real =  X[i][k].real * X[j][k].real + X[i][k].imag * X[j][k].imag;
                    //f_imag = -X[i][k].real * X[j][k].imag + X[i][k].imag * X[j][k].real;
                    //Rxx[k][i][j].real += f_real;
                    //Rxx[k][i][j].imag += f_imag;
                    //Rxx[k][i][j].real *= sum_count_1;
                    //Rxx[k][i][j].imag *= sum_count_1;
                	x_j = X[j][k];
                	tmp  =  x_i.real * x_j.real;
                	tmp +=  x_i.imag * x_j.imag;
                	tmp +=  Rxx[k][i][j].real;
                	tmp *=  sum_count_1;
                    Rxx[k][i][j].real = tmp;
                    tmp  = -x_i.real * x_j.imag;
                    tmp +=  x_i.imag * x_j.real;
                    tmp +=  Rxx[k][i][j].imag;
                    tmp *=  sum_count_1;
                    Rxx[k][i][j].imag = tmp;
                }
            }
            //LOGI("k=%d\n", k); k=16から88
        }
    }
}

/************************************************************
**
**  Public  : 
**
**  History :
**            2013.04.16 K.Takagi if文のmindeg、maxdeg判定を >= と <= に修正
**            2012.12.09 K.Takagi 新規作成
**
**  Description
**  -----------
**  ピーク検出
**
**
*/
static int SearchPeak(float *power, int power_num, int *peak_cand, float *SDirection, int mindeg, int maxdeg)
{
    int source_count;
    int i;
    int i_last;
    float p1;
    float p2;
    float p3;
    int deg;
    
    if(power_num < 2){
        // 検索要素が1つの場合
        peak_cand[0] = 0;
        return 1;
    }
    
    while(maxdeg - mindeg > 360) maxdeg -= 360;
    while(maxdeg - mindeg <= 0)  maxdeg += 360;
    
    source_count = 0;
    i_last = power_num - 1;
    for(i=0; i<power_num; i++)
    {
        deg = (int)(SDirection[i] + 0.5);
        
        while(deg - mindeg > 360) deg -= 360;
        while(deg - mindeg <= 0)  deg += 360;
        
        if((deg >= mindeg) && (deg <= maxdeg)){
            p1 = (i==0 ? power[i_last] : power[i-1]);
            p2 = power[i];
            p3 = (i==i_last ? power[0] : power[i+1]);
            
            if (p2 - p1 > 0 && p3 - p2 < 0){
                peak_cand[source_count] = i;
                source_count++;
                //LOGI("Peak ID = %d, %g\n", i, p2);
            }
        printf("deg=%d, power=%f\n", deg, power[i]);
        }
    }
    return source_count;
}

/************************************************************
**
**  Public  : 
**
**  History :
**            2016.05.04 M.Takigahira 新規作成
**
**  Description
**  -----------
**  方向ピーク検出
**
**
*/
static int SearchDirectionPeak(float *power, int power_num, int *peak_cand, float *SDirection, int mindeg, int maxdeg)
{
    int source_count;
    int i, j;
    int deg, deg_a, deg_b;
    int direc_num = 3;
    float direc_info[direc_num][4];

    int direc_j[direc_num];
    for(i=0; i<DirecNum; i++){
        deg_a = (int)(DirecInfo[i][0]);
        while(deg_a - mindeg > 360) deg_a -= 360;
        while(deg_a - mindeg <= 0)  deg_a += 360;
        deg_b = (int)(DirecInfo[i][1]);
        while(deg_b - mindeg > 360) deg_b -= 360;
        while(deg_b - mindeg <= 0)  deg_b += 360;
        direc_info[i][0] = (deg_a < deg_b) ? deg_a : deg_b;
        direc_info[i][1] = (deg_a < deg_b) ? deg_b : deg_a;
        direc_info[i][2] = DirecInfo[i][2];
        direc_info[i][3] = DirecInfo[i][3];
    }
    /*
    direc_info[0][0] = -40.0f; direc_info[0][1] = -20.0f;
    direc_info[0][2] = 1.0f;   direc_info[0][3] = 0.0f;
    direc_info[1][0] = -10.0f; direc_info[1][1] = 10.0f;
    direc_info[1][2] = 0.7f;   direc_info[1][3] = 0.0f;
    direc_info[2][0] = 20.0f;  direc_info[2][1] = 40.0f;
    direc_info[2][2] = 0.99f;   direc_info[2][3] = 0.0f;
    */
    if(power_num < 2){
        peak_cand[0] = 0;
        return 1;
    }
    while(maxdeg - mindeg > 360) maxdeg -= 360;
    while(maxdeg - mindeg <= 0)  maxdeg += 360;
    source_count = 0;
    for(j=direc_num-1; j>=0; j--){ direc_j[j] = -1; }
    for(i=0; i<power_num; i++){
        deg = (int)(SDirection[i] + 0.5);
        while(deg - mindeg > 360) deg -= 360;
        while(deg - mindeg <= 0)  deg += 360;
        for(j=direc_num-1; j>=0; j--){
            if((deg >= direc_info[j][0]) && (deg <= direc_info[j][1])){
                power[i] = power[i] * direc_info[j][2] + direc_info[j][3];
                if(direc_j[j]<0){ direc_j[j] = source_count; peak_cand[source_count] = i; source_count++; }
                else if(power[peak_cand[direc_j[j]]]<power[i]){ peak_cand[direc_j[j]] = i; }
            }
        }
        printf("deg=%d, power=%f\n", deg, power[i]);
    }
    return source_count;
}

/************************************************************
**
**  Public  : 
**
**  History :
**            2016.05.04 M.Takigahira 新規作成
**
**  Description
**  -----------
**  差動ピーク検出
**
**
*/
static int SearchDifferentialPeak(float *power, int power_num, int *peak_cand, float *SDirection, int mindeg, int maxdeg)
{
    int source_count;
    int i, j;
    int deg, deg_a, deg_b;
    int direc_num = 3;
    float direc_info[direc_num][4];
    float power_tmp[direc_num];
    int direc_j[direc_num];
    float dp, pp, np;
    for(i=0; i<DirecNum; i++){
        deg_a = (int)(DirecInfo[i][0]);
        while(deg_a - mindeg > 360) deg_a -= 360;
        while(deg_a - mindeg <= 0)  deg_a += 360;
        deg_b = (int)(DirecInfo[i][1]);
        while(deg_b - mindeg > 360) deg_b -= 360;
        while(deg_b - mindeg <= 0)  deg_b += 360;
        direc_info[i][0] = (deg_a < deg_b) ? deg_a : deg_b;
        direc_info[i][1] = (deg_a < deg_b) ? deg_b : deg_a;
        direc_info[i][2] = DirecInfo[i][2];
        direc_info[i][3] = DirecInfo[i][3];
    }
    /*
    direc_info[0][0] = -40.0f; direc_info[0][1] = -20.0f;
    direc_info[0][2] = 1.0f;   direc_info[0][3] = 0.0f;
    direc_info[1][0] = -10.0f; direc_info[1][1] = 10.0f;
    direc_info[1][2] = 0.7f;   direc_info[1][3] = 0.0f;
    direc_info[2][0] = 20.0f;  direc_info[2][1] = 40.0f;
    direc_info[2][2] = 0.99f;   direc_info[2][3] = 0.0f;
    */
    if(power_num < 2){
        peak_cand[0] = 0;
        return 1;
    }
    while(maxdeg - mindeg > 360) maxdeg -= 360;
    while(maxdeg - mindeg <= 0)  maxdeg += 360;
    source_count = 0;
    for(j=direc_num-1; j>=0; j--){ direc_j[j] = -1; }
    for(i=0; i<power_num; i++){
        deg = (int)(SDirection[i] + 0.5);
        while(deg - mindeg > 360) deg -= 360;
        while(deg - mindeg <= 0)  deg += 360;
        for(j=direc_num-1; j>=0; j--){
            if((deg >= direc_info[j][0]) && (deg <= direc_info[j][1])){
                power[i] = power[i] * direc_info[j][2] + direc_info[j][3];
                if(direc_j[j]<0){
                    if(deg >= -10 && deg <= 10){ np = power[i]; }
                    else if(deg >= -40 && deg <= -20){ dp = power[i]; }
                    else if(deg >= 20 && deg <= 40){ pp = power[i]; }
                    direc_j[j] = source_count;
                    peak_cand[source_count] = i;
                    source_count++;
                }
                else if(power[peak_cand[direc_j[j]]]<power[i]){
                    if(deg >= -10 && deg <= 10){ np = power[i]; }
                    else if(deg >= -40 && deg <= -20){ dp = power[i]; }
                    else if(deg >= 20 && deg <= 40){ pp = power[i]; }
                    peak_cand[direc_j[j]] = i;
                }
            }
        }
        printf("deg=%d, power=%f\n", deg, power[i]);
    }
    float select = (pp - np) - (dp - np);
    printf("select=%f\n", select);
    for(i=0; i<source_count; i++){
        deg = (int)(SDirection[peak_cand[i]]+0.5);
        while(deg - mindeg > 360) deg -= 360;
        while(deg - mindeg <= 0)  deg += 360;
        //if((deg >= -10) && (deg <= 10)){
        //    power[peak_cand[i]] = power[peak_cand[i]];
        //}
        if((deg >= -40) && (deg <= -20)){
            power[peak_cand[i]] = (select < -2.0f) ? power[peak_cand[i]] : 0.0f;
            printf("driver power=%f\n", power[peak_cand[i]]);
        }
        else if((deg >= 20) && (deg <= 40)){
            power[peak_cand[i]] = (select > 3.0f) ? power[peak_cand[i]] : 0.0f;
            printf("passenger power=%f\n", power[peak_cand[i]]);
        }
    }
    return source_count;
}

/************************************************************
**
**  Public  : 
**
**  History : 
**            2013.04.16 K.Takagi tmp配列の要素を拡張
**            2013.03.29 K.Takagi マージ
**            2012.12.09 K.Takagi 新規作成
**
**  Description
**  -----------
**  検出された音源位置をPavg_sumのレベル順にソート
**
**
*/
static void Partial_Sort(int *candidate, float *Pavg_sum, int source_num, int *direction)
{
    int id;
    int i,j,i_next;
    float tmp[(SEARCH_DIRECTION_MAX_NUM/2)]; // 検出音源最大数
    float val;
    
    for(i=0; i<source_num; i++){
    	tmp[i] = Pavg_sum[candidate[i]];
    }

    //for(j=0; j<source_num-1; j++){
    for(j=source_num-1; j>=1; j--){
        //for(i=0, i_next=1; i<source_num-j-1; i++, i_next++){
        for(i=0, i_next=1; i<j; i++, i_next++){
            if(tmp[i] < tmp[i_next]){
                val      = tmp[i];
                tmp[i]   = tmp[i_next];
                tmp[i_next] = val;
                
                id             = candidate[i];
                candidate[i]   = candidate[i_next];
                candidate[i_next] = id;
            }
        }
    }    
    
    for(i=0; i<source_num; i++){
        //LOGI("id = %d, val = %f\n", candidate[i], tmp[i]);
        direction[i] = candidate[i];
    }
    
    //LOGI("---\n");
}

/************************************************************
**  History :
**    2013.03.06 K.Takagi doubleの計算をすべてfloatに変更
**    2012.12.10 K.Takagi 処理を一通り実装
**    2012.12.05 K.Takagi 新規作成
*/
// Public : 固有値展開　evd(R, EigenValue, V) で呼び出し
//#include <fpu_control.h>
//fpu_control_t wctype = _FPU_IEEE | _FPU_RC_ZERO;
static void Evd(COMPLEX_FLOAT **a, float *d, COMPLEX_FLOAT **v)
{
    static float b[16];
    static float z[16];
    int n;
    float sm;
    //double sm;
    float thresh;
    float g;
    //double g;
    COMPLEX_FLOAT h;
    COMPLEX_FLOAT t;
    float c;
    //double c;
    COMPLEX_FLOAT s;
    COMPLEX_FLOAT tau;
    //COMPLEX_FLOAT theta;
    
    float norm_a;
    float t_tmp;
    float norm_t;
    //double norm_a;
    //double t_tmp;
    //double norm_t;
    
    int i,j;
    int ip, iq;
    
    float d_tmp;
    float v_tmp;
    float g_real, g_imag;
    float h_real, h_imag;
    float tmp_real, tmp_imag;
    float tmp;
    
    //double d_tmp;
    //double v_tmp;
    //double g_real, g_imag;
    //double h_real, h_imag;
    //double tmp_real, tmp_imag;
    
    //_FPU_SETCW(wctype);
    
    //n = amatrix_info.mic_num;
    n = localize_music_info.tf_channel_num;
    
    //デバック（既知の値で結果がほぼ一致することを確認：Pavg_sumの値で確認）
    /*
    for(ip=0; ip<n; ip++){
        for(iq=0; iq<n; iq++){
            a[ip][iq].real = 0.1 * ip;
            a[ip][iq].imag = 0.1 + 0.02*iq;
        }
    }
    for(ip=0; ip<n; ip++){
        for(iq=0; iq<n; iq++){
            LOGI("(%g,%g) ", a[ip][iq].real, a[ip][iq].imag);
        }
        LOGI("\n");
    }
    LOGI("-----------------------\n");
    */

    /* 単位行列の作成 */
    for(j=0; j<n; j++){
        for(i=0; i<n; i++){
            v[j][i].real = 0.0f;
            v[j][i].imag = 0.0f;
        }
        v[j][j].real = 1.0f;
    }

    for(ip=0; ip<n; ip++){
        //b[ip] = d[ip] = a(ip, ip).real();
        //z[ip] = 0;
        b[ip] = a[ip][ip].real;
        d[ip] = a[ip][ip].real;
        z[ip] = 0;
    }

    for(i=0; i<50; i++){
        sm = 0.0f;
        for(ip=0; ip<n-1; ip++){
            for(iq=ip+1; iq<n; iq++){
                //sm += std::abs(a(ip, iq));
                //sm += sqrt(a[ip][iq].real * a[ip][iq].real + a[ip][iq].imag * a[ip][iq].imag);
                sm += sqrtf(a[ip][iq].real * a[ip][iq].real + a[ip][iq].imag * a[ip][iq].imag);
            }
        }
        // デバック
        //LOGI("i = %d : %g\n", i, sm);
        
        //if (sm == 0.0) 
        //if (sm <= __FLT_EPSILON__) 
        if(sm <= 1.0e-7){
            //LOGI("sm == 0.0\n");
            return;
        }
        
        //if(i<4){ thresh = 0.2f * sm / (float)(n * n); }
        //else{    thresh = 0.0f; }
        thresh = ((i>=4) ? 0.0f : (0.2f * sm / (float)(n * n)));
        //LOGI("thresh = %g\n", thresh);
        
        for(ip=0; ip<n-1; ip++){
            for(iq=ip+1; iq<n; iq++){
                float abs_a;
                //double abs_a;
                
                //g = 100.0 * std::abs(a(ip, iq));
                abs_a = sqrtf(a[ip][iq].real * a[ip][iq].real + a[ip][iq].imag * a[ip][iq].imag);
                g = 100.0f * abs_a;
                //g = 100.0f * sqrtf((float)(a[ip][iq].real * a[ip][iq].real + a[ip][iq].imag * a[ip][iq].imag));
                
                //LOGI("-- %d, %d : %d --\n", i, ip, iq);
                //LOGI("%g, %g\n", a[ip][iq].real, a[ip][iq].imag);
                //LOGI("g = %g\n", g);
                
                
                //if (i > 4 && std::abs(d[ip] + g) == std::abs(d[ip]) &&
                //    (std::abs(d[iq]) + g) == std::abs(d[iq])) 
                
                //if ((i > 4) && 
                //    ( fabsf(d[ip]  + g) == fabsf(d[ip])) &&
                //    ((fabsf(d[iq]) + g) == fabsf(d[iq])) ) 
                //if ((i > 4) && (g <= ( __FLT_EPSILON__ )))
                //if ((i > 4) && (g <= 1.0e-10))
                if ((i > 4) && (g <= 5.0e-9))
                //if ((i > 4) && 
                //    (fabsf( fabsf(d[ip]  + g) - fabsf(d[ip])) < (__FLT_EPSILON__ / 2.0)) &&
                //    (fabsf((fabsf(d[iq]) + g) - fabsf(d[iq])) < (__FLT_EPSILON__ / 2.0)) ) 
                {
                    //a(ip, iq) = 0.0;
                    a[ip][iq].real = 0.0f;
                    a[ip][iq].imag = 0.0f;
                    
                    //デバック
                    //LOGI("a(ip,iq) = (0, 0)\n");
                }
                else if (abs_a > thresh){
                    //h.real = d[iq] - d[ip];
                    //h.imag = 0.0f;
                    tmp = d[iq] - d[ip]; // hの実部(tmp)しか使っていない

                    //デバック
                    //LOGI("h = %g , %g\n", h.real, h.imag);
                    
                    //if ((std::abs(h) + g) == std::abs(h)) 
                    //if((fabs(h.real) + g) == fabs(h.real)) // この時点では h は実部しか持たない
                    if(g <= 1.0e-8){ // この時点では h は実部しか持たない
                        //t = conj(a(ip, iq)) / h;
                        //t.real =  a[ip][iq].real / h.real;
                        //t.imag = -a[ip][iq].imag / h.real;
                        t.real =  a[ip][iq].real / tmp; // hの実部(tmp)しか使っていない
                        t.imag = -a[ip][iq].imag / tmp; // hの実部(tmp)しか使っていない

                        //ktakagi
                        //LOGI("t = conj(a(ip, iq)) / h : t = %g, %g\n",  t.real, t.imag);
                    }
                    else{
                        //t = (-h.real() + std::sqrt(h.real() * h.real() +
                        //               T(4.0) * std::norm(a(ip, iq)))) / (T(2.0) * a(ip, iq));
                        norm_a  = a[ip][iq].real * a[ip][iq].real;
                        norm_a += a[ip][iq].imag * a[ip][iq].imag;
                        //t_tmp  = -h.real + sqrt(h.real * h.real + 4.0 * norm_a);
                        //t_tmp  = -(d[iq] - d[ip]) + sqrt((double)(d[iq] - d[ip]) * (double)(d[iq] - d[ip]) + 4.0 * norm_a);
                        //t_tmp  = -(d[iq] - d[ip]) + sqrtf((d[iq] - d[ip]) * (d[iq] - d[ip]) + 4.0f * norm_a);
                        t_tmp  = -tmp + sqrtf(tmp * tmp + 4.0f * norm_a); // h.real=>(d[iq] - d[ip])=>tmp
                        //if( fabs(t_tmp) < 1.0e-9)
                        //{
                        //    t_tmp = 0.0;  //試し
                        //}
                        t.real =  t_tmp * a[ip][iq].real / (2.0f * norm_a);
                        t.imag = -t_tmp * a[ip][iq].imag / (2.0f * norm_a);
                        
                        //ktakagi
                        //LOGI("%g\n", -(d[iq] - d[ip]) + sqrt((double)(d[iq] - d[ip]) * (double)(d[iq] - d[ip]) + 4.0 * norm_a));
                        //LOGI("norm_a = %g, t_tmp = %g, t = %g, %g\n",  norm_a, t_tmp, t.real, t.imag);
                    }
                    
                    //デバック
                    //LOGI("t = %g , %g\n", t.real, t.imag);
                    //LOGI("a = %g , %g\n", a[ip][iq].real, a[ip][iq].imag);
                    
                    //c = 1.0 / sqrt(1 + norm(t));
                    norm_t = t.real * t.real + t.imag * t.imag;
                    c = 1.0 / sqrtf(1.0 + norm_t);
                    //s = t * c;
                    s.real = t.real * c;
                    s.imag = t.imag * c;
                    //tau = s / (float)(1.0 + c);
                    tau.real = s.real / (float)(1.0f + c);
                    tau.imag = s.imag / (float)(1.0f + c);
                    //tau.real = t.real * c / (float)(1.0 + c);
                    //tau.imag = t.imag * c / (float)(1.0 + c);
                    //h.real  = t.real * a[ip][iq].real;
                    //h.real -= t.imag * a[ip][iq].imag;
                    //h.imag  = t.real * a[ip][iq].imag;
                    //h.imag += t.imag * a[ip][iq].real;
                    tmp  = t.real * a[ip][iq].real; // hの実部(tmp)しか使っていない
                    tmp -= t.imag * a[ip][iq].imag; // hの実部(tmp)しか使っていない
                    //z[ip] -= h.real;
                    //z[iq] += h.real;
                    //d[ip] -= h.real;
                    //d[iq] += h.real;
                    z[ip] -= tmp; // hの実部(tmp)しか使っていない
                    z[iq] += tmp; // hの実部(tmp)しか使っていない
                    d[ip] -= tmp; // hの実部(tmp)しか使っていない
                    d[iq] += tmp; // hの実部(tmp)しか使っていない
                    a[ip][iq].real = 0.0f;
                    a[ip][iq].imag = 0.0f;
                    
                    //デバック
                    //LOGI("h = %g , %g\n", h.real, h.imag);
                    //LOGI("%g , %g\n", s.real, s.imag);
                    //LOGI("%g , %g\n", tau.real, tau.imag);
                    
                    for(j=0; j<=ip-1; j++){
                        ////rotate(a, j, ip, j, iq, c, s, tau);
                        //std::complex<T> g = a(j, ip);
                        //std::complex<T> h = a(j, iq);
                        g_real = a[j][ip].real;
                        g_imag = a[j][ip].imag;
                        h_real = a[j][iq].real;
                        h_imag = a[j][iq].imag;
                        //a(j, ip) = g - s * (h + g * conj(tau));
                        tmp_real  = h_real;
                        tmp_real += g_real * tau.real;
                        tmp_real += g_imag * tau.imag;
                        tmp_imag  = h_imag;
                        tmp_imag -= g_real * tau.imag;
                        tmp_imag += g_imag * tau.real;
                        tmp  = g_real;
                        tmp -= s.real * tmp_real;
                        tmp += s.imag * tmp_imag;
                        a[j][ip].real = tmp;
                        tmp  = g_imag;
                        tmp -= s.real * tmp_imag;
                        tmp -= s.imag * tmp_real;
                        a[j][ip].imag = tmp;
                        
                        //a(j, iq) = h + conj(s) * (g - h * tau);
                        tmp_real  = g_real;
                        tmp_real -= h_real * tau.real;
                        tmp_real += h_imag * tau.imag;
                        tmp_imag  = g_imag;
                        tmp_imag -= h_real * tau.imag;
                        tmp_imag -= h_imag * tau.real;
                        tmp  = h_real;
                        tmp += s.real * tmp_real;
                        tmp += s.imag * tmp_imag;
                        a[j][iq].real = tmp;
                        tmp  = h_imag;
                        tmp += s.real * tmp_imag;
                        tmp -= s.imag * tmp_real;
                        a[j][iq].imag = tmp;
                    }
                    for(j=ip+1; j<=iq-1; j++){
                        ////rotate(a, ip, j, j, iq, c, s, tau);
                        //std::complex<T> g = a(ip, j);
                        //std::complex<T> h = a(j, iq);
                        g_real = a[ip][j].real;
                        g_imag = a[ip][j].imag;
                        h_real = a[j][iq].real;
                        h_imag = a[j][iq].imag;
                        //a(ip, j) = g - conj(s) * (conj(h) + g * tau);
                        tmp_real  =  h_real;
                        tmp_real +=  g_real * tau.real;
                        tmp_real -=  g_imag * tau.imag;
                        tmp_imag  = -h_imag;
                        tmp_imag +=  g_real * tau.imag;
                        tmp_imag +=  g_imag * tau.real;
                        tmp  = g_real;
                        tmp -= s.real * tmp_real;
                        tmp -= s.imag * tmp_imag;
                        a[ip][j].real = tmp;
                        tmp  = g_imag;
                        tmp -= s.real * tmp_imag;
                        tmp += s.imag * tmp_real;
                        a[ip][j].imag = tmp;
                        //a(j, iq) = h + conj(s) * (conj(g) - h * tau);
                        tmp_real  =  g_real;
                        tmp_real -=  h_real * tau.real;
                        tmp_real +=  h_imag * tau.imag;
                        tmp_imag  = -g_imag;
                        tmp_imag -= h_real * tau.imag;
                        tmp_imag -= h_imag * tau.real;
                        tmp  = h_real;
                        tmp += s.real * tmp_real;
                        tmp += s.imag * tmp_imag;
                        a[j][iq].real = tmp;
                        tmp  = h_imag;
                        tmp += s.real * tmp_imag;
                        tmp -= s.imag * tmp_real;
                        a[j][iq].imag = tmp;
                    }
                    for(j=iq+1; j<n; j++){
                        ////rotate(a, ip, j, iq, j, c, s, tau);
                        //std::complex<T> g = a(ip, j);
                        //std::complex<T> h = a(iq, j);
                        g_real = a[ip][j].real;
                        g_imag = a[ip][j].imag;
                        h_real = a[iq][j].real;
                        h_imag = a[iq][j].imag;
                        //a(ip, j) = g - conj(s) * (h + g * tau);
                        tmp_real  = h_real;
                        tmp_real += g_real * tau.real;
                        tmp_real -= g_imag * tau.imag;
                        tmp_imag  = h_imag;
                        tmp_imag += g_real * tau.imag;
                        tmp_imag += g_imag * tau.real;
                        tmp  = g_real;
                        tmp -= s.real * tmp_real;
                        tmp -= s.imag * tmp_imag;
                        a[ip][j].real = tmp;
                        tmp  = g_imag;
                        tmp -= s.real * tmp_imag;
                        tmp += s.imag * tmp_real;
                        a[ip][j].imag = tmp;
                        //a(iq, j) = h + s * (g - h * conj(tau));
                        tmp_real  = g_real;
                        tmp_real -= h_real * tau.real;
                        tmp_real -= h_imag * tau.imag;
                        tmp_imag  = g_imag;
                        tmp_imag += h_real * tau.imag;
                        tmp_imag -= h_imag * tau.real;
                        tmp  = h_real;
                        tmp += s.real * tmp_real;
                        tmp -= s.imag * tmp_imag;
                        a[iq][j].real = tmp;
                        tmp  = h_imag;
                        tmp += s.real * tmp_imag;
                        tmp += s.imag * tmp_real;
                        a[iq][j].imag = tmp;
                    }
                    for(j=0; j<n; j++){
                        ////rotate(v, j, ip, j, iq, c, s, tau);
                        //std::complex<T> g = v(j, ip);
                        //std::complex<T> h = v(j, iq);
                        g_real = v[j][ip].real;
                        g_imag = v[j][ip].imag;
                        h_real = v[j][iq].real;
                        h_imag = v[j][iq].imag;
                        //LOGI("%d , %d , %d : %g, %g, %g, %g\n", ip, iq, j, g_real, g_imag, h_real, h_imag);
                        
                        //v(j, ip) = g - s * (h + g * conj(tau));
                        tmp_real  = h_real;
                        tmp_real += g_real * tau.real;
                        tmp_real += g_imag * tau.imag;
                        tmp_imag  = h_imag;
                        tmp_imag -= g_real * tau.imag;
                        tmp_imag += g_imag * tau.real;
                        tmp  = g_real;
                        tmp -= s.real * tmp_real;
                        tmp += s.imag * tmp_imag;
                        v[j][ip].real = tmp;
                        tmp  = g_imag;
                        tmp -= s.real * tmp_imag;
                        tmp -= s.imag * tmp_real;
                        v[j][ip].imag = tmp;
//                        LOGI("<%g, %g>\n", tmp_real, tmp_imag);
//                        LOGI("<%g, %g>\n", s.real, s.imag);
//                        LOGI("[%g, %g]\n", s.real * tmp_imag , s.imag * tmp_real);
                        //v(j, iq) = h + conj(s) * (g - h * tau);
                        tmp_real  = g_real;
                        tmp_real -= h_real * tau.real;
                        tmp_real += h_imag * tau.imag;
                        tmp_imag  = g_imag;
                        tmp_imag -= h_real * tau.imag;
                        tmp_imag -= h_imag * tau.real;
                        tmp  = h_real;
                        tmp += s.real * tmp_real;
                        tmp += s.imag * tmp_imag;
                        v[j][iq].real = tmp;
                        tmp  = h_imag;
                        tmp += s.real * tmp_imag;
                        tmp -= s.imag * tmp_real;
                        v[j][iq].imag = tmp;
                    }
#if 0 // vを要確認
                    {
                        int ipp, iqq;
                        LOGI("-- %d, %d : %d --\n", i, ip, iq);
                        for(ipp = 0; ipp < n; ipp++){
                            for(iqq = 0; iqq < n; iqq++){
                                LOGI("(%g,%g) ", a[ipp][iqq].real, a[ipp][iqq].imag);
                                //LOGI("(%g,%g) ", v[ipp][iqq].real, v[ipp][iqq].imag);
                            }
                            LOGI("\n");
                        }
                        LOGI("=====================\n");
                        for(ipp = 0; ipp < n; ipp++){
                            for(iqq = 0; iqq < n; iqq++){
                                //LOGI("(%g,%g) ", a[ipp][iqq].real, a[ipp][iqq].imag);
                                LOGI("(%g,%g) ", v[ipp][iqq].real, v[ipp][iqq].imag);
                            }
                            LOGI("\n");
                        }
                        LOGI("#####################\n");
                    }
#endif
                }
            }
        }
#if 0
        {
            int ipp, iqq;
            for(ipp = 0; ipp < n; ipp++){
                for(iqq = 0; iqq < n; iqq++){
                    LOGI("(%g,%g) ", a[ipp][iqq].real, a[ipp][iqq].imag);
                }
                LOGI("\n");
            }
            LOGI("\n");
        }
#endif
        for(ip=0; ip<n; ip++){
            //LOGI("z[%d] = %g\n", ip, z[ip]);
            b[ip] += z[ip];
            d[ip] = b[ip];
            z[ip] = 0.0;
        }
    }
    
    // sorting eigenvalues in ascending order
    for(i=0; i<n-1; i++){
        float min = d[i];
        int imin = i;
        for(j=i+1; j<n; j++){
            if(min > d[j]){
                min = d[j];
                imin = j;
            }
        }
        if(imin != i){
            //std::swap(d[i], d[imin]);
            d_tmp   = d[i];
            d[i]    = d[imin];
            d[imin] = d_tmp;
            //ublas::column(v, i).swap(ublas::column(v, imin));
            for(j=0; j<n; j++){
                v_tmp           = v[j][i].real;
                v[j][i].real    = v[j][imin].real;
                v[j][imin].real = t_tmp;
                
                v_tmp           = v[j][i].imag;
                v[j][i].imag    = v[j][imin].imag;
                v[j][imin].imag = t_tmp;
            }
        }
    }
}

/************************************************************
**  History :
**    2013.04.03 K.Takagi マージ（計算処理をチューニング）
**    2013.03.29 K.Takagi マージ（無駄な代入処理をコメントアウト）
**    2013.03.07 K.Takagi doubleの計算をすべてfloatに変更
**    2012.01.08 K.Takagi 計算ミスを修正
**    2013.01.07 K.Takagi 新規作成
*/
// Public : 固有値展開　svd(TempCalc2, REigenValue, TempCalc4, TempCalc3) で呼び出し
float ABS(float a)
{
    return (a < 0.0f ? -a : a > 0.0f ? a : 0.0f);
}

static void Svd(COMPLEX_FLOAT **a, float *s, COMPLEX_FLOAT **u, COMPLEX_FLOAT **v)
{
    int m;
    int n;
    int p;  // transformation Conj(Tran(u)) is applied to the p vectors given in columns n, n+1, ..., n+p-1 of matrix a
    int nu; // considered number of cols of u
    int nv; // considered number of cols of v
    
    float b[16];
    float c[16];
    float t[16];
    
    COMPLEX_FLOAT q;
    COMPLEX_FLOAT r;
    int i, j, k, k1, L, L1, nM1, np;
    
    float cs, eps, eta, f, g, h;
    float sn;
    float tol, w, x, y, z;
    float tmp, tmp2, tmp_real, tmp_imag;
    //float f_real;
    //float f_imag;

    //m = amatrix_info.mic_num;
    //n = amatrix_info.mic_num;
    m = localize_music_info.tf_channel_num;
    n = localize_music_info.tf_channel_num;
    p = 0;
    nu = m;
    nv = n;
    
    eta = 1.5E-07F;
    tol = 1.5E-31F;
    np  = n + p;
    nM1 = n - 1;
    L   = 0;
    // HOUSEHOLDER REDUCTION
    c[0] = 0.0F;
    k = 0;
    
    //デバック
    //LOGI("(%f, %f), (%f, %f)\n", a[0][0].real, a[0][0].imag, a[0][1].real, a[0][1].imag);

    while(1){
        k1 = k + 1;
        // ELIMINATION OF a(i,k), i = k, ..., m-1
        z = 0.0F;
        
        for(i = k; i < m; i++){
    	    //z += norm(a(i,k));
    	    z += a[i][k].real * a[i][k].real;
    	    z += a[i][k].imag * a[i][k].imag;
    	}
    	//デバック
    	//LOGI("%d : %g\n", k, z);
    	
        b[k] = 0.0F;
        if(z > tol){
            //z = (float)sqrt(z);
            z = sqrtf(z);
            b[k] = z;
            
            //w = abs(a(k,k));
            //w = sqrt(a[k][k].real * a[k][k].real + a[k][k].imag * a[k][k].imag);
            w = sqrtf(a[k][k].real * a[k][k].real + a[k][k].imag * a[k][k].imag);
            
            if(w != 0.0F){
                //q = a(k,k) / w;
                //q.real = a[k][k].real / w;
                //q.imag = a[k][k].imag / w;
                tmp = 1.0f / w;
                q.real = a[k][k].real * tmp;
                q.imag = a[k][k].imag * tmp;
            }
            else{
                q.real = 1.0f;
                q.imag = 0.0f;
            }
            //a(k,k) = q * (z + w);
            //a[k][k].real = q.real * (z + w);
            //a[k][k].imag = q.imag * (z + w);
            tmp = (z + w);
            a[k][k].real = q.real * tmp;
            a[k][k].imag = q.imag * tmp;
            tmp = 1.0f / (tmp * z);
            
            if(k != np - 1){
                for(j=k1; j<np; j++){
                    //q = std::complex<T>(0.0F, 0.0F);
                    q.real = 0.0f;
                    q.imag = 0.0f;
                    
                    for (i=k; i<m; i++){
                        //q += conj(a(i,k)) * a(i,j);
                        q.real += a[i][k].real * a[i][j].real;
                        q.real += a[i][k].imag * a[i][j].imag;
                        q.imag += a[i][k].real * a[i][j].imag;
                        q.imag -= a[i][k].imag * a[i][j].real;
                    }
                    
                    //q /= z * (z + w);
                    ////q.real = q.real / (z * (z + w));
                    ////q.imag = q.imag / (z * (z + w));
                    q.real *= tmp;
                    q.imag *= tmp;
                    for(i=k; i<m; i++){
                        //a(i,j) -= q * a(i,k);
                        tmp_real  = q.real * a[i][k].real;
                        tmp_real -= q.imag * a[i][k].imag;
                        tmp_imag  = q.real * a[i][k].imag;
                        tmp_imag += q.imag * a[i][k].real;
                        a[i][j].real -= tmp_real;
                        a[i][j].imag -= tmp_imag;
                    }
                }
            }
            // PHASE TRANSFORMATION
            //q = -conj(a(k,k)) / abs(a(k,k));
            //w = sqrt(a[k][k].real * a[k][k].real + a[k][k].imag * a[k][k].imag);
            w = sqrtf(a[k][k].real * a[k][k].real + a[k][k].imag * a[k][k].imag);
            //q.real = -a[k][k].real / w;
            //q.imag =  a[k][k].imag / w;
            tmp = 1.0f / w;
            q.real = -a[k][k].real * tmp;
            q.imag =  a[k][k].imag * tmp;
            
            //デバック
            //LOGI("q = (%g,%g)\n", q.real, q.imag);
            
            for(j=k1; j<np; j++){
                //a(k,j) *= q;
                tmp_real  = a[k][j].real * q.real;
                tmp_real -= a[k][j].imag * q.imag;
                tmp_imag  = a[k][j].real * q.imag;
                tmp_imag += a[k][j].imag * q.real;
                a[k][j].real = tmp_real;
                a[k][j].imag = tmp_imag;
            }
        }
#if 0
        //デバック
        LOGI("(%g,%g), (%g,%g), (%g,%g), (%g,%g)\n",
                a[0][0].real, a[0][0].imag,
                a[0][1].real, a[0][1].imag,
                a[0][2].real, a[0][2].imag,
                a[0][3].real, a[0][3].imag);
        LOGI("(%g,%g), (%g,%g), (%g,%g), (%g,%g)\n",
                a[1][0].real, a[1][0].imag,
                a[1][1].real, a[1][1].imag,
                a[1][2].real, a[1][2].imag,
                a[1][3].real, a[1][3].imag);
        LOGI("(%g,%g), (%g,%g), (%g,%g), (%g,%g)\n",
                a[2][0].real, a[2][0].imag,
                a[2][1].real, a[2][1].imag,
                a[2][2].real, a[2][2].imag,
                a[2][3].real, a[2][3].imag);
#endif
        // ELIMINATION OF a(k][j], j = k+2, ..., n-1
        if (k == nM1) break;
        z = 0.0F;
        for(j=k1; j<n; j++){
            //z += norm(a(k,j));
            z += a[k][j].real * a[k][j].real;
            z += a[k][j].imag * a[k][j].imag;
        }
        
        c[k1] = 0.0F;
        if(z > tol){
            //z = (float)sqrt(z);
            z = sqrtf(z);
            c[k1] = z;
            
            //w = abs(a(k,k1));
            //w = sqrt(a[k][k1].real * a[k][k1].real + a[k][k1].imag * a[k][k1].imag);
            w = sqrtf(a[k][k1].real * a[k][k1].real + a[k][k1].imag * a[k][k1].imag);
            
            if(w != 0.0F){
                //q = a(k,k1) / w;
                //q.real = a[k][k1].real / w;
                //q.imag = a[k][k1].imag / w;
                tmp = 1.0f / w;
                q.real = a[k][k1].real * tmp;
                q.imag = a[k][k1].imag * tmp;
            }
            else{
                q.real = 1.0f;
                q.imag = 0.0f;
            }
            //a(k,k1) = q * (z + w);
            tmp = (z + w);
            ////a[k][k1].real = q.real * (z + w);
            ////a[k][k1].imag = q.imag * (z + w);
            a[k][k1].real = q.real * tmp;
            a[k][k1].imag = q.imag * tmp;
            tmp = 1.0f / (tmp * z);
            
            for(i=k1; i<m; i++){
                //q = std::complex<T>(0.0F, 0.0F);
                q.real = 0.0f;
                q.imag = 0.0f;
                
                for(j=k1; j<n; j++){
	                //q += conj(a(k,j)) * a(i,j);
	                q.real += a[k][j].real * a[i][j].real;
	                q.real += a[k][j].imag * a[i][j].imag;
	                q.imag += a[k][j].real * a[i][j].imag;
	                q.imag -= a[k][j].imag * a[i][j].real;
                }
                
                //q /= z * (z + w);
                ////q.real = q.real / (z * (z + w));
                ////q.imag = q.imag / (z * (z + w));
                q.real *= tmp;
                q.imag *= tmp;
                for(j=k1; j<n; j++){
	                //a(i,j) -= q * a(k,j);
	                tmp_real  = q.real * a[k][j].real;
	                tmp_real -= q.imag * a[k][j].imag;
	                tmp_imag  = q.real * a[k][j].imag;
	                tmp_imag += q.imag * a[k][j].real;
	                a[i][j].real -= tmp_real;
	                a[i][j].imag -= tmp_imag;
	            }
            }
            // PHASE TRANSFORMATION
            //q = -conj(a(k,k1)) / abs(a(k,k1));
            //w = sqrt(a[k][k1].real * a[k][k1].real + a[k][k1].imag * a[k][k1].imag);
            w = sqrtf(a[k][k1].real * a[k][k1].real + a[k][k1].imag * a[k][k1].imag);
            q.real = -a[k][k1].real / w;
            q.imag =  a[k][k1].imag / w;
            for(i=k1; i<m; i++){
                //a(i,k1) *= q;
                tmp_real  = a[i][k1].real * q.real;
                tmp_real -= a[i][k1].imag * q.imag;
                tmp_imag  = a[i][k1].real * q.imag;
                tmp_imag += a[i][k1].imag * q.real;
                a[i][k1].real = tmp_real;
                a[i][k1].imag = tmp_imag;
            }
        }
        k = k1;
    } /* End of while(1) */
    
#if 0
    //デバック
    for(i=0; i<8; i++){
        LOGI("(%g,%g), (%g,%g), (%g,%g), (%g,%g), (%g,%g), (%g,%g), (%g,%g), (%g,%g)\n",
                a[i][0].real, a[i][0].imag,
                a[i][1].real, a[i][1].imag,
                a[i][2].real, a[i][2].imag,
                a[i][3].real, a[i][3].imag,
                a[i][4].real, a[i][4].imag,
                a[i][5].real, a[i][5].imag,
                a[i][6].real, a[i][6].imag,
                a[i][7].real, a[i][7].imag);
    }
#endif
    // TOLERANCE FOR NEGLIGIBLE ELEMENTS
    eps = 0.0F;
    for(k=0; k<n; k++){
        s[k] = b[k];
        t[k] = c[k];
        if(s[k] + t[k] > eps){
            eps = s[k] + t[k];
        }
    }
    eps *= eta;
    //デバック
    //LOGI("eps = %g\n", eps);
    
    // INITIALIZATION OF u AND v
    if(nu > 0){
        //for(j=0; j<nu; j++){
            //for(i=0; i<m; i++){
        for(i=0; i<m; i++){
            for(j=0; j<nu; j++){
                //u(i,j) = std::complex<T>(0.0F, 0.0F);
                u[i][j].real = 0.0f;
                u[i][j].imag = 0.0f;
            }
            //u(j,j) = std::complex<T>(1.0F, 0.0F);
            //u[j][j].real = 1.0f; // skip because already set! u[j][j].imag = 0.0f;
            u[i][i].real = 1.0f; // swap loop i,j;
        }
    }
    
    if(nv > 0){
        //for(j=0; j<nv; j++){
            //for(i=0; i<n; i++){
        for(i=0; i<n; i++){
            for(j=0; j<nv; j++){
                //v(i,j) = std::complex<T>(0.0F, 0.0F);
                v[i][j].real = 0.0f;
                v[i][j].imag = 0.0f;
            }
            //v(j,j) = std::complex<T>(1.0F, 0.0F);
            //v[j][j].real = 1.0f; // skip because already set! v[j][j].imag = 0.0f;
            v[i][i].real = 1.0f; // swap loop i,j;
        }
    }
    //QR DIAGONALIZATION
    for(k=nM1; k>=0; k--){
        //デバック
        //LOGI("k = %d\n", k);

        //TEST FOR SPLIT
        while(1){
            for(L=k; L>=0; L--){
                //デバック
	            //LOGI("%d : %g, %g, eps = %g\n", L, t[L], s[L-1], eps);
                if (ABS(t[L]) <= eps) goto Test;
                if(L > 0){
                    if (ABS(s[L - 1]) <= eps) break;
                }
            }

            //CANCELLATION OF E(L)
            cs = 0.0F;
            sn = 1.0F;
            L1 = L - 1;
            for(i=L; i<=k; i++){
                f = sn * t[i];
                t[i] *= cs;
                
                //デバック
                //LOGI("%d : %g <= %g ?\n", i, f, eps);
                
                if (ABS(f) <= eps) goto Test;
                
                h = s[i];
                //w = (float)sqrt(f * f + h * h);
                w = sqrtf(f * f + h * h);
                s[i] = w;
                //cs = h / w;
                //sn = -f / w;
                tmp = 1.0f / w;
                cs = h * tmp;
                sn = -f * tmp;
                
                if(nu > 0){
                    for(j=0; j<n; j++){
                        //x = real(u(j,L1));
                        //y = real(u(j,i));
                        x = u[j][L1].real;
                        y = u[j][i].real;
                        
                        //u(j,L1) = std::complex<T>(x * cs + y * sn, 0.0F);
                        tmp  = x * cs;
                        tmp += y * sn;
                        u[j][L1].real = tmp;
                        u[j][L1].imag = 0.0f;
                        
                        //u(j,i) = std::complex<T>(y * cs - x * sn, 0.0F);
                        tmp  = y * cs;
                        tmp -= x * sn;
                        u[j][i].real = tmp;
                        u[j][i].imag = 0.0f;
                    }
	            }
	            
                if(np == n) continue;
                
                for(j=n; j<np; j++){
	                //q = a(L1,j);
	                q = a[L1][j];
	                //r = a(i,j);
	                r = a[i][j];
	                
	                //a(L1,j) = q * cs + r * sn;
	                tmp  = q.real * cs;
	                tmp += r.real * sn;
	                a[L1][j].real = tmp;
	                tmp  = q.imag * cs;
	                tmp += r.imag * sn;
	                a[L1][j].imag = tmp;
	                
	                //a(i,j) = r * cs - q * sn;
	                tmp  = r.real * cs;
	                tmp -= q.real * sn;
	                a[i][j].real = tmp;
	                tmp  = r.imag * cs;
	                tmp -= q.imag * sn;
	                a[i][j].imag = tmp;
	            }
            }

            //TEST FOR CONVERGENCE
Test:	    w = s[k];
            if (L == k) break;

            //ORIGIN SHIFT
            x = s[L];
            y = s[k - 1];
            g = t[k - 1];
            h = t[k];
            f = ((y - w) * (y + w) + (g - h) * (g + h)) / (2.0F * h * y);
            //g = (float)sqrt(f * f + 1.0F);
            g = sqrtf(f * f + 1.0F);
            if(f < 0.0F){ g = -g; }
            f = ((x - w) * (x + w) + (y / (f + g) - h) * h) / x;
  
            //QR STEP
            cs = 1.0F;
            sn = 1.0F;
            L1 = L + 1;
            for(i=L1; i<=k; i++){
                g = t[i];
                y = s[i];
                h = sn * g;
                g = cs * g;
                //w = (float)sqrt(h * h + f * f);
                w = sqrtf(h * h + f * f);
                t[i - 1] = w;
                //cs = f / w;
                //sn = h / w;
                tmp = 1.0f / w;
                cs = f * tmp;
                sn = h * tmp;
                f  = x * cs;
                f += g * sn;
                g  = g * cs;
                g -= x * sn;
                h  = y * sn;
                y  = y * cs;
                if(nv > 0){
	                for(j=0; j<n; j++){
                        //x = real(v(j,i - 1));
                        //w = real(v(j,i));
                        x = v[j][i-1].real;
                        w = v[j][i].real;
                        
                        //v(j,i - 1) = std::complex<T>(x * cs + w * sn, 0.0F);
                        tmp  = x * cs;
                        tmp += w * sn;
                        v[j][i-1].real = tmp;
                        v[j][i-1].imag = 0.0f;
                        //v(j,i) = std::complex<T>(w * cs - x * sn, 0.0F);
                        tmp  = w * cs;
                        tmp -= x * sn;
                        v[j][i].real = tmp;
                        v[j][i].imag = 0.0f;
                    }
                }
                //w = (float)sqrt(h * h + f * f);
                w = sqrtf(h * h + f * f);
                s[i - 1] = w;
                //cs = f / w;
                //sn = h / w;
                tmp = 1.0f / w;
                cs = f * tmp;
                sn = h * tmp;
                f = cs * g + sn * y;
                x = cs * y - sn * g;
                if(nu > 0){
                    for(j=0; j<n; j++){
                        //y = real(u(j,i - 1));
                        //w = real(u(j,i));
                        y = u[j][i-1].real;
                        w = u[j][i].real;
                        
                        //u(j,i - 1) = std::complex<T>(y * cs + w * sn, 0.0F);
                        tmp  = y * cs;
                        tmp += w * sn;
                        u[j][i-1].real = tmp;
                        u[j][i-1].imag = 0.0f;
                        //u(j,i) = std::complex<T>(w * cs - y * sn, 0.0F);
                        tmp  = w * cs;
                        tmp -= y * sn;
                        u[j][i].real = tmp;
                        u[j][i].imag = 0.0f;
                    }
                }
	            
	            if (n == np) continue;
	            
	            for(j=n; j<np; j++){
                    //q = a(i - 1,j);
                    //r = a(i,j);
                    q = a[i-1][j];
                    r = a[i][j];
                    
                    //a(i - 1,j) = q * cs + r * sn;
                    tmp  = q.real * cs;
                    tmp += r.real * sn;
                    a[i-1][j].real = tmp;
                    tmp  = q.imag * cs;
                    tmp += r.imag * sn;
                    a[i-1][j].imag = tmp;
                    //a(i,j) = r * cs - q * sn;
                    tmp  = r.real * cs;
                    tmp -= q.real * sn;
                    a[i][j].real = tmp;
                    tmp  = r.imag * cs;
                    tmp -= q.imag * sn;
                    a[i][j].imag = tmp;
                }
            }
	        t[L] = 0.0F;
	        t[k] = f;
	        s[k] = x;
	    } /* End of while(1) */
	    
        //CONVERGENCE
        if (w >= 0.0F) continue;
        
        s[k] = -w;
        
        if (nv == 0) continue;
        
        for(j=0; j<n; j++){
	        //v(j,k) = -v(j,k);
	        v[j][k].real = -v[j][k].real;
	        v[j][k].imag = -v[j][k].imag;
	    }
    }
    
    //SORT SINGULAR VALUES
    for(k=0; k<n; k++){	/* sort descending */
        g = -1.0F;
        j = k;
        for(i=k; i<n; i++){	/* sort descending */
            if (s[i] <= g) continue;
            
            g = s[i];
            j = i;
        }
        
        if (j == k) continue;
        
        s[j] = s[k];
        s[k] = g;
        if(nv > 0){
            for(i=0; i<n; i++){
                //q = v(i,j);
                //v(i,j) = v(i,k);
                //v(i,k) = q;
                q = v[i][j];
                v[i][j] = v[i][k];
                v[i][k] = q;
            }
        }
        
        if(nu > 0){
            for(i=0; i<n; i++){
                //q = u(i,j);
                //u(i,j) = u(i,k);
                //u(i,k) = q;
                q = u[i][j];
                u[i][j] = u[i][k];
                u[i][k] = q;
            }
        }
        
        if (n == np) continue;
        
        for(i=n; i<np; i++){
            //q = a(j,i);
            //a(j,i) = a(k,i);
            //a(k,i) = q;
            q = a[j][i];
            a[j][i] = a[k][i];
            a[k][i] = q;
        }
    }

    //BACK TRANSFORMATION
    if(nu > 0){
        for(k=nM1; k>=0; k--){
            if (b[k] == 0.0F) continue;
            
            //w = sqrt(a[k][k].real * a[k][k].real + a[k][k].imag * a[k][k].imag);
            w = sqrtf(a[k][k].real * a[k][k].real + a[k][k].imag * a[k][k].imag);
            //q = -a(k,k) / abs(a(k,k));
            //q.real = -a[k][k].real / w;
            //q.imag = -a[k][k].imag / w;
            tmp2 = 1.0f / (w * b[k]);
            tmp = 1.0f / w;
            q.real = -a[k][k].real * tmp;
            q.imag = -a[k][k].imag * tmp;
            
            for(j=0; j<nu; j++){
                //u(k,j) *= q;
                tmp_real  = u[k][j].real * q.real;
                tmp_real -= u[k][j].imag * q.imag;
                tmp_imag  = u[k][j].real * q.imag;
                tmp_imag += u[k][j].imag * q.real;
                u[k][j].real = tmp_real;
                u[k][j].imag = tmp_imag;
            }
            
            for(j=0; j<nu; j++){
                //q = std::complex<T>(0.0F, 0.0F);
                q.real = 0.0f;
                q.imag = 0.0f;
                for(i=k; i<m; i++){
                    //q += conj(a(i,k)) * u(i,j);
                    q.real += a[i][k].real * u[i][j].real;
                    q.real += a[i][k].imag * u[i][j].imag;
                    q.imag += a[i][k].real * u[i][j].imag;
                    q.imag -= a[i][k].imag * u[i][j].real;
                }
                
                //w = sqrt(a[k][k].real * a[k][k].real + a[k][k].imag * a[k][k].imag);
                //w = sqrtf(a[k][k].real * a[k][k].real + a[k][k].imag * a[k][k].imag); // このループ内でaは更新しないので不要
                //q /= abs(a(k,k)) * b[k];
                //q.real = q.real / (w * b[k]);
                //q.imag = q.imag / (w * b[k]);
                q.real = q.real * tmp2;
                q.imag = q.imag * tmp2;
                
                for(i=k; i<m; i++){
                    //u(i,j) -= q * a(i,k);
                    tmp  = q.real * a[i][k].real;
                    tmp -= q.imag * a[i][k].imag;
                    u[i][j].real -= tmp;
                    tmp  = q.real * a[i][k].imag;
                    tmp += q.imag * a[i][k].real;
                    u[i][j].imag -= tmp;
                }
            }
        }
    }
    
    if(nv > 0 && n > 1){
        for(k=n-2; k>=0; k--){
            k1 = k + 1;
            if (c[k1] == 0.0F) continue;
            
            //w = sqrt(a[k][k1].real * a[k][k1].real + a[k][k1].imag * a[k][k1].imag);
            w = sqrtf(a[k][k1].real * a[k][k1].real + a[k][k1].imag * a[k][k1].imag);
            //q = -conj(a(k,k1)) / abs(a(k,k1));
            //q.real = -a[k][k1].real / w;
            //q.imag =  a[k][k1].imag / w;
            tmp2 = 1.0f / (w * c[k1]);
            tmp = 1.0f / w;
            q.real = -a[k][k1].real * tmp;
            q.imag =  a[k][k1].imag * tmp;
            
            for(j=0; j<nv; j++){
                //v(k1,j) *= q;
                tmp_real  = v[k1][j].real * q.real;
                tmp_real -= v[k1][j].imag * q.imag;
                tmp_imag  = v[k1][j].real * q.imag;
                tmp_imag += v[k1][j].imag * q.real;
                v[k1][j].real = tmp_real;
                v[k1][j].imag = tmp_imag;
            }
            
            for(j=0; j<nv; j++){
                //q = std::complex<T>(0.0F, 0.0F);
                q.real = 0.0f;
                q.imag = 0.0f;
                
                for(i=k1; i<n; i++){
                    //q += a(k,i) * v(i,j);
                    q.real += a[k][i].real * v[i][j].real;
                    q.real -= a[k][i].imag * v[i][j].imag;
                    q.imag += a[k][i].real * v[i][j].imag;
                    q.imag += a[k][i].imag * v[i][j].real;
                }
                
                //w = sqrt(a[k][k1].real * a[k][k1].real + a[k][k1].imag * a[k][k1].imag);
                //w = sqrtf(a[k][k1].real * a[k][k1].real + a[k][k1].imag * a[k][k1].imag); // このループ内でaは更新しないので不要
                //q = q / (abs(a(k,k1)) * c[k1]);
                //q.real = q.real / (w * c[k1]);
                //q.imag = q.imag / (w * c[k1]);
                q.real = q.real * tmp2;
                q.imag = q.imag * tmp2;
                
                for(i=k1; i<n; i++){
                    //v(i,j) -= q * conj(a(k,i));
                    tmp  =  q.real * a[k][i].real;
                    tmp +=  q.imag * a[k][i].imag;
                    v[i][j].real -= tmp;
                    tmp  = -q.real * a[k][i].imag;
                    tmp +=  q.imag * a[k][i].real;
                    v[i][j].imag -= tmp;
                }
            }
        }
    }

}

/************************************************************
**  History :
**    2013.03.29 K.Takagi マージ
**    2013.01.07 K.Takagi 新規作成
*/
// Public : aiを小さい順に並べ替えと、aiの大きさ順にbiの各行の要素を並べ替え
void SortEigFvecAscending(float *ai, COMPLEX_FLOAT **bi, float *af, COMPLEX_FLOAT **bf)
{
    int i, j, j_next;
    //int m;
    int index[16];
    int i_tmp;
    float f_tmp;
    
    //m = amatrix_info.mic_num;
    const int m = localize_music_info.tf_channel_num;
    
    /*
    for(i=0; i<m; i++){
        LOGI("%f, ", ai[i]);
    }
    LOGI("\n");
    
    for(i=0; i<m; i++){
        LOGI("(%f, %f), ", bi[0][i].real, bi[0][i].imag);
    }
    LOGI("\n");
    */

    for(i=0; i<m; i++){
        af[i] = ai[i];
        index[i] = i;
    }
    
    //for(i=1; i<m; i++){
    for(i=m-1; i>=1; i--){
        //for(j=0, j_next=1; j<m-i; j++, j_next++){
        for(j=0, j_next=1; j<i; j++, j_next++){
            if(af[j] > af[j_next]){
                f_tmp = af[j];
                af[j] = af[j_next];
                af[j_next] = f_tmp;
                
                i_tmp = index[j];
                index[j] = index[j_next];
                index[j_next] = i_tmp;
            }
        }
    }

    for(i=0; i<m; i++){
        for(j=0; j<m; j++){
            bf[i][j] = bi[i][index[j]];
        }
    }
    
    /*
    for(i=0; i<m; i++){
        LOGI("%f, ", af[i]);
    }
    LOGI("\n");
    
    for(i=0; i<m; i++){
        LOGI("(%f, %f), ", bf[0][i].real, bf[0][i].imag);
    }
    LOGI("\n");
    */
}

/************************************************************
**  History : 
**    2013.04.03 K.Takagi マージ（AddNormalizeCorrelationの追加など）
**    2013.03.29 K.Takagi マージ（処理のスリムアップ）
**    2013.03.06 K.Takagi 平滑化のためのスペクトル加算は各フレームで処理するように修正
**    2013.02.13 K.Takagi sourcesに入れるフレームカウントを修正
**    2012.12.14 K.Takagi 引数追加
**    2012.12.13 K.Takagi 動作確認
**    2012.12.05 K.Takagi 新規作成
*/
// Public : 定位処理　まずはSEVD、ノイズ入力なしで実装
int LocalizeMUSIC(int frame_count, SOURCE_DATA_ARY_t *sources)
{
#ifdef OLD_CALC
    int look_back;
    int look_ahead;
#endif
    int i,j,k;
    int pslength;
    int nb_channel;
    
#ifndef TAKIGAHIRA
    int sum_count;
    float sum_count_1;
#endif
    
    int id, ir;
    
    int n_result;
    
    float theta;
    float phi;
    float cos_phi;
    // deg to rad coef. TAKIGAHIRA
    double rad_coef;
    
    int candidate[SEARCH_DIRECTION_MAX_NUM]; // ピーク候補

#ifdef OLD_CALC
    look_back  = 0;
    look_ahead = localize_music_info.window;
#endif
    const int period        = localize_music_info.period;
    const int direction_num = amatrix_info.direction_num;
    const int range_num     = amatrix_info.range_num;
#ifdef OLD_CALC
    /* 演算するタイミングになってから必要数のスペクトルを加算する方法 */
    
    if((frame_count == 0) || ((frame_count % period) != 0)){
        /* 定位を実行する周期でなければ何もしない */
        //LOGI("skip(%d)\n", frame_count);
        //sources->count      = frame_count;
        sources->count      = frame_count - period;
        sources->source_num = 0;
        
        return 0;
    }
    
    pslength   = localize_music_info.length / 2 + 1;
    nb_channel = localize_music_info.tf_channel_num;
    
    if(localize_music_info.music_algorithm != SEVD){
        {
            /* ノイズ入力なし（単位行列の作成） */
            for(k=0; k<pslength; k++){
                for(i=0; i<nb_channel; i++){
                    for(j=0; j<nb_channel; j++){
                        if(i==j){
                            RxxN[k][i][j].real = 1.0;
                            RxxN[k][i][j].imag = 0.0;
                        }
                        else{
                            RxxN[k][i][j].real = 0.0;
                            RxxN[k][i][j].imag = 0.0;
                        }
                    }
                }
            }
        }
    }
    else{
        // SEVD の場合は RxxN の初期化なし
    }
    
    /* スペクトルの平滑化処理 */
    sum_count = 0;
    for(i=look_back; i<look_ahead; i++){
        // スペクトルデータを平滑化する数だけ戻って取得
        fft_data_ary_ptr = HABufferGetFFTData(frame_count-look_ahead+i);
        if(fft_data_ary_ptr == NULL){
            //LOGI("HABufferGet Error\n");
            return -1;
        }
        else{
            //LOGI("HABufferGet (%d / %d)\n", frame_count, fft_data_ary_ptr->count);
        }
        X = fft_data_ary_ptr->data;
        
        // 直積の計算
        // AddToCorrelation
        AddCorrelation(pslength, nb_channel);
        sum_count++;
    }
    // ここでデータを確認／複数ポイントをピックアップし比較（2012.12.11）

#else
    /* 2013.03.06 平滑化のためのスペクトル加算だけは実行する方法 */

    pslength   = localize_music_info.length / 2 + 1;
    nb_channel = localize_music_info.tf_channel_num;
    
    /* 平滑用にスペクトルの加算 */
    fft_data_ary_ptr = HABufferGetFFTData(frame_count);
    if(fft_data_ary_ptr == NULL){
        //LOGI("HABufferGet Error\n");
        return -1;
    }
    X = fft_data_ary_ptr->data;
    
#ifndef TAKIGAHIRA
    // 直積の計算
    // AddToCorrelation
    AddCorrelation(pslength, nb_channel);
    
    period_count++;
    frame_count += 1; /* 音源情報に記録するフレーム番号の調整 */
    if(period_count != period){
        sources->count      = frame_count - period;
        sources->source_num = 0;
        
        return 0;
    }
    sum_count = period_count;
    period_count = 0;
#else
    period_count++;
    frame_count += 1; /* 音源情報に記録するフレーム番号の調整 */
    if(period_count != period){
        AddCorrelation(pslength, nb_channel);
        sources->count      = frame_count - period;
        sources->source_num = 0;

        return 0;
    }
    AddNormalizeCorrelation(pslength, nb_channel, period_count);
    period_count = 0;
#endif
    if(localize_music_info.music_algorithm != SEVD){
        {
            /* ノイズ入力なし（単位行列の作成） */
            for(k=0; k<pslength; k++){
                for(i=0; i<nb_channel; i++){
                    for(j=0; j<nb_channel; j++){
                        RxxN[k][i][j].real = 0.0f;
                        RxxN[k][i][j].imag = 0.0f;
                    }
                    RxxN[k][i][i].real = 1.0f;
                }
            }
        }
    }
    else{
        // SEVD の場合は RxxN の初期化なし
    }
#endif

#ifndef TAKIGAHIRA
    //NormalizeCorrelation
    sum_count_1 = 1.0f / (float)sum_count;
    for(k=0; k<pslength; k++){
        if(UsesFreq[k]){
            for(i=0; i<nb_channel; i++){
                for(j=0; j<nb_channel; j++){
                    Rxx[k][i][j].real = Rxx[k][i][j].real * sum_count_1;
                    Rxx[k][i][j].imag = Rxx[k][i][j].imag * sum_count_1;
                }
            }
        }
    }
    // ここでデータを確認／複数ポイントをピックアップし比較（2012.12.11）
#if 0
    // デバック
    LOGI("Rxx[16][0][0] = %g, %g\n", Rxx[16][0][0].real, Rxx[16][0][0].imag);
    LOGI("Rxx[16][0][1] = %g, %g\n", Rxx[16][0][1].real, Rxx[16][0][1].imag);
    for(k=0; k<pslength; k++){
        for(i=0; i<nb_channel; i++){
            for(j=0; j<nb_channel; j++){
                Rxx[k][i][j].real = 0.0;
                Rxx[k][i][j].imag = 0.0;
                RxxN[k][i][j].real = 0.0;
                RxxN[k][i][j].imag = 0.0;
            }
        }
    }
    return 0;    
    // デバック
#endif
    // localizer->Localize
    {
        for(k=0; k<pslength; k++){
            if(!UsesFreq[k]){
                continue;
            }
            
            // Rxxの正規化（周波数単位）
            {
                float f_max;
                float f_tmp;
                f_max = 0.0;
                
                // MaxOfAbsValue
                for(i=0; i<nb_channel; i++){
                    for(j=0; j<nb_channel; j++){
                        f_tmp = fabsf(Rxx[k][i][j].real);
                        if(f_tmp > f_max){
                            f_max = f_tmp;
                        }
                    }
                }
                // ここでデータを確認／複数ポイントをピックアップし比較（2012.12.11）
                // デバック
                //LOGI("f_max = %g\n", f_max);
                
                f_max += 1.0f;
                for(i=0; i<nb_channel; i++){
                    for(j=0; j<nb_channel; j++){
                        R[i][j].real = Rxx[k][i][j].real / f_max;
                        R[i][j].imag = Rxx[k][i][j].imag / f_max;
                    }
                }
            }
#if 0
            // ここでデータを確認／複数ポイントをピックアップし比較（2012.12.11）
            // デバック
            LOGI("k = %d\n", k);
            LOGI("R[0][0] = %g, %g\n", R[0][0].real, R[0][0].imag);
            LOGI("R[0][1] = %g, %g\n", R[0][1].real, R[0][1].imag);
#endif // End of TAKIGAHIRA
#else
    // ここでデータを確認／複数ポイントをピックアップし比較（2012.12.11）
#if 0
    // デバック
    LOGI("Rxx[16][0][0] = %g, %g\n", Rxx[16][0][0].real, Rxx[16][0][0].imag);
    LOGI("Rxx[16][0][1] = %g, %g\n", Rxx[16][0][1].real, Rxx[16][0][1].imag);
    for(k=0; k<pslength; k++){
        for(i=0; i<nb_channel; i++){
            for(j=0; j<nb_channel; j++){
                Rxx[k][i][j].real = 0.0;
                Rxx[k][i][j].imag = 0.0;
                RxxN[k][i][j].real = 0.0;
                RxxN[k][i][j].imag = 0.0;
            }
        }
    }
    return 0;
    // デバック
#endif
    // localizer->Localize
    {
        for(k=0; k<pslength; k++){
            if(!UsesFreq[k]){
                continue;
            }

            // Rxxの正規化（周波数単位）
            {
                float f_max;
                float f_tmp;
                f_max = 0.0;
                // MaxOfAbsValue
                for(i=0; i<nb_channel; i++){
                    for(j=0; j<nb_channel; j++){
                        f_tmp = fabsf(Rxx[k][i][j].real);
                        if(f_tmp > f_max){
                            f_max = f_tmp;
                        }
                    }
                }
                // ここでデータを確認／複数ポイントをピックアップし比較（2012.12.11）
                // デバック
                //LOGI("f_max = %g\n", f_max);

                f_max = 1.0f / (f_max + 1.0f);
                for(i=0; i<nb_channel; i++){
                    for(j=0; j<nb_channel; j++){
                        R[i][j].real = Rxx[k][i][j].real * f_max;
                        R[i][j].imag = Rxx[k][i][j].imag * f_max;
                    }
                }
            }
#if 0
            // ここでデータを確認／複数ポイントをピックアップし比較（2012.12.11）
            // デバック
            LOGI("k = %d\n", k);
            LOGI("R[0][0] = %g, %g\n", R[0][0].real, R[0][0].imag);
            LOGI("R[0][1] = %g, %g\n", R[0][1].real, R[0][1].imag);
#endif
#endif
            // TAKIGAHIRA 使っていないのでコメントアウト
            //if(localize_music_info.music_algorithm != SEVD){
            //    // RxxNの正規化
            //    // MaxOfAbsValue
            //    
            //    // ノイズ入力なしは単位行列なので正規化不要
            //}
            
            if(localize_music_info.music_algorithm == SEVD){
                //LOGI("\nk = %d\n", k);
                // evd(R, EigenValue[k], V)
                Evd(R, EigenValue[k], V);
            }
            else if(localize_music_info.music_algorithm == GEVD){
                // 非サポート
            }
            else if(localize_music_info.music_algorithm == GSVD){
                // invMAT(RN,TempCalc1);
                // noalias(TempCalc2) = prod(TempCalc1, R);
                // 単位行列の逆行列は単位行列、また単位行列との積で行列は変化しない

                // svd(TempCalc2, REigenValue, TempCalc4, TempCalc3); --> svd(R, REigenValue, TempCalc4, TempCalc3)
                Svd(R, REigenValue, TempCalc4, TempCalc3);
#if 0
                //デバック
                LOGI("REigenValue = %g, %g, %g, %g, %g, %g, %g, %g\n",
                    REigenValue[0], REigenValue[1], REigenValue[2], REigenValue[3], REigenValue[4], REigenValue[5], REigenValue[6], REigenValue[7]);
                LOGI("TempCalc4 = \n");
                for(i=0; i<8; i++){
                    LOGI("(%g, %g), (%g, %g), (%g, %g), (%g, %g), (%g, %g), (%g, %g), (%g, %g), (%g, %g)\n",
                            TempCalc4[i][0].real, TempCalc4[i][0].imag,
                            TempCalc4[i][1].real, TempCalc4[i][1].imag,
                            TempCalc4[i][2].real, TempCalc4[i][2].imag,
                            TempCalc4[i][3].real, TempCalc4[i][3].imag,
                            TempCalc4[i][4].real, TempCalc4[i][4].imag,
                            TempCalc4[i][5].real, TempCalc4[i][5].imag,
                            TempCalc4[i][6].real, TempCalc4[i][6].imag,
                            TempCalc4[i][7].real, TempCalc4[i][7].imag
                            );
                }
#endif
                // sort_eig_fvec_ascending(REigenValue, TempCalc4, EigenValue[k], V);
                SortEigFvecAscending(REigenValue, TempCalc4, EigenValue[k], V);
            }
            
            // E[k] = V
            for(i=0; i<nb_channel; i++){
                for(j=0; j<nb_channel; j++){
                    E[k][i][j].real = V[i][j].real;
                    E[k][i][j].imag = V[i][j].imag;
                    //LOGI("(%g, %g) ", V[i][j].real, V[i][j].imag);
                }
                // デバック
                //LOGI("\n");
            }

            // デバック EigenValueはほぼ同じ
            //for(i=0; i<nb_channel; i++)
            //{
            //    LOGI("EigenValue[%d][%d] %g\n", k, i, EigenValue[k][i]);
            //}
            //LOGI("k = %d\n", k);
            //LOGI("V[0][0] = %g, %g\n", V[0][0].real, V[0][0].imag);
            //LOGI("V[0][1] = %g, %g\n", V[0][1].real, V[0][1].imag);
            
        } // for(k=0; k<pslength; k++)のループここまで
#if 0
        {
            k=16;
            for(i=0; i<nb_channel; i++){
                for(j=0; j<nb_channel; j++){
                    LOGI("(%g, %g) ", E[k][i][j].real = V[i][j].real, E[k][i][j].imag = V[i][j].imag);
                }
                LOGI("\n");
            }
            LOGI("-----\n");
        }
#endif
        // CalcAveragePower(EigenValue, E, A, UsesFreq, NumSource, Pavg, Pavg_bin, MSSW, enable_eigenvalue_weight)
        {
        	//memo :: int num_mic = nb_channel;
            int m;
            int m_start;
            ////////int num_source;
            
            //float G;
            //float norm_a;
            //float EV_Weight;
            //float fval_real;
            //float fval_imag;
            
            double G;
            double norm_a;
            double EV_Weight;
            double fval_real;
            double fval_imag;
            
            int nb_channel_1;

            m_start    = (nb_channel - localize_music_info.num_source - 1);
            nb_channel_1 = nb_channel - 1;
            ////////num_source = localize_music_info.num_source;
            
            for(id=0; id<direction_num; id++){
                for(ir=0; ir<range_num; ir++){
                    Pavg[id][ir] = 0.0f;
                }
            }
            
#if 0 // COMMENT_OUT
            for(k=0; k<pslength; k++){
                for(id=0; id<amatrix_info.direction_num; id++){
                    Pavg_bin[k][id] = 0.0;
                }
            }
#endif
            
            for(k=0; k<pslength; k++){
                if(!UsesFreq[k]){
                    continue;
                }
                
                EV_Weight = MSSW[k]*(localize_music_info.enable_eigenvalue_weight ? sqrt(EigenValue[k][nb_channel_1]) : 1.0f);
                for(id=0; id<direction_num; id++){
                    for(ir=0; ir<range_num; ir++){
                        //デバック
                        /*
                        E[k][0][4].real = 0.1;  E[k][0][4].imag = 0.05+0.001;
                        E[k][1][4].real = 0.1;  E[k][1][4].imag = 0.05+0.002;
                        E[k][2][4].real = 0.1;  E[k][2][4].imag = 0.05+0.003;
                        E[k][3][4].real = 0.1;  E[k][3][4].imag = 0.05+0.004;
                        E[k][4][4].real = 0.1;  E[k][4][4].imag = 0.05+0.005;
                        E[k][5][4].real = 0.1;  E[k][5][4].imag = 0.05+0.006;
                        E[k][6][4].real = 0.1;  E[k][6][4].imag = 0.05+0.007;
                        E[k][7][4].real = 0.1;  E[k][7][4].imag = 0.05+0.008;
                        
                        AMatrix[id][ir][k][0].real = 0.1+0.09;  AMatrix[id][ir][k][0].imag = 0.05+0.001;
                        AMatrix[id][ir][k][1].real = 0.1+0.08;  AMatrix[id][ir][k][1].imag = 0.05+0.002;
                        AMatrix[id][ir][k][2].real = 0.1+0.07;  AMatrix[id][ir][k][2].imag = 0.05+0.003;
                        AMatrix[id][ir][k][3].real = 0.1+0.06;  AMatrix[id][ir][k][3].imag = 0.05+0.004;
                        AMatrix[id][ir][k][4].real = 0.1+0.05;  AMatrix[id][ir][k][4].imag = 0.05+0.005;
                        AMatrix[id][ir][k][5].real = 0.1+0.04;  AMatrix[id][ir][k][5].imag = 0.05+0.006;
                        AMatrix[id][ir][k][6].real = 0.1+0.03;  AMatrix[id][ir][k][6].imag = 0.05+0.007;
                        AMatrix[id][ir][k][7].real = 0.1+0.02;  AMatrix[id][ir][k][7].imag = 0.05+0.008;
                        */
                        
                        G = 0.0f;
                        for(m=m_start; m>=0; m--){
                            // G += norm(inner_prod(herm(row(A[id][ir],k), column(E[k], m))
                            fval_real = 0.0f;
                            fval_imag = 0.0f;
                            for(i=0; i<nb_channel; i++){
                                //デバック
                                //LOGI("(%g,%g) ", AMatrix[id][ir][k][i].real, AMatrix[id][ir][k][i].imag);
                                
                                // inner_prod部
                                fval_real += AMatrix[id][ir][k][i].real * E[k][i][m].real;
                                fval_real += AMatrix[id][ir][k][i].imag * E[k][i][m].imag;
                                fval_imag += AMatrix[id][ir][k][i].real * E[k][i][m].imag;
                                fval_imag -= AMatrix[id][ir][k][i].imag * E[k][i][m].real;
                            }
                            // norm部
                            G += fval_real * fval_real;
                            G += fval_imag * fval_imag;
                            //LOGI("\nG = %g\n", G);
                        }
                        //LOGI("G = %g\n", G);
                        
                        // norm_a = inner_prod(herm(row(A[id][ir],k), row(A[id][ir],k)))
                        norm_a = 0.0f;
                        for(i=0; i<nb_channel; i++){
                            norm_a += AMatrix[id][ir][k][i].real * AMatrix[id][ir][k][i].real;
                            norm_a += AMatrix[id][ir][k][i].imag * AMatrix[id][ir][k][i].imag;
                        }
                        //デバック（2012.12.12この値は一致）
                        //LOGI("norm_a = %g\n", norm_a);
                        
        	            //float EV_Weight = (enable_eigenvalue_weight ? sqrt(w[k][num_mic - 1]) : 1.0f );
                        //// k以外のループ外に出す TAKIGAHIRA
                        ////EV_Weight = (localize_music_info.enable_eigenvalue_weight ? sqrt(EigenValue[k][nb_channel_1]) : 1.0f);
                        //LOGI("EV_Weight = %g\n", EV_Weight);
                        
                        //Pavg[id][ir] += (norm_a / G) * Weight[k] * EV_Weight;
                        //// MSSW[k]*EV_Weightはkにしか影響を受けないのでEV_Weight*=MSSW[k]としてk以外のループ外に出す TAKIGAHIRA
                        ////Pavg[id][ir] += (norm_a / G) * MSSW[k] * EV_Weight;
                        Pavg[id][ir] += (norm_a / G) * EV_Weight;
                    }
#if 0 // COMMENT_OUT
                    Pavg_bin[k][id] = 10 * log10((norm_a / G * MSSW[k]) + 1.0f);
#endif
                }
            }
        }
        // CalcAveragePowerここまで
#if 0
        // デバック
        for(k=0; k<pslength; k++){
            for(i=0; i<nb_channel; i++){
                for(j=0; j<nb_channel; j++){
                    Rxx[k][i][j].real = 0.0;
                    Rxx[k][i][j].imag = 0.0;
                    RxxN[k][i][j].real = 0.0;
                    RxxN[k][i][j].imag = 0.0;
                }
            }
        }
        return 0;
#endif
        for(id=0; id<direction_num; id++){
            Pavg_sum[id] = 0.0f;
            for(ir=0; ir<range_num; ir++){
                Pavg_sum[id] += Pavg[id][ir];
            }
            Pavg_sum[id] = 10 * log10(Pavg_sum[id] + 1.0f);
            
            //デバック
            //LOGI("Pavg_sum[%d] = %g\n", id, Pavg_sum[id]);
        }
#ifdef EXPORT_DIRECTION_POWER
    	char fname[128];
    	FILE *fp = NULL;
    	sprintf(fname, "%s.flt", OUT_FILE_NAME_DIRECTION_POWER);
		if ((fp = fopen(fname, "ab")) == NULL) {
			fprintf(stderr, "file open error!!\n");
		}
		fwrite(Pavg_sum, sizeof(float) * amatrix_info.direction_num, 1, fp);
		fclose(fp);
#endif
        //LOGI("--------------------------------\n");
        
        // SearchLocationここから
        // FoundSourceCount = SearchLocation(Pavg_sum, Pavg, NumSource,
        //                                   FoundDirection, FoundRange, SearchDirection, mindeg, maxdeg);
        {
            for(id=0; id<direction_num; id++){
                fSearchDirection[id] = (float)SearchDirection[id]; // 毎回は不要？
            }
            // SearchLocation(Pavg_sum, Pavg, NumSource,
            //                FoundDirection, FoundRange, fSearchDirection, mindeg, maxdeg);
            
            // まずはDirectionでサーチ
            if(0){
                n_result = SearchPeak(Pavg_sum, direction_num,
                                      candidate, fSearchDirection, 
                                      localize_music_info.min_deg, localize_music_info.max_deg);
            }
            else if(0){
                n_result = SearchDirectionPeak(Pavg_sum, direction_num,
                                               candidate, fSearchDirection, 
                                               localize_music_info.min_deg, localize_music_info.max_deg);
            }
            else{
                n_result = SearchDifferentialPeak(Pavg_sum, direction_num,
                                               candidate, fSearchDirection, 
                                               localize_music_info.min_deg, localize_music_info.max_deg);
            }
            
            // ソート
            Partial_Sort(candidate, Pavg_sum, n_result, FoundDirection);
            
            // ソート後に音源数をチェックする
            // TAKIGAHIRA : Peak検出数を任意に設定できるように変更
			int num_source = localize_music_info.export_num_source;
            if(num_source <= 0){
    			num_source = localize_music_info.num_source;
            }
            if(n_result > num_source){
                n_result = num_source;
            }
            FoundSourceCount = n_result;
            
            // 次にRangeでサーチ
            if(range_num == 1){
                // Rangeが1つなら常に0（サーチ不要：デフォルトではRange=1）
                for(i=0; i<n_result; i++){
                    FoundRange[i] = 0;
                }
            }
            else{
                for(i=0; i<n_result; i++){
                    id = FoundDirection[i];
                }
            }
        }
        // SearchLocationここまで
    }
    // Localizeここまで
    
    // Localizer->GetFoundSourceCount
    
    // 定位結果の出力
    n_result = FoundSourceCount;
    //sources->count = frame_count;
    sources->count = frame_count - period;
    sources->source_num = n_result;
    rad_coef = M_PI / 180.0f;
    for(i=0; i<n_result; i++){
        int found_direction;
        
        found_direction = FoundDirection[i];
        sources->data[i].id = 0;
        sources->data[i].power = Pavg_sum[found_direction];
        
        //theta   = SearchDirection[found_direction] * M_PI / 180.0f;
        //phi     = localize_music_info.elevation * M_PI / 180.0f;
        theta   = SearchDirection[found_direction] * rad_coef;
        phi     = localize_music_info.elevation * rad_coef;
        cos_phi = cos(phi);
        
        sources->data[i].x[0] = cos(theta) * cos_phi;
        sources->data[i].x[1] = sin(theta) * cos_phi;
        sources->data[i].x[2] = sin(phi);
        
        //デバック
        //LOGI("%d\n", found_direction);
        //LOGI("power = %f, r = %d, x[0] = %f, x[1] = %f, x[2] = %f\n",
        //Pavg_sum[found_direction], SearchDirection[found_direction],
        //sources->data[i].x[0],sources->data[i].x[1],sources->data[i].x[2]);
        
    }
    //LOGI("###############################\n");
    
    // ClearCorrelation Rxx
    // ClearCorrelation RxxN
    for(k=0; k<pslength; k++){
        for(i=0; i<nb_channel; i++){
            for(j=0; j<nb_channel; j++){
                Rxx[k][i][j].real = 0.0f;
                Rxx[k][i][j].imag = 0.0f;
                RxxN[k][i][j].real = 0.0f;
                RxxN[k][i][j].imag = 0.0f;
            }
        }
    }
    return 1;
}

#ifdef STANDALON_TEST
/************************************************************
**  History :
**    2012.10.22 K.Takagi 新規作成
*/
// Public : 単体テストのメイン
int main(int argc, char **argv)
{
    int stat;
    int channels;
    int index;

    Param_LocalizeMUSIC_t music_param;

    if(argc != 2){
        LOGI("Usage : %s <M2PG file>\n", argv[0]);
        return -1;
    }
    
    // パラメータ設定
    channels = 8;
    
    music_param.music_algorithm = SEVD;
    music_param.length = 512;
    music_param.sampling_rate = 16000;
    music_param.window = 50;
    music_param.period = 50;
    music_param.elevation = 16.7; // デフォルト
    music_param.num_source = 2;
    music_param.spectrum_weight_type = Uniform;
    music_param.low_bound_frequency   = 500;
    music_param.upper_bound_frequency = 2800;
    music_param.min_deg = -180;
    music_param.max_deg = 180;
    music_param.a_char_scaling = 1.0;
    music_param.enable_eigenvalue_weight = 1;
    music_param.spline_num = 0;
    music_param.square_num = 0;
    
    music_param.tf_channel_num = channels;
    for(index=0; index<channels; index++){
        music_param.tf_channel_selection[index] = index;
    }
    
    strcpy(music_param.a_matrix, argv[1]);
    
    HASetParamLocalizeMUSIC(&music_param);
    
    //stat = ReadTransferFile(argv[1], ch, channels);
    //LOGI("ReadTransferFile : %d\n", stat);
    HAInitLocalizeMUSIC();
    
    LOGI("\nAMATRIX INFO \n");
    LOGI(" fft_length    = %d\n", amatrix_info.fft_length);
    LOGI(" mic_num       = %d\n", amatrix_info.mic_num);
    LOGI(" direction_num = %d\n", amatrix_info.direction_num);
    LOGI(" range_num     = %d\n", amatrix_info.range_num);
    
    if(AMatrix != NULL)
    {
        free(AMatrix[0][0][0]);
        free(AMatrix[0][0]);
        free(AMatrix[0]);
        free(AMatrix);
    }
    return 0;
}
#endif // STANDALON_TEST
#endif // BUILD_MODULE_LOCALIZE_MUSIC

/* EOF */


