
/************************************************************
**  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 "header.h"
#include "HA_log.h"
#include "android_hark.h"

#include "HABuffer.h"
#include "LocalizeMUSIC.h"
#include "MatrixMemoryAllocate.h"

#ifdef ENABLE_HARKIO3
#include <complex.h>
#include <libharkio3_macros.h>
#include <libharkio3.h>
#endif

//// test code start
#include <time.h>
#include <sys/time.h>
void print_time(const char* prefix){
  struct timeval tv;
  struct tm *time_st;
  gettimeofday(&tv, NULL); time_st = localtime(&tv.tv_sec);
  printf("Time(%s): %02d:%02d:%02d.%06d\n", prefix, time_st->tm_hour, time_st->tm_min, time_st->tm_sec, tv.tv_usec);
}
//// test code end

///////////////////////////////////////////////////////////////
//  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){ }
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
static int SearchPeak(float *power, int power_num, int *peak_cand, int mindeg, int maxdeg){ return 0; }
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
static int SearchPeak(float *power, int power_num, int *peak_cand, float *SDirection, int mindeg, int maxdeg){ return 0; }
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
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;
#ifdef ENABLE_HARKIO3
    int tf_pos_num;
#else // ENABLE_HARKIO3
    int direction_num;
    int range_num;
#endif // ENABLE_HARKIO3
} AMATRIX_INFO_t;

typedef struct _POS_3D_t_
{
    float x;
    float y;
    float z;
#ifdef ENABLE_HARKIO3
    int id;
#endif // ENABLE_HARKIO3
} POS_3D_t;

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

static Param_LocalizeMUSIC_t localize_music_info;
static AMATRIX_INFO_t amatrix_info;
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
static COMPLEX_FLOAT ***AMatrix;    // [iid][f][m]
static COMPLEX_FLOAT ***AMatrixTmp; // [iid][f][m]
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
static COMPLEX_FLOAT ****AMatrix;    // [id][ir][f][m]
static COMPLEX_FLOAT ****AMatrixTmp; // [id][ir][f][m]
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
static FFT_DATA_ARY_t *fft_data_ary_ptr;

#define SEARCH_DIRECTION_MAX_NUM  360
#define SEARCH_RANGE_MAX_NUM      4
// libharkio3 (5deg:72 x 5deg:72/2 = 2529 points)
// It limit up to 32 neighbors to each sound source
#define SEARCH_INDEX_MAX_NUM      2529
#define SEARCH_NEIGHBORS_MAX_NUM  32

// m2pg_mic_pos     : The coordinates of the microphone acquired from the file.
// m2pg_sound_pos   : Direction information of the sound source acquired from the file.
// - If set ENABLE_HARKIO3
// AId              : 
// SearchId         : Transfer Function Index of the sound source to be searched.
// NbrsIds          : 
// - If not set ENABLE_HARKIO3
// SearchDirection  : Direction information of the sound source to be searched. (e.g. 5deg,72elems)
// fSearchDirection : Direction information of the sound source to be searched. (For "float".)
// SearchRange      : Distance information of the sound source to be searched. (Usually one.)
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
// ================================================================================
static POS_3D_t m2pg_mic_pos[16];
static POS_3D_t m2pg_sound_pos[SEARCH_INDEX_MAX_NUM];
static int AId[SEARCH_INDEX_MAX_NUM]; // Index to ID
static int rAId[SEARCH_INDEX_MAX_NUM]; // ID to Index
static float SearchId[SEARCH_INDEX_MAX_NUM][6]; // [0]~[2] cartesian, [3]~[5] polar
static int NbrsIds[SEARCH_INDEX_MAX_NUM]; // Index to ID
static int rNbrsIds[SEARCH_INDEX_MAX_NUM]; // ID to Index
static int NumNbrs[SEARCH_INDEX_MAX_NUM];
//static int NbrsNbrs[SEARCH_INDEX_MAX_NUM][SEARCH_INDEX_MAX_NUM];
static int NbrsNbrs[SEARCH_INDEX_MAX_NUM][SEARCH_NEIGHBORS_MAX_NUM];
static int NbrsNbrsById[SEARCH_INDEX_MAX_NUM][SEARCH_INDEX_MAX_NUM]; //// test code

static float Pavg[SEARCH_INDEX_MAX_NUM];
static float Pavg_sum[SEARCH_INDEX_MAX_NUM];
#ifndef COMMENT_OUT
static float **Pavg_bin;
#endif // COMMENT_OUT
// ================================================================================
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
// ================================================================================
static POS_3D_t m2pg_mic_pos[16];
static POS_3D_t m2pg_sound_pos[SEARCH_DIRECTION_MAX_NUM][SEARCH_RANGE_MAX_NUM];
static int   SearchDirection[SEARCH_DIRECTION_MAX_NUM];
static float fSearchDirection[SEARCH_DIRECTION_MAX_NUM];
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 // COMMENT_OUT
// ================================================================================
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH

static int FoundSourceCount; // Detected number of sound sources


// 最大サイズは構造体の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;

#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
static int FoundDirection[SEARCH_INDEX_MAX_NUM];
static int FoundRange[SEARCH_INDEX_MAX_NUM];
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
static int FoundDirection[(SEARCH_DIRECTION_MAX_NUM/2)];
static int FoundRange[16];
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH

//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];

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

///////////////////////////////////////////////////////////////
//  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(file_name, NULL);
    int num_tf_pos = tfp->poses->size;
    int num_mic    = tfp->mics->size;
    int num_freq   = tfp->cfg->nfft / 2 + 1;
    amatrix_info.tf_pos_num = num_tf_pos;
    amatrix_info.mic_num    = num_mic;
    amatrix_info.fft_length = num_freq;

/*
    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]);
    }
*/

    /* In order to speed up the process, mixing of Cartesian and Polar is not allowed. */
    /* Get sound source coordinates */
    if(tfp->poses->pos[0]->sys == Cartesian){
        //LOGD("Source positions is Cartesian");
        for(s=0; s<num_tf_pos; s++){
            m2pg_sound_pos[s].x  = tfp->poses->pos[s]->coord[0];
            m2pg_sound_pos[s].y  = tfp->poses->pos[s]->coord[1];
            m2pg_sound_pos[s].z  = tfp->poses->pos[s]->coord[2];
            m2pg_sound_pos[s].id = tfp->poses->pos[s]->id;
            //LOGI("POS : sourceLocations[%d] = (id=%d)(%.6f, %.6f, %.6f) ", s, m2pg_sound_pos[s].id, m2pg_sound_pos[s].x, m2pg_sound_pos[s].y, m2pg_sound_pos[s].z);
        }
    }
    else{
        //LOGD("Source positions is Polar");
        for(s=0; s<num_tf_pos; s++){
            harkio_Position_Convert(tfp->poses->pos[s], coord_tmp, Cartesian);
            m2pg_sound_pos[s].x  = coord_tmp[0];
            m2pg_sound_pos[s].y  = coord_tmp[1];
            m2pg_sound_pos[s].z  = coord_tmp[2];
            m2pg_sound_pos[s].id = tfp->poses->pos[s]->id;
            //LOGI("POS : sourceLocations[%d] = (id=%d)(%.6f, %.6f, %.6f) ", s, m2pg_sound_pos[s].id, m2pg_sound_pos[s].x, m2pg_sound_pos[s].y, m2pg_sound_pos[s].z);
        }
    }
    /* Get search id of sound sources */
    for(s=0; s<num_tf_pos; s++){
        AId[s] = tfp->poses->pos[s]->id;
    }
    //printf("AId: ("); for(s=0; s<num_tf_pos; s++){ printf("%d, ", AId[s]); } printf(")\n");
    for(s=0; s<num_tf_pos; s++){
        if(AId[s]>=0 && AId[s]<SEARCH_INDEX_MAX_NUM){
            rAId[AId[s]] = s;
        }
    }
    //printf("rAId: ("); for(s=0; s<num_tf_pos; s++){ printf("%d, ", rAId[s]); } printf(")\n");
    if(tfp->poses->pos[0]->sys == Cartesian){
        for(s=0; s<num_tf_pos; s++){
            harkio_Position_Convert(tfp->poses->pos[s], coord_tmp, Polar);
            SearchId[s][0] = tfp->poses->pos[s]->coord[0];
            SearchId[s][1] = tfp->poses->pos[s]->coord[1];
            SearchId[s][2] = tfp->poses->pos[s]->coord[2];
            SearchId[s][3] = coord_tmp[0];
            SearchId[s][4] = coord_tmp[1];
            SearchId[s][5] = coord_tmp[2];
        }
    }
    else{
        for(s=0; s<num_tf_pos; s++){
            harkio_Position_Convert(tfp->poses->pos[s], coord_tmp, Cartesian);
            SearchId[s][0] = coord_tmp[0];
            SearchId[s][1] = coord_tmp[1];
            SearchId[s][2] = coord_tmp[2];
            SearchId[s][3] = tfp->poses->pos[s]->coord[0];
            SearchId[s][4] = tfp->poses->pos[s]->coord[1];
            SearchId[s][5] = tfp->poses->pos[s]->coord[2];
        }
    }
    /* Get microphone coordinates */
    if(tfp->mics->pos[0]->sys == Cartesian){
        //LOGD("Mic positions is Cartesian");
        for(m=0; m<num_mic; m++){
            m2pg_mic_pos[m].x = tfp->mics->pos[m]->coord[0];
            m2pg_mic_pos[m].y = tfp->mics->pos[m]->coord[1];
            m2pg_mic_pos[m].z = tfp->mics->pos[m]->coord[2];
            //LOGI("MIC : micLocations[%d] = (%.6f, %.6f, %.6f) ", m, m2pg_mic_pos[m].x, m2pg_mic_pos[m].y, m2pg_mic_pos[m].z);
        }
    }
    else{
        //LOGD("Mic positions is Polar");
        for(m=0; m<num_mic; m++){
            harkio_Position_Convert(tfp->mics->pos[m], coord_tmp, Cartesian);
            m2pg_mic_pos[m].x = coord_tmp[0];
            m2pg_mic_pos[m].y = coord_tmp[1];
            m2pg_mic_pos[m].z = coord_tmp[2];
            //LOGI("MIC : micLocations[%d] = (%.6f, %.6f, %.6f) ", m, m2pg_mic_pos[m].x, m2pg_mic_pos[m].y, m2pg_mic_pos[m].z);
        }
    }
    /* Get neighbors indices */
    for(s=0; s<tfp->nbrs->size; s++){
        NbrsIds[s] = tfp->nbrs->ids[s];
        NumNbrs[s] = tfp->nbrs->numNeighbors[s];
        //LOGI("Neighborhood : numNeighbors[%d] = (id=%d)(%d) ", s, NbrsIds[s], NumNbrs[s]);
    }
    for(; s<SEARCH_INDEX_MAX_NUM; s++){
        NbrsIds[s] = -1;
        NumNbrs[s] = -1;
    }
    //printf("NbrsIds: ("); for(s=0; s<num_tf_pos; s++){ printf("%d, ", NbrsIds[s]); } printf(")\n");
    for(s=0; s<num_tf_pos; s++){
        if(NbrsIds[s]>=0 && NbrsIds[s]<SEARCH_INDEX_MAX_NUM){
            rNbrsIds[NbrsIds[s]] = s;
        }
    }
    //printf("rNbrsIds: ("); for(s=0; s<num_tf_pos; s++){ printf("%d, ", rNbrsIds[s]); } printf(")\n");
    for(s=0; s<tfp->nbrs->size; s++){
        for(m=0; m<tfp->nbrs->numNeighbors[s]; m++){
            NbrsNbrs[s][m] = tfp->nbrs->neighbors[s][m];
            //LOGI("Neighbors : neighbors[%d][%d] = (%d) ", s, m, NbrsNbrs[s][m]);
        }
        for(; m<SEARCH_NEIGHBORS_MAX_NUM; m++){
            NbrsNbrs[s][m] = -1;
        }
    }
    for(; s<SEARCH_INDEX_MAX_NUM; s++){
        for(m=0; m<SEARCH_NEIGHBORS_MAX_NUM; m++){
            NbrsNbrs[s][m] = -1;
        }
    }

//// test code start

    // This is to prepare the lists of the neighbors by the reversed searching algorithm.
    // The meaning of the reverse searching are founding self ID's neighbors
    // from another neighbors including the self ID.
    for(s=0; s<num_tf_pos; s++){
        //int l_continue = 1;
        int l_s, l_m, ll_m;
        // Since the neighbors of the self ID should be the nearest, it is replicated first.
        for(m=0; m<SEARCH_NEIGHBORS_MAX_NUM && NbrsNbrs[s][m] != -1; m++){
            NbrsNbrsById[s][m] = NbrsNbrs[s][m];
        }
        // Next, search and added for lost neighbors as a result of being limited.
        //for(; m<SEARCH_INDEX_MAX_NUM && l_continue != 0; m++){
        //    l_continue=0;
        //}
        for(l_s=0; l_s<num_tf_pos; l_s++){
            if(s==l_s){ continue; } // skip self ID
            for(l_m=0; l_m<SEARCH_NEIGHBORS_MAX_NUM && NbrsNbrs[l_s][l_m] != -1; l_m++){
                if(NbrsIds[s] == NbrsNbrs[l_s][l_m]){
                    ll_m=0;
                    while(ll_m<m){
                        // NbrsIds[l_s] already included to NbrsNbrsById.
                        if(NbrsIds[l_s] == NbrsNbrsById[s][ll_m]){ break; }
                        ll_m++;
                    }
                    // Added NbrsIds[l_s] because not registered in NbrsNbrsById yet.
                    if(ll_m==m){ NbrsNbrsById[s][m] = NbrsIds[l_s]; m++; }
                    break; // This NbrsIds[l_s] is already used. go next.
                }
            }
        }
        for(; m<SEARCH_INDEX_MAX_NUM; m++){
            NbrsNbrsById[s][m] = -1;
        }
    }
    for(; s<SEARCH_INDEX_MAX_NUM; s++){
        for(m=0; m<SEARCH_INDEX_MAX_NUM; m++){
            NbrsNbrsById[s][m] = -1;
        }
    }

//// test code end

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

    /* インパルス応答のバッファを確保 complex<float> [direction_num][range_num][fft_length/2+1][mic_num] */
    if(AMatrix != NULL){
#ifdef ENABLE_CALC_BY_NH
        // There is no need to do anything.
#else // ENABLE_CALC_BY_NH
        free(AMatrix[0][0][0]);
#endif // ENABLE_CALC_BY_NH
        free(AMatrix[0][0]);
        free(AMatrix[0]);
        free(AMatrix);
    }
    AMatrix = (COMPLEX_FLOAT ***)MemoryAllocate3D(num_tf_pos, num_freq, num_mic, sizeof(COMPLEX_FLOAT));
    if(AMatrix == NULL){
        return -1;
    }
    
    if(AMatrixTmp != NULL){
#ifdef ENABLE_CALC_BY_NH
        // There is no need to do anything.
#else // ENABLE_CALC_BY_NH
        free(AMatrixTmp[0][0][0]);
#endif // not ENABLE_CALC_BY_NH
        free(AMatrixTmp[0][0]);
        free(AMatrixTmp[0]);
        free(AMatrixTmp);
    }
    AMatrixTmp = (COMPLEX_FLOAT ***)MemoryAllocate3D(num_tf_pos, num_freq, num_mic, sizeof(COMPLEX_FLOAT));
    if(AMatrixTmp == NULL){
        return -1;
    }
    
    // Read Impulse Response
    // TF file is stored in the order of [tfindex_num][mic_num][fft_length/2+1] .
    // [tfindex_num][fft_length/2+1][mic_num] in the buffer in this order.
#ifdef ENABLE_CALC_BY_NH
    for(s=0; s<num_tf_pos; s++){
        if(tfp->loctfs[s]->rows == num_mic && tfp->loctfs[s]->cols == num_freq){
            for(m=0; m<num_mic; m++){
                for(f=0; f<num_freq; f++){
                    AMatrixTmp[s][f][m].real = crealf(tfp->loctfs[s]->data.complexdata[num_freq * m + f]);
                    AMatrixTmp[s][f][m].imag = cimagf(tfp->loctfs[s]->data.complexdata[num_freq * m + f]);
                }
            }
        }
    }
#else // ENABLE_CALC_BY_NH
    // A-matrix probably need to store transfer functions in sorted order by direction and range.
    // not implements yet, because this is very troublesome.
#endif // ENABLE_CALC_BY_NH

    //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;
    
    // Extract only the data of CH actually needed
#ifdef ENABLE_CALC_BY_NH
    for(s=0; s<num_tf_pos; s++){
        for(f=0; f<num_freq; f++){
            for(m=0; m<channels; m++){
                AMatrix[s][f][m].real = AMatrixTmp[s][f][ch[m]].real;
                AMatrix[s][f][m].imag = AMatrixTmp[s][f][ch[m]].imag;
            }
        }
    }
#else // ENABLE_CALC_BY_NH
    // A-matrix probably need to store transfer functions in sorted order by direction and range.
    // not implements yet, because this is very troublesome.
#endif // ENABLE_CALC_BY_NH

#ifndef ENABLE_CALC_BY_NH
    free(AMatrixTmp[0][0][0]);
#endif // not ENABLE_CALC_BY_NH
    free(AMatrixTmp[0][0]);
    free(AMatrixTmp[0]);
    free(AMatrixTmp);
    AMatrixTmp = NULL;
#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);
    
    /* 実際に必要な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;
#endif // ENABLE_HARKIO3
    
    //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);
    }
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
    Pavg_bin = (float **)MemoryAllocate2D((fft_length/2+1), tf_pos_num, sizeof(float));
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
    Pavg_bin = (float **)MemoryAllocate2D((fft_length/2+1), direction_num, sizeof(float));
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
#endif // COMMENT_OUT
    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;
        } 
    }
    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){
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
        // There is no need to do anything.
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
        free(AMatrix[0][0][0]);
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
        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
    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
**  -----------
**  ピーク検出
**
**
*/
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
static int SearchPeak(float *power, int power_num, int *peak_cand, int mindeg, int maxdeg)
{
    int source_count, i, j, k, l, pf, deg;
#if 0
    float x, y, z, maxc;
#endif
    // In case of the number of elements for search target is 1.
    if(power_num < 2){ peak_cand[0] = 0; return 1; }
    while(maxdeg - mindeg > 360){ maxdeg -= 360; }
    while(maxdeg - mindeg <= 0){  maxdeg += 360; }
#if 0
    // HARK OSS version like implementation
    source_count = 0;
    for(i=0; i<power_num; i++){
        x = (SearchId[i][0] == -0.0f) ? 0.0f : SearchId[i][0];
        y = (SearchId[i][1] == -0.0f) ? 0.0f : SearchId[i][1];
        deg = (int)(180.0f / M_PI * atan2(y, x));
        while(deg - mindeg > 360){ deg -= 360; }
        while(deg - mindeg <= 0){  deg += 360; }
        if((deg >= mindeg) && (deg <= maxdeg)){
            maxc=power[i]; pf=1;
            for(j=0; j<SEARCH_INDEX_MAX_NUM && NbrsIds[j] != -1; j++){
                if(NbrsIds[j] == AId[i]){ break; }
            }
            for(k=0; k<NumNbrs[j]; k++){ // k < SEARCH_NEIGHBORS_MAX_NUM
                for(l=0; l<SEARCH_INDEX_MAX_NUM && AId[l] != -1; l++){
                    if(NbrsNbrs[j][k] == AId[l]){ break; }
                }
                if(maxc < power[l]){ pf=0; }
            }
            if(pf){
                peak_cand[source_count] = i;
                source_count++;
                //LOGI("Peak ID = %d, %g\n", i, power[i]);
            }
        }
    }
#elif 0
    // Fast caluclated and Early loop terminated version
    source_count = 0;
////////int test_count=0;
    for(i=0; i<power_num; i++){
        // Coordinate transformation was done at the transfer function loading.
        deg = ((int)(SearchId[i][3]));
////////LOGD("SearchId[%d] (coord=%f,%f,%f)(deg=%f,%f,%f)", i, SearchId[i][0], SearchId[i][1], SearchId[i][2], SearchId[i][3], SearchId[i][4], SearchId[i][5]);
        while(deg - mindeg > 360){ deg -= 360; }
        while(deg - mindeg <= 0){  deg += 360; }
        if((deg >= mindeg) && (deg <= maxdeg)){
            pf=1; j=0; k=0;
            do{
                if(NbrsIds[j] == AId[i]){
                    do{
                        l=0;
                        do{
                            if(NbrsNbrs[j][k] == AId[l]){ /*LOGD("NbrsNbrs[%d][%d]==AId[%d]", j, k, l);*/ break; }
                            if(AId[l] == -1){ /*LOGD("AId[%d]==-1", l);*/ break; }
                            l++;////////test_count++;
                        } while(l<SEARCH_INDEX_MAX_NUM);
                        if(power[i] < power[l]){ pf=0; break; }
                        k++;
                    } while(k<NumNbrs[j]); // k < SEARCH_NEIGHBORS_MAX_NUM
                    break;
                }
                if(NbrsIds[j] == -1){ /*LOGD("NbrsIds[%d]==-1", j);*/ break; }
                j++;
            } while(j<SEARCH_INDEX_MAX_NUM);
            if(pf){
                peak_cand[source_count] = i;
                source_count++;
                //LOGI("Peak ID = %d, %g\n", i, power[i]);
            }
        }
    }
////////LOGD("test_count: %d", test_count);
#else
    // Super Fast caluclated version
    source_count = 0;
////////int test_count=0;
    for(i=0; i<power_num; i++){
        // Coordinate transformation was done at the transfer function loading.
        deg = (int)SearchId[i][3];
////////LOGD("SearchId[%d] (coord=%f,%f,%f)(deg=%f,%f,%f)", i, SearchId[i][0], SearchId[i][1], SearchId[i][2], SearchId[i][3], SearchId[i][4], SearchId[i][5]);
        while(deg - mindeg > 360){ deg -= 360; }
        while(deg - mindeg <= 0){  deg += 360; }
        if((deg >= mindeg) && (deg <= maxdeg)){
            pf=1; l=rNbrsIds[AId[i]];
            for(j=0; j<SEARCH_INDEX_MAX_NUM && NbrsNbrsById[l][j]!=-1; j++){
////////test_count++; LOGD("(i=%d,l=%d,j=%d)",i,l,j);
                if(power[i] < power[rAId[NbrsNbrsById[l][j]]]){ pf=0; break; }
            }
            if(pf){
                peak_cand[source_count] = i;
                source_count++;
                //LOGI("Peak ID = %d, %g\n", i, power[i]);
            }
        }
    }
////////LOGD("test_count: %d", test_count);
#endif
    return source_count;
}
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
static int SearchPeak(float *power, int power_num, int *peak_cand, float *SDirection, int mindeg, int maxdeg)
{
    int source_count, i, i_last, deg;
    float p1, p2, p3;
    // In case of the number of elements for search target is 1.
    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;
    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);
            }
        }
    }
    return source_count;
}
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH

/************************************************************
**
**  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;
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
    float tmp[SEARCH_INDEX_MAX_NUM];
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
    float tmp[(SEARCH_DIRECTION_MAX_NUM/2)]; // 検出音源最大数
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
    float val;
    //LOGD("src: %d", source_num);
    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 = v_tmp;
                
                v_tmp           = v[j][i].imag;
                v[j][i].imag    = v[j][imin].imag;
                v[j][imin].imag = v_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
    
#ifdef ENABLE_HARKIO3
    int id;
#else // ENABLE_HARKIO3
    int id, ir;
#endif // ENABLE_HARKIO3
    
    int n_result;
    
    float theta;
    float phi;
    float cos_phi;
    // deg to rad coef. TAKIGAHIRA
    double rad_coef;
    
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
    int candidate[SEARCH_INDEX_MAX_NUM]; // ピーク候補
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
    int candidate[SEARCH_DIRECTION_MAX_NUM]; // ピーク候補
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH

#ifdef OLD_CALC
    look_back  = 0;
    look_ahead = localize_music_info.window;
#endif
    const int period        = localize_music_info.period;
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
    const int tf_pos_num    = amatrix_info.tf_pos_num;
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
    const int direction_num = amatrix_info.direction_num;
    const int range_num     = amatrix_info.range_num;
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
#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;
            
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
            for(id=0; id<tf_pos_num; id++){
                Pavg[id] = 0.0f;
            }
#if 0 // COMMENT_OUT
            for(k=0; k<pslength; k++){
                for(id=0; id<tf_pos_num; id++){
                    Pavg_bin[k][id] = 0.0f;
                }
            }
#endif // COMMENT_OUT
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
            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<direction_num; id++){
                    Pavg_bin[k][id] = 0.0f;
                }
            }
#endif // COMMENT_OUT
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
            
//print_time("Pavg(all) start");
            for(k=0; k<pslength; k++){
//LOGD("USeFreq[%d]: %d", k, (int)UsesFreq[k]);
                if(!UsesFreq[k]){
                    continue;
                }
                
                EV_Weight = MSSW[k]*(localize_music_info.enable_eigenvalue_weight ? sqrt(EigenValue[k][nb_channel_1]) : 1.0f);
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
//print_time("Pavg start");
                for(id=0; id<tf_pos_num; id++){
                    /*
                    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][k][0].real = 0.1+0.09;  AMatrix[id][k][0].imag = 0.05+0.001;
                    AMatrix[id][k][1].real = 0.1+0.08;  AMatrix[id][k][1].imag = 0.05+0.002;
                    AMatrix[id][k][2].real = 0.1+0.07;  AMatrix[id][k][2].imag = 0.05+0.003;
                    AMatrix[id][k][3].real = 0.1+0.06;  AMatrix[id][k][3].imag = 0.05+0.004;
                    AMatrix[id][k][4].real = 0.1+0.05;  AMatrix[id][k][4].imag = 0.05+0.005;
                    AMatrix[id][k][5].real = 0.1+0.04;  AMatrix[id][k][5].imag = 0.05+0.006;
                    AMatrix[id][k][6].real = 0.1+0.03;  AMatrix[id][k][6].imag = 0.05+0.007;
                    AMatrix[id][k][7].real = 0.1+0.02;  AMatrix[id][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],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][k][i].real, AMatrix[id][k][i].imag);

                            // inner_prod部
                            fval_real += AMatrix[id][k][i].real * E[k][i][m].real;
                            fval_real += AMatrix[id][k][i].imag * E[k][i][m].imag;
                            fval_imag += AMatrix[id][k][i].real * E[k][i][m].imag;
                            fval_imag -= AMatrix[id][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],k), row(A[id],k)))
                    norm_a = 0.0f;
                    for(i=0; i<nb_channel; i++){
                        norm_a += AMatrix[id][k][i].real * AMatrix[id][k][i].real;
                        norm_a += AMatrix[id][k][i].imag * AMatrix[id][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] += (norm_a / G) * Weight[k] * EV_Weight;
                    //// MSSW[k]*EV_Weightはkにしか影響を受けないのでEV_Weight*=MSSW[k]としてk以外のループ外に出す TAKIGAHIRA
                    ////Pavg[id] += (norm_a / G) * MSSW[k] * EV_Weight;
                    Pavg[id] += (norm_a / G) * EV_Weight;
#if 0 // COMMENT_OUT
                    Pavg_bin[k][id] = 10 * log10((norm_a / G * MSSW[k]) + 1.0f);
#endif // COMMENT_OUT
                }
//print_time("Pavg end");
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
                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
                }
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
            }
        }
//print_time("Pavg(all) end");
        // 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

#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
//print_time("Pavg_sum start");
        for(id=0; id<tf_pos_num; id++){
            Pavg_sum[id] = 10 * log10(Pavg[id] + 1.0f);
            //LOGI("Pavg_sum[%d] = %g\n", id, Pavg_sum[id]);
        }
//print_time("Pagv_sum end");
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
        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]);
        }
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH

#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");
        }
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
        fwrite(Pavg_sum, sizeof(float) * amatrix_info.tf_pos_num, 1, fp);
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
        fwrite(Pavg_sum, sizeof(float) * amatrix_info.direction_num, 1, fp);
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
        fclose(fp);
#endif
        //LOGI("--------------------------------\n");
        
        // SearchLocationここから
        // FoundSourceCount = SearchLocation(Pavg_sum, Pavg, NumSource, FoundDirection,
        //                                   FoundRange, SearchDirection, mindeg, maxdeg);
        // FoundSourceCount = SearchLocation(Pavg_sum, Pavg, NumSource, Direction, mindeg, maxdeg);
        {
#ifndef ENABLE_HARKIO3
            for(id=0; id<direction_num; id++){
                fSearchDirection[id] = (float)SearchDirection[id]; // 毎回は不要？
            }
#endif // not ENABLE_HARKIO3
            // SearchLocation(Pavg_sum, Pavg, NumSource,
            //                FoundDirection, FoundRange, fSearchDirection, mindeg, maxdeg);
            
            // まずはDirectionでサーチ
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
            for(int xxx=0; xxx<SEARCH_INDEX_MAX_NUM; xxx++){ candidate[xxx]=0; }
            //printf("("); for(int xxx=0; xxx<80; xxx++){ printf("%d, ", candidate[xxx]); } printf("\n");
//print_time("SearchPeak start");
            n_result = SearchPeak(Pavg_sum, tf_pos_num, candidate,
                                  localize_music_info.min_deg, localize_music_info.max_deg);
//print_time("SearchPeak end");
            //printf("("); for(int xxx=0; xxx<80; xxx++){ printf("%d, ", candidate[xxx]); } printf("\n");
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
            n_result = SearchPeak(Pavg_sum, direction_num,
                                  candidate, fSearchDirection, 
                                  localize_music_info.min_deg, localize_music_info.max_deg);
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
            
            // ソート
            for(int xxx=0; xxx<SEARCH_INDEX_MAX_NUM; xxx++){ FoundDirection[xxx]=0; }
//print_time("Partial_Sort start");
            Partial_Sort(candidate, Pavg_sum, n_result, FoundDirection);
//print_time("Partial_Sort end");
            //printf("("); for(int xxx=0; xxx<80; xxx++){ printf("%d, ", FoundDirection[xxx]); } printf("\n");
            
            // ソート後に音源数をチェックする
            // 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;
            
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
            // There is probably no need to do anything.
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
            // 次に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];
                }
            }
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
        }
        // SearchLocationここまで
    }
    // Localizeここまで
    
    // Localizer->GetFoundSourceCount
    
    // 定位結果の出力
    n_result = FoundSourceCount;
    //sources->count = frame_count;
    sources->count = frame_count - period;
    sources->source_num = n_result;
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
    rad_coef = M_PI / 180.0f;
    for(i=0; i<n_result; i++){
        int found_direction = FoundDirection[i];
        sources->data[i].id = 0;
        sources->data[i].power = Pavg_sum[found_direction];
        sources->data[i].x[0] = SearchId[found_direction][0];
        sources->data[i].x[1] = SearchId[found_direction][1];
        sources->data[i].x[2] = SearchId[found_direction][2];
        //LOGI("%d\n", found_direction);
        //LOGI("power = %f, x[0] = %f, x[1] = %f, x[2] = %f\n",
        //sources->data[i].power, sources->data[i].x[0],sources->data[i].x[1],sources->data[i].x[2]);
    }
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
    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]);
    }
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
    //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);
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
    LOGI(" tf_pos_num    = %d\n", amatrix_info.tf_pos_num);
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
    LOGI(" direction_num = %d\n", amatrix_info.direction_num);
    LOGI(" range_num     = %d\n", amatrix_info.range_num);
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
    
    if(AMatrix != NULL){
#if defined(ENABLE_HARKIO3) && defined(ENABLE_CALC_BY_NH)
        // There is no need to do anything.
#else // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
        free(AMatrix[0][0][0]);
#endif // ENABLE_HARKIO3 && ENABLE_CALC_BY_NH
        free(AMatrix[0][0]);
        free(AMatrix[0]);
        free(AMatrix);
    }
    return 0;
}
#endif // STANDALON_TEST
#endif // BUILD_MODULE_LOCALIZE_MUSIC

/* EOF */


