
#include <stdlib.h>
#include <iostream>
#include <string.h>
#include <vector>

// ---------------------------------------------------------------------
#include "hark-lib/node/GHDSS_exec.h"
// ---------------------------------------------------------------------

namespace hark
{
namespace node
{

// コンストラクタ
GHDSS_exec::GHDSS_exec()
{
    this->SepMat = SavedSeparation();    //分離行列保存値

    ////this->loc_src_fixed_noise.resize(0);

    //TFDB = TFDatabase();
    //this->SMDB = nullptr; // not pointer
    this->filename_mic = "";
    this->filename_fixed_noise = "";
    this->filename_sm = "";
    this->filename_tf = "";
    //this->loc_mic.resize(0);
    //this->mic_pos_shift = 1; // default : FIX
    //m_updateWの確保
    //this->update_w = UpdateSeparationMatrix(0, 0, 0);
    // previous_loc_srcの確保
    this->previous_loc_src.resize(0);
    // updated_method_wの確保
    this->updated_methods_w.resize(0);



    // SS
    this->g_SS_Ryy = hark::Matrix<std::complex<double> >();
    //this->g_SS_Zns1 = hark::Matrix<std::complex<double> >();
    //this->g_SS_Znm1 = hark::Matrix<std::complex<double> >();
    this->g_SS_E = hark::Matrix<std::complex<double> >();
    this->g_SS_Jss = hark::Matrix<std::complex<double> >();
    this->g_SS_Jpri = hark::Matrix<std::complex<double> >();
    //this->g_SS_Znsns = hark::Matrix<std::complex<double> >();
    this->g_SS_Znsnm = hark::Matrix<std::complex<double> >();
    this->g_SS_Z11 = hark::Matrix<std::complex<double> >();
    this->g_SS_PY = hark::Matrix<std::complex<float> >();
    this->g_SS_QY = hark::Matrix<std::complex<float> >();
    this->g_SS_Z11 = hark::Matrix<std::complex<double> >(1, 1);

    // LC
    this->g_LC_E = hark::Matrix<std::complex<double> >();
    //this->g_LC_Znsns = hark::Matrix<std::complex<double> >();
    //this->g_LC_Znsnm = hark::Matrix<std::complex<double> >();
    this->g_LC_Jlc = hark::Matrix<std::complex<double> >();
    //this->g_LC_Atmp = hark::Matrix<std::complex<double> >();

    // Update
    this->g_Up_A = hark::Matrix<std::complex<float> >();
    this->g_Up_X = hark::Matrix<std::complex<float> >();
    this->g_Up_W = hark::Matrix<std::complex<float> >();
    this->g_Up_Y = hark::Matrix<std::complex<float> >();
    this->g_Up_Ryy = hark::Matrix<std::complex<double> >();
    this->g_Up_Rxx = hark::Matrix<std::complex<double> >();
    this->g_Up_J_SS = hark::Matrix<std::complex<float> >();
    this->g_Up_J_LC = hark::Matrix<std::complex<float> >();

    this->g_SepMat_Nsrc = 0;
    this->g_SepMat_Nmic = 0;


    this->Rxx = hark::FreqBins<std::complex<double> >();
    this->Ryy = hark::FreqBins<std::complex<double> >();
}

// ---------------------------------------------------------------------
// デストラクタ
GHDSS_exec::~GHDSS_exec()
{
}




//分離行列保存値、周波数ライン、マイク、音源座標配列の確保と作成
void GHDSS_exec::Initialize(/*const std::string &filename_tf,*/
                            /*const std::string &filename_sm,*/
                            /*int select_tf_type*/)
{
    ////if(filename_tf.empty()){ hark::RuntimeError("GHDSS: TF filename is empty."); }

    //SavedSeparationFree(this->SepMat);
    this->SepMat.TFDB.clear();
    this->SepMat.sm.clear();
    //this->m_PrmStepSize.ha_fStepSizePrm=fStepPrm;

    //TFマトリクスファイル
    ////this->filename_tf = filename_tf;
    // Neighborの計算 // acceptable_distance はユークリッド距離[m]
    this->harkio_tf.CalcNeighbors(this->filename_tf, this->acceptable_distance);

    //分離行列初期値(Calc or Read)
    ////if(!filename_sm.empty()){ this->filename_sm = filename_sm; }

    //this->tf_type = select_tf_type;
}
// ---------------------------------------------------------------------
// フレームパラメータの設定
////
/*
int GHDSS_exec::SetPrmFrame(PrmFrame &prm_frame, int sampling_rate,
                            int length, int overlap, int enumWin)
{
    this->sampling_rate = sampling_rate;
    prm_frame.advance = length - overlap;
    this->length = length;
    prm_frame.window_length = length;
    prm_frame.window = (MICARY_WindowEnum) enumWin;
    return 1;
}
*/
////
// ---------------------------------------------------------------------
//マイク座標ファイルの読み込みと配列の作成 (private)
/*
int GHDSS_exec::ReadMicPos()
{
    std::cout << "File open as HARK = " << this->filename_mic << std::endl;
    if(!this->harkio_tmp.LoadPositionData(this->filename_mic, this->loc_mic)){
        std::cout << "Fail to Read mic position file." << std::endl;
        return 0;
    }
    return 1;
}
*/
// --------------------------------------------------------------------
// 伝達関数の作成(自由音場) (private)
/*
int GHDSS_exec::MakeTFMatrix(int upper, PrmFrame &prm_frame)
{
    if(CalcTFFreeH(this->SepMat.TFDB, upper, prm_frame, this->SepMat.TFDB.loc_src,
                   this->SepMat.TFDB.loc_mic, this->sound_speed) == 0){
        std::cout << "Fail to calculate TF-Matrix." << std::endl;
        return 0;
    }
    return 1;
}
*/
// --------------------------------------------------------------------
// 伝達関数の作成 (private)
/*
int GHDSS_exec::MakeTFMatrix2(PrmFrame &prm_frame, int idsrc)
{
   if(CalcTFFreeH2(this->SepMat.TFDB, prm_frame, idsrc, this->sound_speed) == 0){
        std::cout << "Fail to calculate TF-Matrix." << std::endl;
        return 0;
    }
    return 1;
}
*/
// --------------------------------------------------------------------
// 伝達関数ファイルの読み込み(Originalファイル仕様) (private)
void GHDSS_exec::ReadTFMatrix()
{
    this->harkio_tf.OpenFile(this->filename_tf);

    /*
    if(this->TFDB.loc_src.size() != this->harkio_tf.srcs.size()
    || this->TFDB.loc_mic.size() != this->harkio_tf.mics.size()
    || this->TFDB.freq_bin.size() != this->harkio_tf.freqs.size()
    || this->TFDB.tf.size() != this->harkio_tf.srcs.size()
                             * this->harkio_tf.mics.size()
                             * this->harkio_tf.freqs.size()){
        this->TFDB = TFDatabase(this->harkio_tf.freqs.size(),
                                this->harkio_tf.srcs.size(),
                                this->harkio_tf.mics.size());
    }
    */
    std::vector<Location> srcs = this->PositionsToLocations(this->harkio_tf.srcs,
                                                            this->harkio_tf.srcs_polar);
    std::vector<Location> mics = this->PositionsToLocations(this->harkio_tf.mics,
                                                            this->harkio_tf.mics_polar);
    this->TFDB = TFDatabase(this->harkio_tf.sepdata, this->harkio_tf.freqs, srcs, mics);
}
//-----------------------------------------------------------------------------
// TF Matrixの初期化
//TFデータベース、分離行列初期値データベースのファイル読み込み
int GHDSS_exec::InitTFMatrix()
{
    if(this->TFDB.tf.empty()){    //Initialフレーム処理（データベースファイルの読み込み)
        if(this->tf_type == TF_INPUT_TYPE::FILE){
            this->ReadTFMatrix();
        }
    }  // End of initial frame process
    // ONLINEの場合は伝達関数が更新されている場合がある
    if(this->tf_type == TF_INPUT_TYPE::ONLINE){
        // 伝達関数は毎フレーム更新
        //if(this->SetTransferFunction() == 0){ return 0; }

        // 入力ビン数と伝達関数データベースのサイズを確認する
        if(this->length / 2 + 1 != this->TFDB.freq_bin.size()){
            std::cerr << "Input \n"
                      << "The bin size is not matched in InputSpec and TFdata!\n"
                      << "Bin size of InputSpec = " << this->length / 2 + 1 << "\n"
                      << "Bin size of TFdata = " << this->TFDB.freq_bin.size()
                      << std::endl;
            return 0;
        }
    }

    int lower = this->SepMat.sm.freq_index[0];
    int upper = this->SepMat.sm.freq_index.size() + lower - 1;
    // Initialize
    size_t num_freq = upper - lower + 1;
    size_t num_src = this->SepMat.TFDB.loc_src.size();
    // not Init frame
    size_t num_mic = this->TFDB.loc_mic.size();
    // サイズが違えば切りなおす
    if(this->SepMat.TFDB.loc_mic.size() != num_mic){
        this->SepMat.TFDB.loc_mic.resize(num_mic);
    }
    // マイク座標
    //マイクに関しては角度，TFIndexは使わない
    for(size_t i = 0; i < num_mic; i++){
        this->SepMat.TFDB.loc_mic[i].x[0] = this->TFDB.loc_mic[i].x[0];
        this->SepMat.TFDB.loc_mic[i].x[1] = this->TFDB.loc_mic[i].x[1];
        this->SepMat.TFDB.loc_mic[i].x[2] = this->TFDB.loc_mic[i].x[2];
        this->SepMat.TFDB.loc_mic[i].p[0] = this->TFDB.loc_mic[i].p[0];
        this->SepMat.TFDB.loc_mic[i].p[1] = this->TFDB.loc_mic[i].p[1];
        this->SepMat.TFDB.loc_mic[i].p[2] = this->TFDB.loc_mic[i].p[2];
        this->SepMat.TFDB.loc_mic[i].iddir = this->TFDB.loc_mic[i].iddir;
        this->SepMat.TFDB.loc_mic[i].idpos = this->TFDB.loc_mic[i].idpos;
        this->SepMat.TFDB.loc_mic[i].tfpos = this->TFDB.loc_mic[i].tfpos;
        this->SepMat.TFDB.loc_mic[i].valid = this->TFDB.loc_mic[i].valid;
    }
    if(this->SepMat.TFDB.freq_bin.size() != num_freq){
        this->SepMat.TFDB.freq_bin.resize(num_freq);
    }
    for(size_t i = 0; i < num_freq; i++){
        if(i < this->TFDB.freq_bin.size()){
            this->SepMat.TFDB.freq_bin[i] = this->TFDB.freq_bin[i];
        }
        else{
            this->SepMat.TFDB.freq_bin[i] = num_freq;
        }
    }
    if(this->SepMat.TFDB.tf.size() != num_src * num_mic * num_freq){
        //this->SepMat.TFDB.tf = TFDatabase(num_freq, num_src, num_mic);
        this->SepMat.TFDB.tf.resize(num_freq * num_src * num_mic);
    }
    //必要な情報を取り出す
    if(this->GetNearestTFMatrixFromDB(this->SepMat.TFDB, this->TFDB) == 0){ return 0; }
    return 1;
}
// -------------------------------------------------------------------------------------
// TFデータベース、分離行列初期値データベースのファイル読み込み
int GHDSS_exec::LoadInitialTFMatrix(int src_index)
{
    int lower, upper, num_mic, num_src, num_freq;
    lower = this->SepMat.sm.freq_index[0];
    upper = this->SepMat.sm.freq_index.size() + lower - 1;
    // Initialize
    num_freq = upper - lower + 1;
    num_src = this->SepMat.TFDB.loc_src.size();
    // not Init frame
    num_mic = this->TFDB.loc_mic.size();
    //必要な情報を取り出す
    if(this->GetNearestTFMatrixFromDB2(this->SepMat.TFDB, this->TFDB, src_index) == 0){
        return 0;
    }
    return 1;
}
// -------------------------------------------------------------------------------------
int GHDSS_exec::InitSepMatrix()
{
    //分離行列の初期化
    if(this->filename_sm.empty()){    // Read TF
        // TFから計算
        this->MakeWMatGHDSS();
        // TFファイルはconjで定義
    }
    else{    // Read intW data file
        if(this->SMDB.sm.sep_mat.empty()){    //Initial frame process
            this->ReadSepMatrix();
        } // End of initial frame process
        // 一番近い座標を取り出す
        int lower, upper, num_mic, num_src, num_freq;
        lower = this->SepMat.sm.freq_index[0];
        upper = this->SepMat.sm.freq_index.size() + lower - 1;
        num_mic = this->SepMat.TFDB.loc_mic.size();
        num_freq = upper - lower + 1;
        num_src = this->SepMat.TFDB.loc_src.size();
        if(this->SepMat.sm.sep_mat.size() != num_freq * num_src * num_mic){
            this->SepMat.sm.sep_mat.resize(num_freq * num_src * num_mic);
            this->SepMat.sm.num_inputs = num_mic;
            this->SepMat.sm.num_outputs = num_src;
        }
        // assign freq line index
        for(int i = 0; i < this->SMDB.sm.freq_index.size(); i++){
            this->SepMat.sm.freq_index[i] = this->SMDB.sm.freq_index[i];
        }
        //必要な情報を取り出す
        if(this->GetNearestSepMatrixFromDB(this->SepMat, this->SMDB) == 0){
            // 初期値使用できない場合はTFから計算
            // return 0;
            std::cout << "Cannot found sources from InitW at 1st step. GHDSS uses the separation matrix calculated by TFCONJ." << std::endl;
            this->MakeWMatGHDSS();
        }
    }
    return 1;
}
// ---------------------------------------------------------------------
// 音源インデックスで分離行列初期値を読み込み
int GHDSS_exec::LoadInitialSepMatrix(int src_index)
{
    //分離行列の初期化
    if(this->filename_sm.empty()){    // Calculation
        this->MakeWMatGHDSS2(src_index);
        // TFファイルはconjで定義
    }
    else{    // Read data file
        // 一番近い座標を取り出す
        //size_t num_ch, num_pos, num_freq, lower, upper;
        //lower = this->SepMat.sm.freq_index[0];
        //upper = this->SepMat.sm.freq_index.size() + lower - 1;
        //num_ch = this->SepMat.TFDB.loc_mic.size();
        //num_freq = upper - lower + 1;
        //num_pos = this->SepMat.TFDB.loc_src.size();
        //必要な情報を取り出す
        if(this->GetNearestSepMatrixFromDB2(this->SepMat, this->SMDB, src_index) == 0){
            // TF計算による初期化
            // return 0;
            std::cout << "Cannot found source-index" << src_index <<  " from InitW. GHDSS uses the separation matrix calculated by TFCONJ." << std::endl;
            this->MakeWMatGHDSS2(src_index);
        }
    }
    return 1;
}
// ---------------------------------------------------------------------
// 音源座標配列の代入
int GHDSS_exec::AssignSrcPos(std::vector<Location> &loc_src_pos)
{
    if(loc_src_pos.empty()){ return 0; }
    if(this->SepMat.TFDB.loc_src.size() != loc_src_pos.size()){
      this->SepMat.TFDB.loc_src.resize(loc_src_pos.size());
    }
    for(size_t n = 0; n < loc_src_pos.size(); n++){
        this->SepMat.TFDB.loc_src[n].x[0] = loc_src_pos[n].x[0];
        this->SepMat.TFDB.loc_src[n].x[1] = loc_src_pos[n].x[1];
        this->SepMat.TFDB.loc_src[n].x[2] = loc_src_pos[n].x[2];
        this->SepMat.TFDB.loc_src[n].p[0] = loc_src_pos[n].p[0];
        this->SepMat.TFDB.loc_src[n].p[1] = loc_src_pos[n].p[1];
        this->SepMat.TFDB.loc_src[n].p[2] = loc_src_pos[n].p[2];
        this->SepMat.TFDB.loc_src[n].idpos = loc_src_pos[n].idpos;
        this->SepMat.TFDB.loc_src[n].tfpos = loc_src_pos[n].tfpos;
        this->SepMat.TFDB.loc_src[n].valid = loc_src_pos[n].valid;
        this->SepMat.TFDB.loc_src[n].iddir = 0;
    }

    return 1;
}
// FixedNoise_noiseTFの作成用音源座標の作成 (removed)
/*
int GHDSS_exec::MakeFixedNoise_Pos(std::vector<Location> &loc_src)
{
    for(size_t i = 0; i < this->loc_src_fixed_noise.size(); i++){
        Location fixed_noise;
        fixed_noise.x[0] = this->loc_src_fixed_noise[i].x[0];
        fixed_noise.x[1] = this->loc_src_fixed_noise[i].x[1];
        fixed_noise.x[2] = this->loc_src_fixed_noise[i].x[2];
        fixed_noise.p[0] = this->loc_src_fixed_noise[i].p[0];
        fixed_noise.p[1] = this->loc_src_fixed_noise[i].p[1];
        fixed_noise.p[2] = this->loc_src_fixed_noise[i].p[2];
        fixed_noise.idpos = -n-1; // fixed noise position id is negative.
        fixed_noise.tfpos = -n-1;
        fixed_noise.valid = this->loc_src_fixed_noise[i].valid;
        loc_src.push_back(std::move(fixed_noise));
    }
    return 1;
}
*/
// GHDSSでのFixed Noiseは廃止 (removed)
/*
bool GHDSS_exec::CheckFixedNoise_noise(std::vector<Location> &loc_src)
{
    bool bFixedNoise_Src = false;
    float src[3];
    std::vector<std::vector<float> > fixed_noise;
    float norm, p;
    // fixednoiseの準備
    for(size_t n = 0; n < this->loc_src_fixed_noise.size(); n++){
        std::vector v;
        v.resize(3);
        norm = (this->loc_src_fixed_noise[0].x[0] * this->loc_src_fixed_noise[0].x[0] +
                this->loc_src_fixed_noise[0].x[1] * this->loc_src_fixed_noise[0].x[1] +
                this->loc_src_fixed_noise[0].x[2] * this->loc_src_fixed_noise[0].x[2]);
        norm = std::pow(norm, 0.5f);
        v[0] = this->loc_src_fixed_noise[0].x[0] / norm;
        v[1] = this->loc_src_fixed_noise[0].x[1] / norm;
        v[2] = this->loc_src_fixed_noise[0].x[2] / norm;
        fixed_noise.push_back(std::move(v));
    }
    for(size_t i = 0; i < loc_src.size(); i++){
        norm = (loc_src[i].x[0] * loc_src[i].x[0] +
                loc_src[i].x[1] * loc_src[i].x[1] +
                loc_src[i].x[2] * loc_src[i].x[2]);
        norm = std::pow(norm, 0.5f);
        src[0] = loc_src[i].x[0] / norm;
        src[1] = loc_src[i].x[1] / norm;
        src[2] = loc_src[i].x[2] / norm;
        for(size_t n = 0; n < this->loc_src_fixed_noise.size(); n++){
            p = this->inner_product(src, fixed_noise[n]);
            if(std::abs(p) > 0.999){
                loc_src[i].idpos = fixed_noise[n].idpos; // noise is negative id;
                bFixedNoise_Src = true;
            }
        }
    }
    return bFixedNoise_Src;
}
*/
// ------------------------------------------------------------------
// 分離行列データベースファイルの読み込み
void GHDSS_exec::ReadSepMatrix()
{
    this->harkio_w.OpenFile(this->filename_sm);

    /*
    if(this->SMDB.loc_src.size() != this->harkio_w.srcs.size()
    || this->SMDB.loc_mic.size() != this->harkio_w.mics.size()
    || this->SMDB.sm.freq_index.size() != this->harkio_w.freqs.size()
    || this->SMDB.sm.sep_mat.size() != this->harkio_w.srcs.size()
                                     * this->harkio_w.mics.size()
                                     * this->harkio_w.freqs.size()){
        this->SMDB = SMDatabase(this->harkio_w.freqs.size(),
                                this->harkio_w.srcs.size(),
                                this->harkio_w.mics.size());
    }
    */
    std::vector<Location> srcs = this->PositionsToLocations(this->harkio_w.srcs,
                                                            this->harkio_w.srcs_polar);
    std::vector<Location> mics = this->PositionsToLocations(this->harkio_w.mics,
                                                            this->harkio_w.mics_polar);
    SeparationMatrix sm = SeparationMatrix(this->harkio_w.sepdata,
                                           this->harkio_w.freqs,
                                           this->harkio_w.srcs.size(),
                                           this->harkio_w.mics.size());

    this->SMDB = SMDatabase(sm, srcs, mics);
    //if(!this->harkio_w.ReadInfo(this->filename_sm, this->SMDB.loc_src, this->SMDB.loc_mic, this->SMDB.sm.freq_index)){
    //    std::cout << "Can not read SM Database.\n";
    //    return false;
    //}
    //if(!this->harkio_w.ReadData(this->filename_sm, this->SMDB.sm.sep_mat)){
    //    std::cout << "Can not read SM Database.\n";
    //    return false;
    //}
    //return true;
}
// ----------------------------------------------------------------
bool GHDSS_exec::Initialize_Change(size_t num_ch, size_t num_pos, size_t lower, size_t upper)
{
    // 音源数変更時の配列の初期化
    const size_t bins = (upper - lower/* + 1*/);
    const size_t size = bins * num_pos * num_ch;
    if(size != this->SepMat.sm.sep_mat.size()){
        this->SepMat.sm.resize(bins, num_pos, num_ch);
        this->SepMat.TFDB.resize(bins, num_pos, num_ch);
    }
    //周波数ライン配列の作成
    if(upper - lower > 0){
        ////DEBUG////std::cout << "TFDB.freq_bin(" << this->SepMat.TFDB.freq_bin.size() << "), upper-lower(" << (upper - lower) << ")\n";
        for(size_t k = 0; k < bins; k++){
            this->SepMat.TFDB.freq_bin[k] = lower + k;
        }
    }
    return true;
}

// -----------------------------------------------------------------
// DBから一番近い情報のIndexを取得 (old implements)
/*
int GHDSS_exec::GetNearestPos(Location &cur, std::vector<Location> &DB)
{
    float max_p, p, norm;
    int max_i;
    std::vector<float> cur_n, pos_n;
    max_p = -32768.0;
    cur_n.resize(3);
    pos_n.resize(3);
    // 正規化
    norm = cur.x[0] * cur.x[0] + cur.x[1] * cur.x[1] + cur.x[2] * cur.x[2];
    norm = std::sqrt(norm);
    cur_n[0] = loCur.x[0] / norm;
    cur_n[1] = loCur.x[1] / norm;
    cur_n[2] = loCur.x[2] / norm;
    //DB内の各ベクトルと内積をとる
    for(size_t i = 0; i < DB.size(); i++){
        // 正規化
        norm = DB[i].x[0] * DB[i].x[0] + DB[i].x[1] * DB[i].x[1] + DB[i].x[2] * DB[i].x[2];
        norm = std::sqrt(norm);
        pos_n[0] = DB[i].x[0] / norm;
        pos_n[1] = DB[i].x[1] / norm;
        pos_n[2] = DB[i].x[2] / norm;
        p = this->inner_product(pos_n, cur_n);
        if(p > max_p){
            max_p = p;
            max_i = i;
        }
    }
    if(max_p <= -32768.0){ max_i = INVALID_SRCID; }
    return max_i;
}
*/
// --------------------------------------------------------------------------------
// 2点のユークリッド距離を求める
float GHDSS_exec::_EuclidDistance(Location &ref_pos, Location &obj_pos)
{
    float dist;
    dist = (ref_pos.x[0] - obj_pos.x[0]) * (ref_pos.x[0] - obj_pos.x[0])
         + (ref_pos.x[1] - obj_pos.x[1]) * (ref_pos.x[1] - obj_pos.x[1])
         + (ref_pos.x[2] - obj_pos.x[2]) * (ref_pos.x[2] - obj_pos.x[2]);
    dist = std::sqrt(dist);
    return dist;
}
// --------------------------------------------------------------------------------
// 最近傍位置のデータインデックスを取得(Neighbor利用)
int GHDSS_exec::GetNearestPos(Location &cur, std::vector<Location> &DB, int level)
{
    int cur_index;
    if(DB.empty()){ return INVALID_SRCID; }
    if(this->tf_type == TF_INPUT_TYPE::FILE){
        cur_index = this->harkio_tf.GetPosIndex(cur.tfpos);
        if(cur_index < 0 || !this->harkio_tf.pPrinbrs){
            return (this->GetNearestPosByCoord(cur, DB));
        }
        // 検索レベルの指定
        int iNumNeighbors = (level > 0) ? level : this->harkio_tf.pPrinbrs->numNeighbors[cur_index];
        // cur.posidに対応するneighborリストを取得し、リスト上位からDBを検索する
        // 見つかったらインデックスを返す
        for(size_t i = 0; i < iNumNeighbors; i++){    // i==0は本人
            for(size_t k = 0; k < DB.size(); k++){
                if(this->harkio_tf.pPrinbrs->neighbors[cur_index][i] == DB[k].tfpos){
                    return k;
                }
            }
        }
    }
    else if(this->tf_type == TF_INPUT_TYPE::ONLINE){
        cur_index = this->GetPosIndex(cur.tfpos);
        if(cur_index < 0 || this->m_tfNeighbor.ids.size() == 0){
            return (this->GetNearestPosByCoord(cur, DB));
        }
        int iNumNeighbors = (level > 0) ? level : this->m_tfNeighbor.neighbors[cur_index].size();
        for(size_t i = 0; i < iNumNeighbors; i++){    // i==0は本人
            for(size_t k = 0; k < DB.size(); k++){
                if(this->m_tfNeighbor.neighbors[cur_index][i] == DB[k].tfpos){
                    return k;
                }
            }
        }
    }
    return INVALID_SRCID;
}
// --------------------------------------------------------------------------------
// 現在のTFからidを指定してposindexを取得
int GHDSS_exec::GetPosIndex(int id)
{
    try{
        if(this->TFDB.tf.empty()){
            std::cerr << "Error: Not loaded TF Database. @GHDSS_exec::GetPosIndex" << std::endl;
            return -1;
        }
        for(size_t i = 0; i < this->TFDB.loc_src.size(); i++){
            if(this->TFDB.loc_src[i].tfpos == id){
                return static_cast<int>(i);
            }
        }
    }
    catch(std::exception &ex){
        std::cerr << "Error @GHDSS_exec::GetPosIndex" << std::endl;
        return -1;
    }
    return -1;
}
// --------------------------------------------------------------------------------
// 最近傍位置のデータインデックスを取得(座標計算)
int GHDSS_exec::GetNearestPosByCoord(Location &cur, std::vector<Location> &DB)
{
    float distmin;
    int index;
    if(DB.empty()){ return INVALID_SRCID; }
    index = 0;
    distmin = this->_EuclidDistance(cur, DB[index]);
    for(size_t i = 1; i < DB.size(); i++){
        float disttmp = this->_EuclidDistance(cur, DB[i]);
        if(disttmp < distmin){
            distmin = disttmp;
            index = static_cast<int>(i);
        }
    }
    return index;
}
// --------------------------------------------------------------------------------
// TFデータベース中で最近傍位置の位置IDを取得(座標計算による)
int GHDSS_exec::GetPosIdFromTFDBByCoord(Location &cur)
{
    hark::Position pos = cur.get_cartesian_position();
    return this->GetPosIdFromTFDBByCoord(pos);
}
int GHDSS_exec::GetPosIdFromTFDBByCoord(hark::Position &pos)
{
    return this->harkio_tf.GetNearestPosID(pos);
}
// --------------------------------------------------------------------------------
// DBからデータの取得(最も近い座標を読む)
int GHDSS_exec::GetNearestTFMatrixFromDB(TFDatabase &OutTF, TFDatabase &TFDB)
{
    // OutTFには周波数ライン，音源座標配列は入っている
    int Npos, Nmic, NposD, Nfreq, NfreqD, iCount;
    bool bFixedNoise_check;
    Location loctmp;
    int iDBpos;
    bFixedNoise_check = false;
    Npos = OutTF.loc_src.size();
    NposD = TFDB.loc_src.size();
    Nmic = TFDB.loc_mic.size();
    NfreqD = TFDB.freq_bin.size();
    Nfreq = OutTF.freq_bin.size();
    if(OutTF.tf.size() != Npos*Nmic*Nfreq){
        std::cout << "OutTF-size = " << OutTF.tf.size() << "\n"
                  << "Srcsize = " << Npos << "\n"
                  << "freqsize = " << Nfreq << "\n"
                  << "micsize = " << Nmic << "\n"
                  << "Size error." << std::endl;
        return 0;
    }
    iCount = 0;
    for(size_t i = 0; i < Npos; i++){
        loctmp = OutTF.loc_src[i];
        // 一番近いIDをDBから取得
        iDBpos = this->GetNearestPos(loctmp, TFDB.loc_src);
        if(iDBpos != INVALID_SRCID){
            for(size_t k = 0; k < Nfreq; k++){
                for(size_t l = 0; l < NfreqD; l++){
                    if(OutTF.freq_bin[k] == TFDB.freq_bin[l]){
                        for(size_t m = 0; m < Nmic; m++){
                            // TF配列はfreq->src->micの順に並んでいる
                            OutTF.tf[m+(Npos*Nmic)*k+i*Nmic] =
                                    TFDB.tf[m+(NposD*Nmic)*l+iDBpos*Nmic];
                        }
                        break;
                    }
                }
            }
            iCount++;
        }
    }
    if(iCount != Npos){
        std::cout << "Can't find TF data! Please check GHDSS Update Parameters.\n"
                  << "Detect size = " << iCount << "/ Source size = " << Npos << std::endl;
        return 0; //音源分見つからなければエラー
    }
    return 1;
}
// --------------------------------------------------------------------------
// DBからデータの取得(最も近い座標を読む)
int GHDSS_exec::GetNearestTFMatrixFromDB2(TFDatabase &OutTF, TFDatabase &TFDB, int idsrc)
{
    // OutTFには周波数ライン，音源座標配列は入っている
    int Npos, Nmic, NposD, Nfreq, NfreqD;
    bool bFixedNoise_check;
    Location loctmp;
    int iDBpos;
    bFixedNoise_check = false;
    Npos = OutTF.loc_src.size();
    NposD = TFDB.loc_src.size();
    Nmic = TFDB.loc_mic.size();
    NfreqD = TFDB.freq_bin.size();
    Nfreq = OutTF.freq_bin.size();
    if(OutTF.tf.size() != Npos*Nmic*Nfreq){
        std::cout << "OutTF-size = " << OutTF.tf.size() << "\n"
                  << "Srcsize = " << Npos << "\n"
                  << "freqsize = " << Nfreq << "\n"
                  << "micsize = " << Nmic << "\n"
                  << "Size error." << std::endl;
        return 0;
    }
    // 一番近いIDをDBから取得
    iDBpos = this->GetNearestPos(OutTF.loc_src[idsrc], TFDB.loc_src);
    if(iDBpos != INVALID_SRCID){
        for(size_t k = 0; k < Nfreq; k++){
            for(size_t l = 0; l < NfreqD; l++){
                if(OutTF.freq_bin[k] == TFDB.freq_bin[l]){
                    for(size_t m = 0; m < Nmic; m++){
                        // TF配列はfreq->src->micの順に並んでいる
                        OutTF.tf[m+(Npos*Nmic)*k+idsrc*Nmic] =
                            TFDB.tf[m+(NposD*Nmic)*l+iDBpos*Nmic];
                    }
                    break;
                }
            }
        }
    }
    return 1;
}
// --------------------------------------------------------------------------
/* (removed)
void GHDSS_exec::SetiMicPosShift(int ivalue)
{
    this->mic_pos_shift = ivalue;
    return;
}
*/
// --------------------------------------------------------------------------
int GHDSS_exec::GetNearestSepMatrixFromDB(SavedSeparation &OutSM, SMDatabase &SMDB)
{
    int Npos, Nmic, NposD, Nfreq, NfreqD, iCount;
    Location loctmp;
    int iDBpos;
    bool bFixedNoise_check;
    bFixedNoise_check = false;
    Npos = OutSM.TFDB.loc_src.size();
    NposD = SMDB.loc_src.size();
    Nmic = SMDB.loc_mic.size();
    NfreqD = SMDB.sm.freq_index.size();
    Nfreq = OutSM.sm.freq_index.size();
    if(OutSM.sm.sep_mat.size() != Npos * Nmic * Nfreq){
        std::cout << "OutSM-size = " << OutSM.sm.sep_mat.size() << "\n"
                  << "Srcsize = " << Npos << "\n"
                  << "freqsize = " << Nfreq << "\n"
                  << "micsize = " << Nmic << "\n"
                  << "Size error." << std::endl;
        return 0;
    }
    iCount = 0;
    for(size_t i = 0; i < Npos; i++){
        loctmp = OutSM.TFDB.loc_src[i];
        // 一番近いIDをDBから取得
        iDBpos = this->GetNearestPos(loctmp, SMDB.loc_src);
        if(iDBpos != INVALID_SRCID){
            for(size_t k = 0; k < Nfreq; k++){
                for(size_t l = 0; l < NfreqD; l++){
                    if(OutSM.TFDB.freq_bin[k] == SMDB.sm.freq_index[l]){
                        for(size_t m = 0; m < Nmic; m++){
                            // SepMat配列はfreq->mic->srcの順に並んでいる
                            OutSM.sm.sep_mat[m*Npos+(Npos*Nmic)*k+i] =
                                    SMDB.sm.sep_mat[m*NposD+(NposD*Nmic)*l+iDBpos];
                        }
                        break;
                    }
                }
            }
            iCount++;
        }
    }
    if(iCount != Npos){
        std::cout << "Cannot find the same number of TF data.\n"
                  << "Detect size = " << iCount << "/ Source size = " << Npos << std::endl;
        return 0; //音源分見つからなければエラー
    }
    return 1;
}
// ----------------------------------------------------------------------------------
int GHDSS_exec::GetNearestSepMatrixFromDB2(SavedSeparation &OutSM, SMDatabase &SMDB, int idsrc)
{
    int Npos, Nmic, NposD, Nfreq, NfreqD;
    Location loctmp;
    int iDBpos;
    bool bFixedNoise_check;
    bFixedNoise_check = false;
    Npos = OutSM.TFDB.loc_src.size();
    NposD = SMDB.loc_src.size();
    Nmic = SMDB.loc_mic.size();
    NfreqD = SMDB.sm.freq_index.size();
    Nfreq = OutSM.sm.freq_index.size();
    if(OutSM.sm.sep_mat.size() != Npos * Nmic * Nfreq){
        std::cout << "OutSM-size = " << OutSM.sm.sep_mat.size() << "\n"
                  << "Srcsize = " << Npos << "\n"
                  << "freqsize = " << Nfreq << "\n"
                  << "micsize = " << Nmic << "\n"
                  << "Size error." << std::endl;
        return 0;
    }
    // 一番近いIDをDBから取得
    iDBpos = this->GetNearestPos(OutSM.TFDB.loc_src[idsrc], SMDB.loc_src);
    if(iDBpos == INVALID_SRCID){
        std::cout << "Cannot find the separation matrix from DB." << std::endl;
        return 0; // An error occurs if the sound source cannot be found.
    }
    for(size_t k = 0; k < Nfreq; k++){
        for(size_t l = 0; l < NfreqD; l++){
            if(OutSM.TFDB.freq_bin[k] == SMDB.sm.freq_index[l]){
                for(size_t m = 0; m < Nmic; m++){
                    // SepMat配列はfreq->mic->srcの順に並んでいる
                    OutSM.sm.sep_mat[m*Npos+(Npos*Nmic)*k+idsrc] =
                            SMDB.sm.sep_mat[m*NposD+(NposD*Nmic)*l+iDBpos];
                }
            }
        }
    }
    return 1;
}
// ----------------------------------------------------------------------------------
int GHDSS_exec::InitExportW(const std::string &filename)
{
    if(this->SepMat.sm.sep_mat.empty()){ return 0; }
    size_t num_mic = this->SepMat.sm.num_inputs;
    size_t num_freq = this->SepMat.sm.freq_index.size();
    //出力ファイルの設定
    if(!filename.empty()){
        SMDatabase dummy;    //ダミーデータ
        dummy = SMDatabase(num_freq, 1, num_mic);
        //マイク座標
        for(size_t i = 0; i < num_mic; i++){
            dummy.loc_mic[i].x[0] = this->update_w.SMDB.loc_mic[i].x[0];
            dummy.loc_mic[i].x[1] = this->update_w.SMDB.loc_mic[i].x[1];
            dummy.loc_mic[i].x[2] = this->update_w.SMDB.loc_mic[i].x[2];
            dummy.loc_mic[i].p[0] = this->update_w.SMDB.loc_mic[i].p[0];
            dummy.loc_mic[i].p[1] = this->update_w.SMDB.loc_mic[i].p[1];
            dummy.loc_mic[i].p[2] = this->update_w.SMDB.loc_mic[i].p[2];
            dummy.loc_mic[i].idpos = this->update_w.SMDB.loc_mic[i].idpos;
            dummy.loc_mic[i].tfpos = this->update_w.SMDB.loc_mic[i].tfpos;
        }
        //周波数ライン
        for(size_t i = 0; i < num_freq; i++){
            dummy.sm.freq_index[i] = this->update_w.SMDB.sm.freq_index[i];
        }
        // dummy source
        dummy.loc_src[0].x[0] = this->SepMat.TFDB.loc_src[0].x[0];
        dummy.loc_src[0].x[1] = this->SepMat.TFDB.loc_src[0].x[1];
        dummy.loc_src[0].x[2] = this->SepMat.TFDB.loc_src[0].x[2];
        dummy.loc_src[0].p[0] = this->SepMat.TFDB.loc_src[0].p[0];
        dummy.loc_src[0].p[1] = this->SepMat.TFDB.loc_src[0].p[1];
        dummy.loc_src[0].p[2] = this->SepMat.TFDB.loc_src[0].p[2];
        dummy.loc_src[0].idpos = this->SepMat.TFDB.loc_src[0].idpos;
        dummy.loc_src[0].tfpos = this->SepMat.TFDB.loc_src[0].tfpos;
        for(size_t i = 0; i < dummy.sm.sep_mat.size(); i++){
            dummy.sm.sep_mat[i] = std::complex<float>(0.0f, 0.0f);
        }
        {
            this->harkio_w.sampling_rate = this->sampling_rate;
            this->harkio_w.num_fft = this->length;
            //this->harkio_w.lNMic = num_mic;
            //this->harkio_w.lNSrc = 1;
            //this->harkio_w.lNFreq = num_freq;
            //this->harkio_w.iVersion = 1;
            this->harkio_w.file_type = HarkFileIO::FORMAT_TYPE::W;
            // 座標を伝達関数ファイルに対応したものに丸める
            dummy.loc_src[0].x[0] = this->TFDB.loc_src[dummy.loc_src[0].tfpos].x[0];
            dummy.loc_src[0].x[1] = this->TFDB.loc_src[dummy.loc_src[0].tfpos].x[1];
            dummy.loc_src[0].x[2] = this->TFDB.loc_src[dummy.loc_src[0].tfpos].x[2];
            std::vector<hark::Position> pos_mic = this->LocationsToCartesianPositions(dummy.loc_mic);
            std::vector<hark::Position> pos_src = this->LocationsToCartesianPositions(dummy.loc_src);
            this->harkio_w.SetW(filename,
                                dummy.sm.freq_index,
                                pos_mic,
                                pos_src,
                                dummy.sm.sep_mat);
            this->harkio_w.WriteData(filename);
        }
    }
    return 1;
}
// ----------------------------------------------------------------------------------
int GHDSS_exec::InitUpdateW()
{
    if(this->SepMat.sm.sep_mat.empty()){ return 0; }
    size_t num_freq, num_loc, num_mic;
    num_loc = this->TFDB.loc_src.size(); //伝達関数の配置と同じで
    num_mic = this->SepMat.sm.num_inputs;
    num_freq = this->SepMat.sm.freq_index.size();
    //this->update_w = UpdateSeparationMat(num_freq, num_loc, num_mic);
    this->update_w.SMDB.resize(num_freq, num_loc, num_mic);
    this->update_w.Update.resize(num_loc);
    //更新履歴の初期化
    for(size_t i = 0; i < this->update_w.Update.size(); i++){
        this->update_w.Update[i] = SRCID_PREVIOUS_STATE::INVALID;
    }
    //マイク位置の代入
    for(size_t i = 0; i < num_mic; i++){
        this->update_w.SMDB.loc_mic[i].x[0] = this->SepMat.TFDB.loc_mic[i].x[0];
        this->update_w.SMDB.loc_mic[i].x[1] = this->SepMat.TFDB.loc_mic[i].x[1];
        this->update_w.SMDB.loc_mic[i].x[2] = this->SepMat.TFDB.loc_mic[i].x[2];
        this->update_w.SMDB.loc_mic[i].p[0] = this->SepMat.TFDB.loc_mic[i].p[0];
        this->update_w.SMDB.loc_mic[i].p[1] = this->SepMat.TFDB.loc_mic[i].p[1];
        this->update_w.SMDB.loc_mic[i].p[2] = this->SepMat.TFDB.loc_mic[i].p[2];
        this->update_w.SMDB.loc_mic[i].idpos = this->SepMat.TFDB.loc_mic[i].idpos;
        this->update_w.SMDB.loc_mic[i].tfpos = this->SepMat.TFDB.loc_mic[i].tfpos;
    }
    // 周波数ラインの代入
    for(size_t i = 0; i < num_freq; i++){
        this->update_w.SMDB.sm.freq_index[i] = this->SepMat.TFDB.freq_bin[i];
    }
    // 音源位置はTFと同じとする.角度もここで取得
    for(size_t i = 0; i < num_loc; i++){
        this->update_w.SMDB.loc_src[i].x[0] = this->TFDB.loc_src[i].x[0];
        this->update_w.SMDB.loc_src[i].x[1] = this->TFDB.loc_src[i].x[1];
        this->update_w.SMDB.loc_src[i].x[2] = this->TFDB.loc_src[i].x[2];
        //this->update_w.SMDB.loc_src[i].p[0] = this->TFDB.loc_src[i].p[0];
        //this->update_w.SMDB.loc_src[i].p[1] = this->TFDB.loc_src[i].p[1];
        //this->update_w.SMDB.loc_src[i].p[2] = this->TFDB.loc_src[i].p[2];
        this->update_w.SMDB.loc_src[i].idpos = this->TFDB.loc_src[i].idpos;
        this->update_w.SMDB.loc_src[i].tfpos = this->TFDB.loc_src[i].tfpos;
        //this->update_w.SMDB.loc_src[i].iddir = this->TFDB.loc_src[i].iddir;
        //this->update_w.SMDB.loc_src[i].valid = this->TFDB.loc_src[i].valid;
    }
    //音源IDの初期化
    for(size_t i = 0; i < num_loc; i++){
        this->update_w.SMDB.loc_src[i].idpos = INVALID_SRCID;
        this->update_w.SMDB.loc_src[i].tfpos = INVALID_SRCID;
    }
    return 1;
}
// ---------------------------------------------------------------------------------------
// TransferFunction データ読み出し
int GHDSS_exec::SetTransferFunction()
{
    int nsrc, nmic, nfrq;
    std::vector<hark::Position> Sources;
    std::vector<hark::Position> Mics;
    int index;
    std::complex<float> value;
    ONLINE_TF_UPDATE_TYPE IsPartial = this->IsTfMatPartial();
    nsrc = this->TFDB.loc_src.size();
    nmic = this->TFDB.loc_mic.size();
    nfrq = this->TFDB.freq_bin.size();
    if(IsPartial == ONLINE_TF_UPDATE_TYPE::ALL){
        // Get Sources
        Sources = this->m_tfData.GetSources();
        nsrc = Sources.size();
        // Get Neighbors
        this->m_tfNeighbor = this->m_tfData.GetNeighbors();
        if(this->m_tfNeighbor.ids.size() != nsrc) {
            std::cerr << "Warning: The number of loctfs does not match to the size of neighbors.\n"
                      << "TransferFunction is not be updated." << std::endl;
            return 0;
        }
        // Get Microphones
        Mics = this->m_tfData.GetMicrophones();
        nmic = Mics.size();
        // Get Configuration
        const hark::Config config = this->m_tfData.GetConfig();
        if(nfrq > 0){
            if(nfrq != config.nfft / 2 + 1){
                std::cerr << "Warning: The number of bins does not match to the input data.\n"
                          << "TransferFunction is not be updated." << std::endl;
                return 0;
            }
        }
        nfrq = config.nfft / 2 + 1;
    }

    // Get TransferFunction 分離用伝達関数 (行がマイク列がビンの行列が方向ごとに)
    const std::map<int, hark::Matrix<std::complex<float> > > mpSep = this->m_tfData.GetSeparation();
    if(mpSep.size() > 0){
        // check bin size
        if(mpSep.begin()->second.cols() != nfrq) {
            std::cerr << "Error: The number of bins does not match to the size of tf input.\n"
                      << "bins = " << nfrq << ", ncols = " << mpSep.begin()->second.cols() << std::endl;
            return 0;
        }
        // check num of mic
        if(mpSep.begin()->second.rows() != nmic) {
            std::cerr << "Error: The number of mics does not match to the size of tf input.\n"
                      << "mics = " << nmic << ", nrows = " << mpSep.begin()->second.rows() << std::endl;
            return 0;
        }
    }
    if(IsPartial == ONLINE_TF_UPDATE_TYPE::ALL){
        // 定位用伝達関数データベース
        // Allocate
        if(nsrc != this->TFDB.loc_src.size() ||
           nmic != this->TFDB.loc_mic.size() ||
           nfrq != this->TFDB.freq_bin.size()) {
            if(this->TFDB.loc_src.size() * this->TFDB.loc_mic.size() * this->TFDB.freq_bin.size() > 0){
                std::cout << "Warning: ";
                if(nsrc != this->TFDB.loc_src.size()){ std::cout << "The number of sources is changed."; }
                if(nmic != this->TFDB.loc_mic.size()){ std::cout << "The number of microphones is changed."; }
                if(nfrq != this->TFDB.freq_bin.size()){ std::cout << "The number of bins is changed."; }
                std::cout << std::endl;
            }
            this->TFDB = TFDatabase(nfrq, nsrc, nmic);
        }

        // マイクロホン位置
        for(size_t i = 0; i < nmic; i++){
            if(Mics[i].sys != hark::TF_Coordinate::Cartesian){
                float azimuth_rad = harkio_Position_convToRadian(Mics[i].coord[0]);
                float elevation_rad = harkio_Position_convToRadian(Mics[i].coord[1]);
                float coef = Mics[i].coord[2] * std::cos(elevation_rad);
                this->TFDB.loc_mic[i].x[0] = coef * std::cos(azimuth_rad);
                this->TFDB.loc_mic[i].x[1] = coef * std::sin(azimuth_rad);
                this->TFDB.loc_mic[i].x[2] = Mics[i].coord[2] * std::sin(elevation_rad);
            }
            else{
                this->TFDB.loc_mic[i].x[0] = Mics[i].coord[0];
                this->TFDB.loc_mic[i].x[1] = Mics[i].coord[1];
                this->TFDB.loc_mic[i].x[2] = Mics[i].coord[2];
            }
            this->TFDB.loc_mic[i].idpos = Mics[i].id;
            this->TFDB.loc_mic[i].tfpos = Mics[i].id;
        }

        // 音源推定位置
        for(size_t i = 0; i < nsrc; i++){
            if(Sources[i].sys != hark::TF_Coordinate::Cartesian){
                float azimuth_rad = harkio_Position_convToRadian(Sources[i].coord[0]);
                float elevation_rad = harkio_Position_convToRadian(Sources[i].coord[1]);
                float coef = Sources[i].coord[2] * std::cos(elevation_rad);
                this->TFDB.loc_src[i].x[0] = coef * std::cos(azimuth_rad);
                this->TFDB.loc_src[i].x[1] = coef * std::sin(azimuth_rad);
                this->TFDB.loc_src[i].x[2] = Sources[i].coord[2] * std::sin(elevation_rad);
            }
            else {
                this->TFDB.loc_src[i].x[0] = Sources[i].coord[0];
                this->TFDB.loc_src[i].x[1] = Sources[i].coord[1];
                this->TFDB.loc_src[i].x[2] = Sources[i].coord[2];
            }
            this->TFDB.loc_src[i].idpos = Sources[i].id;
            this->TFDB.loc_src[i].tfpos = Sources[i].id;
        }

        // 周波数ライン配列
        for(size_t i = 0; i < nfrq; i++) {
            this->TFDB.freq_bin[i] = i;
        }
    }

    // 分離用伝達関数配列(mics, frqs)を(周波数(音源(マイク)))の順に入力
    std::map<int, hark::Matrix<std::complex<float> > >::const_iterator it = mpSep.begin();
    for(; it != mpSep.end(); ++it) {
        int j = it->first; // src
        if(j < 0 || j >=nsrc ){
            std::cerr << "Warning: The specified TF index cannot be used." << std::endl;
            std::cerr << "The specified TF index:" << j << ", Total number of sources:" << nsrc << std::endl;
            continue;
        }
        const hark::Matrix<std::complex<float> > &mat = it->second;
        for(size_t k = 0; k < nfrq; k++){
            index = nsrc * nmic * k + nmic * j;
            for(size_t i = 0; i < nmic; i++){
                this->TFDB.tf[index] = mat(i, k);
                index++;
            }
        }
    }
    return 1;
}
// ---------------------------------------------------------------------------------------
// TF_CONJとWの引き継ぎ
int GHDSS_exec::ChangeSrcPos_TF_CONJ_W(std::vector<Location> &src_pos_cur)
{
    ////DEBUG////std::cout << "start @ChangeSrcPos_TF_CONJ_W\n";
    int Npos_ini, Npos_cur, Nmic, Nfreq, iLower_f, iUpper_f;
    bool bFound, bRtnPos, bRtnID;
    int iRtn = 2;
    int inherit_TF_CONJ = 0;
    int inherit_W = 0;

    std::vector<Location> src_pos_ini;

    Nmic = this->SepMat.sm.num_inputs;
    Npos_ini = this->SepMat.TFDB.loc_src.size();
    iLower_f = this->SepMat.sm.freq_index[0];
    iUpper_f = this->SepMat.sm.freq_index.size() + iLower_f/* - 1*/;
    Nfreq = this->SepMat.sm.freq_index.size();
    Npos_cur = src_pos_cur.size();
    // バックアップ
    // 音源位置
    src_pos_ini.resize(Npos_ini);
    for(size_t n = 0; n < Npos_ini; n++){
       src_pos_ini[n].x[0] = this->SepMat.TFDB.loc_src[n].x[0];
       src_pos_ini[n].x[1] = this->SepMat.TFDB.loc_src[n].x[1];
       src_pos_ini[n].x[2] = this->SepMat.TFDB.loc_src[n].x[2];
       src_pos_ini[n].p[0] = this->SepMat.TFDB.loc_src[n].p[0];
       src_pos_ini[n].p[1] = this->SepMat.TFDB.loc_src[n].p[1];
       src_pos_ini[n].p[2] = this->SepMat.TFDB.loc_src[n].p[2];
       src_pos_ini[n].idpos = this->SepMat.TFDB.loc_src[n].idpos;
       src_pos_ini[n].tfpos = this->SepMat.TFDB.loc_src[n].tfpos;
       src_pos_ini[n].valid = this->SepMat.TFDB.loc_src[n].valid;
    }
    // TF_CONJ
    std::vector<std::complex<float> > tmpTFDB(Nfreq * Npos_ini * Nmic);
    for(size_t n = 0; n < this->SepMat.TFDB.tf.size(); n++){
        tmpTFDB[n] = this->SepMat.TFDB.tf[n];
    }
    // 配列数の切りなおし
    if(Npos_ini != Npos_cur){
        this->Initialize_Change(Nmic, Npos_cur, iLower_f, iUpper_f); // ここで中身はクリアされない
        iRtn = 1;
    }
    if(this->updated_methods_w.size() != Npos_cur){
        this->updated_methods_w.resize(Npos_cur);
    }
    // 音源情報の更新
    this->AssignSrcPos(src_pos_cur);
    // TF_CONJの更新
    if(this->update_method_tf == UPDATE_METHOD::ID){
        for(size_t i = 0; i < Npos_cur; i++){
            bFound = false;
            for(size_t j = 0; j < Npos_ini; j++){
                if(src_pos_cur[i].idpos == src_pos_ini[j].idpos){
                    // 1次元配列なので、音源数が異なる場合はコピーする必要がある
                    if(i != j || // 同じインデックスであればコピーする必要はない
                        Npos_ini != Npos_cur){
                        // 前回とIDの同じものは引き継ぎ
                        for(size_t k = 0; k < Nfreq; k++){
                            for(size_t m = 0; m < Nmic; m++){
                                this->SepMat.TFDB.tf[m+i*Nmic+k*Nmic*Npos_cur]
                                    = tmpTFDB[m+j*Nmic+k*Nmic*Npos_ini];
                            }
                        }
                    }
                    inherit_TF_CONJ++;
                    bFound = true;
                    break;
                }
            }
            if (!bFound){
                // TF_CONJ初期値を読み出し
                this->LoadInitialTFMatrix(i);
            }
        }
    }
    else {
        this->InitTFMatrix();
    }
    // Wの更新
    // std::cout << "ChangeType W: ";
    for(size_t i = 0; i < Npos_cur; i++){
        bRtnID = false;
        bRtnPos = false;
        this->updated_methods_w[i] = this->update_method_w;
        if(this->update_method_w == UPDATE_METHOD::ID
        || this->update_method_w == UPDATE_METHOD::ID_POS){
            ////DEBUG////std::cout << "update_method_w == ID or ID_POS\n" << std::endl;
            // 1次元配列なので、音源数が異なる場合はコピーする必要がある
            if(i < Npos_ini && Npos_ini == Npos_cur){
                ////DEBUG////std::cout << "i=(" << i << ") < Npos_ini=(" << Npos_ini << ") && Npos_ini == Npos_cur=(" << Npos_cur << ")\n" << std::endl;
                ////DEBUG////std::cout << "i < Npos_ini && Npos_ini == Npos_cur\n" << std::endl;
                ////DEBUG////std::cout << "0) bRtnID=" << (bRtnID ? "true" : "false") << std::endl;
                if(src_pos_cur[i].idpos == src_pos_ini[i].idpos){
                    bRtnID = true;    // コピーをする必要なし
                    ////DEBUG////std::cout << "1) bRtnID=" << (bRtnID ? "true" : "false") << std::endl;
                }
            }
            if(!bRtnID){
                bRtnID = this->_CheckSrcID_W(i);
                ////DEBUG////std::cout << "2) bRtnID=" << (bRtnID ? "true" : "false") << std::endl;
            }
        }
        // 実際の更新はID or POS
        if(this->update_method_w == UPDATE_METHOD::ID_POS){
            if(bRtnID == false){
                this->updated_methods_w[i] = UPDATE_METHOD::POS;
                ////DEBUG////std::cout << "POS selected.\n" << std::endl;
            }
            else{
                this->updated_methods_w[i] = UPDATE_METHOD::ID;
                ////DEBUG////std::cout << "ID selected.\n" << std::endl;
                // std::cout << "-ID";
            }
        }
        if(this->updated_methods_w[i] == UPDATE_METHOD::POS){
            // POSベースの確認
            bRtnPos = this->_CheckSrcPos_W(i);
            //if(bRtnPos){
            //    std::cout << "-POS";
            //}
            if(!bRtnPos){
              if(this->update_method_w == UPDATE_METHOD::ID_POS){
                this->updated_methods_w[i] = UPDATE_METHOD::ID;
              }
            }
        }
        if(bRtnID || bRtnPos){
            inherit_W++;
        }
        else {
            // SepMatの初期値
            this->LoadInitialSepMatrix(i);
            ////DEBUG////std::cout << "this->LoadInitialSepMatrix(" << i << ");\n";
            //std::cout << "-INIT";
        }
        //std::cout << ", ";
    }
    ////DEBUG////std::cout << "end @ChangeSrcPos_TF_CONJ_W\nreturn " << (iRtn == 1 ? 1 : (inherit_TF_CONJ != Npos_ini ? 2 : 0)) << ", iRtn=" << iRtn << ", inherit_TF_CONJ=" << inherit_TF_CONJ << "\n";
    //std::cout << "@GHDSS_exec::ChangeSrcPos_TF_CONJ_W" << std::endl;
    /*
    int ifreqindex = 1;
    for(size_t k = 0; k < Npos_cur; k++){
        std::cout << "FD->G(f" << ifreqindex << "s" << this->SepMat.TFDB.loc_src[k].tfpos << "):";
        for(size_t m = 0; m < Nmic; m++){
            std::cout << this->SepMat.sm.sep_mat[k+m*Npos_cur+ifreqindex*Nmic*Npos_cur];
        }
        std::cout << "@GHDSS_exec::ChangeSrcPos_TF_CONJ_W" << std::endl;
    }
    */
    if(iRtn == 1){ return 1; } // 数が変更
    if(inherit_TF_CONJ != Npos_ini){ return 2; } // 位置が変更、数は不変
    return 0;
}
// ---------------------------------------------------------------------------------------
bool GHDSS_exec::_CheckSrcID_TF_CONJ(std::vector<std::complex<float> > &TFDBInit, std::vector<Location> &src_pos_ini)
{
    size_t num_pos_cur, num_pos_ini, num_mic, num_freq;
    //size_t lower, upper;
    size_t inherit;
    num_pos_cur = this->SepMat.TFDB.loc_src.size();
    num_pos_ini = src_pos_ini.size();
    num_mic = this->SepMat.sm.num_inputs;
    //lower = this->SepMat.sm.freq_index[0];
    //upper = this->SepMat.sm.freq_index.size() + lower - 1;
    num_freq = this->SepMat.sm.freq_index.size();

    inherit = 0;
    //TF_CONJデータの引継ぎ/初期化
    for(size_t i = 0; i < num_pos_cur; i++){
        for(size_t j = 0; j < num_pos_ini; j++){
            // 音源の位置が同じ(引継ぎ)
            if(this->SepMat.TFDB.loc_src[i].idpos == src_pos_ini[j].idpos){
                for(size_t k = 0; k < num_freq; k++){
                    for(size_t m = 0; m < num_mic; m++){
                        this->SepMat.TFDB.tf[m + i * num_mic + k * num_mic * num_pos_cur]
                            = TFDBInit[m + j * num_mic + k * num_mic * num_pos_ini];
                    }
                }
                inherit++;
                break;
            }
        }
    }
    if(inherit == num_pos_cur){ return true; } //音源変更なし
    return false; //音源変更あり
}
int GHDSS_exec::ChangeSrcPos_TF_CONJ(std::vector<Location> &src_pos_cur)
{
    int Npos_ini, Npos_cur, Nmic, Nfreq, iLower_f, iUpper_f;
    std::vector<std::complex<float> > tmpTFDB;
    std::vector<Location> src_pos_ini;
    int iRtn = 2;

    Nmic = this->SepMat.sm.num_inputs;
    Npos_ini = this->SepMat.TFDB.loc_src.size();
    iLower_f = this->SepMat.sm.freq_index[0];
    iUpper_f = this->SepMat.sm.freq_index.size() + iLower_f/* -1*/;
    Nfreq = this->SepMat.sm.freq_index.size();
    Npos_cur = src_pos_cur.size();

    // バックアップ
    src_pos_ini.resize(this->SepMat.TFDB.loc_src.size());
    for(size_t n = 0; n < this->SepMat.TFDB.loc_src.size(); n++){
       src_pos_ini[n].x[0] = this->SepMat.TFDB.loc_src[n].x[0];
       src_pos_ini[n].x[1] = this->SepMat.TFDB.loc_src[n].x[1];
       src_pos_ini[n].x[2] = this->SepMat.TFDB.loc_src[n].x[2];
       src_pos_ini[n].p[0] = this->SepMat.TFDB.loc_src[n].p[0];
       src_pos_ini[n].p[1] = this->SepMat.TFDB.loc_src[n].p[1];
       src_pos_ini[n].p[2] = this->SepMat.TFDB.loc_src[n].p[2];
       src_pos_ini[n].valid = this->SepMat.TFDB.loc_src[n].valid;
       src_pos_ini[n].idpos = this->SepMat.TFDB.loc_src[n].idpos;
       src_pos_ini[n].tfpos = this->SepMat.TFDB.loc_src[n].tfpos;
    }
    tmpTFDB.resize(Nfreq * Npos_ini * Nmic);
    for(size_t n = 0; n < this->SepMat.TFDB.tf.size(); n++){
        tmpTFDB[n] = this->SepMat.TFDB.tf[n];
    }
    //配列数の切りなおし
    if(src_pos_ini.size() != src_pos_cur.size()){
        this->Initialize_Change(Nmic, Npos_cur, iLower_f, iUpper_f);
        iRtn = 1;
    }
    //音源情報・TF_CONJの更新
    this->AssignSrcPos(src_pos_cur);
    //TF_CONJの更新
    this->InitTFMatrix();

    bool bRtn = false;
    if(this->update_method_tf == UPDATE_METHOD::ID){
        bRtn = this->_CheckSrcID_TF_CONJ(tmpTFDB, src_pos_ini); // A method that is not called elsewhere.
    }
    //W更新用に一度元に戻す
    this->AssignSrcPos(src_pos_ini);
    if(iRtn == 1){ return 1; } //数が変更
    if(bRtn == false){ return 2; } //位置が変更、数は不変
    return 0; //位置，数変更なし
}
// ---------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------
int GHDSS_exec::ChangeSrcPos_W(std::vector<Location> &src_pos_cur)
{
    size_t Npos_ini,Npos_cur,Nmic,Nfreq;
    //size_t lower, upper;
    bool bRtn ,bRtnID;
    int iRtn = 2;
    Nmic = this->SepMat.sm.num_inputs;
    Npos_ini = this->SepMat.TFDB.loc_src.size();
    //lower = this->SepMat.sm.freq_index[0];
    //upper = this->SepMat.sm.freq_index.size() + lower - 1;
    Nfreq = this->SepMat.sm.freq_index.size();
    Npos_cur = src_pos_cur.size();

    //配列数の切りなおし
    if(Npos_ini != Npos_cur){ iRtn = 1; }
    if(this->updated_methods_w.size() != Npos_cur){
        this->updated_methods_w.resize(Npos_cur);
    }
    // Change_Src_Pos_TF_CONJで切り直されているはず
    if(this->SepMat.sm.sep_mat.size() != Npos_cur * Nfreq * Nmic){
        //this->SepMat.sm = SeparationMatrix(Nfreq, Npos_cur, Nmic);
        this->SepMat.sm.resize(Nfreq, Npos_cur, Nmic);
    }
    //音源情報の更新
    this->AssignSrcPos(src_pos_cur);
    // 分離行列の更新
    this->InitSepMatrix();
    bRtn = false;
    bRtnID = false;
    int inherit;
    inherit = 0;
    //分離行列の引継ぎ確認
    for(size_t i = 0; i < Npos_cur; i++){
        this->updated_methods_w[i] = this->update_method_w;
        if(this->update_method_w == UPDATE_METHOD::ID
        || this->update_method_w == UPDATE_METHOD::ID_POS){
            // IDベースの確認
            bRtnID = this->_CheckSrcID_W(i);
            if(bRtnID == true){ inherit++; }
        }
        //実際の更新はID or POS
        if(this->update_method_w == UPDATE_METHOD::ID_POS){
            this->updated_methods_w[i] = bRtnID ? UPDATE_METHOD::ID : UPDATE_METHOD::POS;
        }
        if(this->updated_methods_w[i] == UPDATE_METHOD::POS){
            //POSベースの確認
            bRtn = this->_CheckSrcPos_W(i);
            if(bRtn == true){ inherit++; }
            else{
                if(this->update_method_w == UPDATE_METHOD::ID_POS){
                    this->updated_methods_w[i] = UPDATE_METHOD::ID;
                }
            }
        }
    }
    if(iRtn == 1){ return 1; } //音源数、変更
    if(inherit == Npos_cur){ return 0; } //全て変更なし
    return 2; //一部変更
}
// ----------------------------------------------------------------
bool GHDSS_exec::_CheckSrcID_W(int id_cur)
{
    size_t Npos_cur, Npos_init, Nmic, Nfreq;
    //size_t lower, upper;
    int inherit;
    int idsave;
    Npos_cur = this->SepMat.TFDB.loc_src.size();
    Npos_init = this->update_w.SMDB.loc_src.size();
    Nmic = this->SepMat.sm.num_inputs;
    //lower = this->SepMat.sm.freq_index[0];
    //upper = this->SepMat.sm.freq_index.size() + lower - 1;
    Nfreq = this->SepMat.sm.freq_index.size();

    //前フレームの音源IDと比較(Create or Exist)
    idsave = this->_CheckFrontSrcLoc(this->SepMat.TFDB.loc_src[id_cur], UPDATE_METHOD::ID);
    if(idsave == INVALID_SRCID){ return false; }

    inherit = 0;
    if(this->update_w.Update[idsave] == SRCID_PREVIOUS_STATE::EXISTS){
    //分離行列の引継ぎ
        for(size_t k = 0; k < this->SepMat.sm.freq_index.size(); k++){
            for(size_t m = 0; m < this->SepMat.sm.num_inputs; m++){
                this->SepMat.sm.sep_mat[id_cur+m*Npos_cur+k*Nmic*Npos_cur]
                    = this->update_w.SMDB.sm.sep_mat[idsave+m*Npos_init+k*Nmic*Npos_init];
            }
        }
        this->SepMat.TFDB.loc_src[id_cur].x[0] = this->update_w.SMDB.loc_src[idsave].x[0];
        this->SepMat.TFDB.loc_src[id_cur].x[1] = this->update_w.SMDB.loc_src[idsave].x[1];
        this->SepMat.TFDB.loc_src[id_cur].x[2] = this->update_w.SMDB.loc_src[idsave].x[2];
        this->SepMat.TFDB.loc_src[id_cur].p[0] = this->update_w.SMDB.loc_src[idsave].p[0];
        this->SepMat.TFDB.loc_src[id_cur].p[1] = this->update_w.SMDB.loc_src[idsave].p[1];
        this->SepMat.TFDB.loc_src[id_cur].p[2] = this->update_w.SMDB.loc_src[idsave].p[2];
        this->SepMat.TFDB.loc_src[id_cur].idpos = this->update_w.SMDB.loc_src[idsave].idpos;
        this->SepMat.TFDB.loc_src[id_cur].tfpos = this->update_w.SMDB.loc_src[idsave].tfpos;
        inherit++;
    }
    if(inherit == 1){ return true; } //音源変更なし(前フレームから引継)
    return false;
}
// -----------------------------------------------------------------
bool GHDSS_exec::_CheckSrcPos_W(int id_cur)
{
    size_t Npos_cur, Npos_init, Nmic, Nfreq;
    //size_t lower, upper;
    bool btmp;
    int iIDpos, inherit;
    //MICARY_Location locPos;
    Npos_cur = this->SepMat.TFDB.loc_src.size();
    Npos_init = this->update_w.SMDB.loc_src.size();
    Nmic = this->SepMat.sm.num_inputs;
    //lower = this->SepMat.sm.freq_index[0];
    //upper = this->SepMat.sm.freq_index.size() + lower - 1;
    Nfreq = this->SepMat.sm.freq_index.size();

    //前フレームの音源IDと比較(Create or Exist)
    iIDpos = this->_CheckFrontSrcLoc(this->SepMat.TFDB.loc_src[id_cur], UPDATE_METHOD::POS);
    if(iIDpos == INVALID_SRCID){ return false; }

    inherit = 0;
    if(this->update_w.Update[iIDpos] == SRCID_PREVIOUS_STATE::EXISTS){
//std::cout << "-EXIST" << iIDpos;
        for(size_t k = 0; k < Nfreq; k++){
            for(size_t m = 0; m < Nmic; m++){
                this->SepMat.sm.sep_mat[id_cur + m * Npos_cur + k * Nmic * Npos_cur]
                    = this->update_w.SMDB.sm.sep_mat[iIDpos+m*Npos_init+k*Nmic*Npos_init];
            }
        }
        this->SepMat.TFDB.loc_src[id_cur].x[0] = this->update_w.SMDB.loc_src[iIDpos].x[0];
        this->SepMat.TFDB.loc_src[id_cur].x[1] = this->update_w.SMDB.loc_src[iIDpos].x[1];
        this->SepMat.TFDB.loc_src[id_cur].x[2] = this->update_w.SMDB.loc_src[iIDpos].x[2];
        this->SepMat.TFDB.loc_src[id_cur].p[0] = this->update_w.SMDB.loc_src[iIDpos].p[0];
        this->SepMat.TFDB.loc_src[id_cur].p[1] = this->update_w.SMDB.loc_src[iIDpos].p[1];
        this->SepMat.TFDB.loc_src[id_cur].p[2] = this->update_w.SMDB.loc_src[iIDpos].p[2];
        this->SepMat.TFDB.loc_src[id_cur].idpos =
            this->update_w.SMDB.loc_src[iIDpos].idpos;
        this->SepMat.TFDB.loc_src[id_cur].tfpos =
            this->update_w.SMDB.loc_src[iIDpos].tfpos;
        inherit++;
    }
    if(inherit == 1){ return true; } //音源変更なし
    return false;
}
// ----------------------------------------------------------------------------------------
// 2点の位置がThreshold_lengthで設定された範囲内であるかの判定(座標計算)
bool GHDSS_exec::_IsThreshPosByCoord(Location &ref_pos, Location &obj_pos)
{
    float fdist = this->_EuclidDistance(ref_pos, obj_pos);
    return (fdist < this->acceptable_distance);
}
// 2点の位置がThreshold_lengthで設定された範囲内であるかの判定(Neighbor利用)
bool GHDSS_exec::_IsThreshPos(Location &ref_pos, Location &obj_pos)
{
    int ref_index;
    if(!this->harkio_tf.pPrinbrs){
        return (this->_IsThreshPosByCoord(ref_pos, obj_pos));
    }
    ref_index = this->harkio_tf.GetPosIndex(ref_pos.idpos);
    for(size_t i = 0; i < this->harkio_tf.pPrinbrs->numNeighbors[ref_index]; i++){
        if(this->harkio_tf.pPrinbrs->neighbors[ref_index][i] == obj_pos.tfpos){
            return true;
        }
    }
    return false;
}
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
/*
void GHDSS_exec::_Ang2Pos(Location &pos)
{
    float theta, phi, radius, pi;
    pi = std::acos(-1.0);
    theta = pos.p[0] * pi / 180.0f;
    phi = pos.p[1] * pi / 180.0f;
    radius = pos.p[2];
    pos.x[0] = radius * std::cos(phi) * std::cos(theta);
    pos.x[1] = radius * std::cos(phi) * std::sin(theta);
    pos.x[2] = radius * std::sin(phi);
    return;
}
*/
// -------------------------------------------------------
size_t GHDSS_exec::_GetNvalidUpdateW()
{
    size_t ret;
    ret = 0;
    //for(size_t i = 0; i < this->update_w.Update.size(); i++){
    for(size_t i = 0; i < this->update_w.SMDB.loc_src.size(); i++){
        if(this->update_w.Update[i] != SRCID_PREVIOUS_STATE::INVALID){
            ret++;
        }
    }
    return ret;
}
void GHDSS_exec::ExportUpdateW(const std::string &filename)
{
    size_t valid_size, num_mic, num_freq, num_pos, iiv;
    valid_size = this->_GetNvalidUpdateW(); // A method that is not called elsewhere.
    if(valid_size == 0){ return; }
    num_mic = this->update_w.SMDB.sm.num_inputs;
    num_pos = this->update_w.SMDB.sm.num_outputs;
    num_freq = this->update_w.SMDB.sm.freq_index.size();
    SMDatabase valid_w(num_freq, valid_size, num_mic);
    for(size_t i = 0; i < num_mic; i++){
        valid_w.loc_mic[i].x[0] = this->update_w.SMDB.loc_mic[i].x[0];
        valid_w.loc_mic[i].x[1] = this->update_w.SMDB.loc_mic[i].x[1];
        valid_w.loc_mic[i].x[2] = this->update_w.SMDB.loc_mic[i].x[2];
        valid_w.loc_mic[i].idpos = this->update_w.SMDB.loc_mic[i].idpos;
        valid_w.loc_mic[i].tfpos = this->update_w.SMDB.loc_mic[i].tfpos;
    }
    for(size_t i = 0; i < num_freq; i++){
        valid_w.sm.freq_index[i] = this->update_w.SMDB.sm.freq_index[i];
    }
    iiv = 0;
    for(size_t i = 0; i < num_pos; i++){
        if(this->update_w.Update[i] == SRCID_PREVIOUS_STATE::CREATE ||
           this->update_w.Update[i] == SRCID_PREVIOUS_STATE::EXISTS){
            valid_w.loc_src[iiv].x[0] = this->update_w.SMDB.loc_src[i].x[0];
            valid_w.loc_src[iiv].x[1] = this->update_w.SMDB.loc_src[i].x[1];
            valid_w.loc_src[iiv].x[2] = this->update_w.SMDB.loc_src[i].x[2];
            valid_w.loc_src[iiv].idpos = this->update_w.SMDB.loc_src[i].idpos;
            valid_w.loc_src[iiv].tfpos = this->update_w.SMDB.loc_src[i].tfpos;

            for(size_t k = 0; k < num_freq; k++){
                for(size_t m = 0; m < num_mic; m++){
                    valid_w.sm.sep_mat[iiv+m*valid_size+k*num_mic*valid_size]
                        = this->update_w.SMDB.sm.sep_mat[i+m*num_pos+k*num_mic*num_pos];
                }
            }
            iiv++;
            if(iiv == valid_size){ break; }
        }
    }
    {
        this->harkio_w.sampling_rate = this->sampling_rate;
        this->harkio_w.num_fft = this->length;
        //this->harkio_w.lNMic = num_mic;
        //this->harkio_w.lNSrc = num_pos;
        //this->harkio_w.lNFreq = num_freq;
        //this->harkio_w.iVersion = 1;
        this->harkio_w.file_type = HarkFileIO::FORMAT_TYPE::W;
        // 座標を伝達関数ファイルに対応したものに丸める
        for(size_t i = 0; i < num_pos; i++){
            valid_w.loc_src[i].x[0] = this->TFDB.loc_src[valid_w.loc_src[i].tfpos].x[0];
            valid_w.loc_src[i].x[1] = this->TFDB.loc_src[valid_w.loc_src[i].tfpos].x[1];
            valid_w.loc_src[i].x[2] = this->TFDB.loc_src[valid_w.loc_src[i].tfpos].x[2];
        }
        std::vector<hark::Position> pos_mic = this->LocationsToCartesianPositions(valid_w.loc_mic);
        std::vector<hark::Position> pos_src = this->LocationsToCartesianPositions(valid_w.loc_src);
        this->harkio_w.SetW(filename,
                            valid_w.sm.freq_index,
                            pos_mic,
                            pos_src,
                            valid_w.sm.sep_mat);
        this->harkio_w.WriteData(filename);
    }
}
// ---------------------------------------------------------
// 音源消滅の確認
void GHDSS_exec::_SetRemoveSrc()
{
    int index1, index2;
    bool bRemove = true;
    for(size_t i = 0; i < this->previous_loc_src.size(); i++){
        switch(this->previous_loc_src[i].valid){
        case UPDATE_METHOD::POS:
            // ここは，音源があるかどうかが重要なので近くない値でも拾わなければならない
            // 1フレーム前に音源があってかつ，それが引き継ぎ範囲外であるかを確認している。
            //j = this->GetNearestPos_ALLSearch(this->previous_loc_src[i], this->SepMat.TFDB.loc_src);
            // 1フレーム前に音源があってかつ，それが引き継ぎ範囲外であるかを確認(処理を整理)
/*
            j = this->GetNearestPos(this->previous_loc_src[i], this->SepMat.TFDB.loc_src);
            if(j != INVALID_SRCID){
                bRemove = this->_IsThreshPos(this->previous_loc_src[i], this->SepMat.TFDB.loc_src[j]);    // ここはneighbor処理でよい
                if(!bRemove){
                    k = this->GetNearestPos(this->previous_loc_src[i], this->update_w.SMDB.loc_src);
                    if(k != INVALID_SRCID){
                        this->update_w.Update[k] = SRCID_PREVIOUS_STATE::REMOVE;
                        #ifdef __DEBUG_UPDATEW
                        std::cout << "Src REMOVE " << std::endl;
                        #endif
                    }
                }
            }
*/
            index1 = this->GetNearestPos(this->previous_loc_src[i], this->SepMat.TFDB.loc_src, 1);
            if(index1 == INVALID_SRCID){ // 現フレームに前フレームの座標がない
                // this->update_w 中のインデックスを検索
                index2 = this->GetNearestPos(this->previous_loc_src[i], this->update_w.SMDB.loc_src, 1);
                if(index2 != INVALID_SRCID){
                    size_t k = static_cast<size_t>(index2);
                    this->update_w.Update[k] = SRCID_PREVIOUS_STATE::REMOVE;
                    #ifdef __DEBUG_UPDATEW
                    std::cout << "Src REMOVE " << std::endl;
                    #endif
                }
            }
            break;
        case UPDATE_METHOD::ID:
            bRemove = true;
            for(size_t j = 0; j < this->SepMat.TFDB.loc_src.size(); j++){
                if(this->previous_loc_src[i].idpos == this->SepMat.TFDB.loc_src[j].idpos){
                    bRemove = false;
                    break;
                }
            }
            if(bRemove){ //前フレームIDが現在IDにない=消滅した場合
                for(size_t k = 0; k < this->update_w.SMDB.loc_src.size(); k++){
                    if(this->update_w.SMDB.loc_src[k].idpos == this->previous_loc_src[i].idpos){
                        this->update_w.Update[k] = SRCID_PREVIOUS_STATE::REMOVE;
                        break;
                    }
                }
            }
            break;
        case UPDATE_METHOD::ID_POS:
            bRemove = true;
            for(size_t j = 0; j < this->SepMat.TFDB.loc_src.size(); j++){
                if(this->previous_loc_src[i].idpos == this->SepMat.TFDB.loc_src[j].idpos){
                    bRemove = false;
                    break;
                }
            }
            if(bRemove){
                // 前フレームの音源位置が現フレームの許容範囲にない
                /*
                index1 = this->GetNearestPos(this->previous_loc_src[i], this->SepMat.TFDB.loc_src);
                if(index1 != INVALID_SRCID){
                    size_t j = static_cast<size_t>(index1);
                    bool btmp = this->_IsThreshPos(this->previous_loc_src[i], this->SepMat.TFDB.loc_src[j]);
                    if(btmp){ bRemove = false; }
                }
                */
                index1 = this->GetNearestPos(this->previous_loc_src[i], this->SepMat.TFDB.loc_src, 1);
                if(index1 != INVALID_SRCID){ bRemove = false; } // 許容範囲内で見つかった
            }
            if(bRemove){    //消滅
                // 現フレームに同じ音源位置があれば、m_Updateに保存される音源位置が複数にならないようにINVALID_SRCIDを登録する
                /*
                index1 = this->GetNearestPos(this->previous_loc_src[i], this->update_w.SMDB.loc_src, 1);
                if(index1 != INVALID_SRCID){
                    size_t j = static_cast<size_t>(index1);
                    this->update_w.Update[j] = SRCID_PREVIOUS_STATE::REMOVE;
                }
                */
                SRCID_PREVIOUS_STATE previous_state = SRCID_PREVIOUS_STATE::REMOVE;
                for(size_t k = i+1; k < this->SepMat.TFDB.loc_src.size(); k++){
                    if(this->previous_loc_src[k].tfpos == this->previous_loc_src[i].tfpos){
                        previous_state = SRCID_PREVIOUS_STATE::INVALID;
                        break;
                    }
                }
                size_t k = this->GetNearestPos(this->previous_loc_src[i], this->update_w.SMDB.loc_src, 1);
                if(k != INVALID_SRCID){
                    this->update_w.Update[k] = previous_state;
                    // Also, the position must be set to INVALID_SRCID.
                    if(previous_state == SRCID_PREVIOUS_STATE::INVALID){
                        this->update_w.SMDB.loc_src[k].idpos = INVALID_SRCID;
                        this->update_w.SMDB.loc_src[k].tfpos = INVALID_SRCID;
                    }
                }
            }
            break;
        }
    }
    // ID_POSのとき、this->update_w に SRCID_PREVIOUS_STATE::EXISTS として存在している音源位置と同じものが SRCID_PREVIOUS_STATE::REMOVE として存在している場合、INVALID_SRCIDに変更する
    for(size_t i = 0; i < this->update_w.SMDB.loc_src.size(); i++){
        if(this->update_w.Update[i] != SRCID_PREVIOUS_STATE::EXISTS){ continue; }
        for(size_t j = 0; j < this->update_w.SMDB.loc_src.size(); j++){
            if(i == j){ continue; } // skip myself.
            if(this->update_w.Update[j] != SRCID_PREVIOUS_STATE::REMOVE){ continue; }
            if(this->update_w.SMDB.loc_src[i].tfpos ==
               this->update_w.SMDB.loc_src[j].tfpos){
                this->update_w.Update[j] = SRCID_PREVIOUS_STATE::INVALID;
                // Also, the position must be set to INVALID_SRCID.
                this->update_w.SMDB.loc_src[j].idpos = INVALID_SRCID;
                this->update_w.SMDB.loc_src[j].tfpos = INVALID_SRCID;
            }
        }
    }
    // 配列サイズの切りなおし
    if(this->previous_loc_src.size() != this->SepMat.sm.num_outputs){
        this->previous_loc_src.resize(this->SepMat.sm.num_outputs);
    }
    for(size_t i = 0; i < this->previous_loc_src.size(); i++){
        this->previous_loc_src[i].x[0] = this->SepMat.TFDB.loc_src[i].x[0];
        this->previous_loc_src[i].x[1] = this->SepMat.TFDB.loc_src[i].x[1];
        this->previous_loc_src[i].x[2] = this->SepMat.TFDB.loc_src[i].x[2];
        this->previous_loc_src[i].p[0] = this->SepMat.TFDB.loc_src[i].p[0];
        this->previous_loc_src[i].p[1] = this->SepMat.TFDB.loc_src[i].p[1];
        this->previous_loc_src[i].p[2] = this->SepMat.TFDB.loc_src[i].p[2];
        this->previous_loc_src[i].idpos = this->SepMat.TFDB.loc_src[i].idpos;
        this->previous_loc_src[i].tfpos = this->SepMat.TFDB.loc_src[i].tfpos;
        this->previous_loc_src[i].valid = this->updated_methods_w[i];
    }
}

void GHDSS_exec::SetUpdateW()
{
    size_t iPos, Nmic, Npos_cur, Npos_all, Nfreq;
    //size_t lower, upper;
    Nmic = this->SepMat.sm.num_inputs;
    Npos_cur = this->SepMat.TFDB.loc_src.size();
    Npos_all = this->update_w.SMDB.loc_src.size();
    //lower = this->SepMat.sm.freq_index[0];
    //upper = this->SepMat.sm.freq_index.size() + lower - 1;
    Nfreq = this->SepMat.sm.freq_index.size();
    if(this->updated_methods_w.size() == 0){
        this->updated_methods_w.resize(Npos_cur);
        for(size_t i = 0; i < Npos_cur; i++){
            this->updated_methods_w[i] = this->update_method_w;
        }
    }
    for(size_t i = 0; i < Npos_cur; i++){
        switch(this->updated_methods_w[i]){
        case UPDATE_METHOD::POS:
            iPos = this->GetNearestPos(this->SepMat.TFDB.loc_src[i], this->update_w.SMDB.loc_src, 1);
            if(iPos == INVALID_SRCID){
              for(size_t j = 0; j < Npos_all; j++){
                if(this->update_w.Update[j] == SRCID_PREVIOUS_STATE::INVALID){ // 新規に保存
                    iPos = j;
                    break;
                }
              }
            }
            for(size_t k = 0; k < Nfreq; k++){
                for(size_t m = 0; m < Nmic; m++){
                    this->update_w.SMDB.sm.sep_mat[iPos+m*Npos_all+k*Npos_all*Nmic] =
                        this->SepMat.sm.sep_mat[i+m*Npos_cur+k*Npos_cur*Nmic];
                }
            }
            //音源情報の更新
            this->update_w.SMDB.loc_src[iPos].x[0] = this->SepMat.TFDB.loc_src[i].x[0];
            this->update_w.SMDB.loc_src[iPos].x[1] = this->SepMat.TFDB.loc_src[i].x[1];
            this->update_w.SMDB.loc_src[iPos].x[2] = this->SepMat.TFDB.loc_src[i].x[2];
            this->update_w.SMDB.loc_src[iPos].p[0] = this->SepMat.TFDB.loc_src[i].p[0];
            this->update_w.SMDB.loc_src[iPos].p[1] = this->SepMat.TFDB.loc_src[i].p[1];
            this->update_w.SMDB.loc_src[iPos].p[2] = this->SepMat.TFDB.loc_src[i].p[2];
            this->update_w.SMDB.loc_src[iPos].idpos = this->SepMat.TFDB.loc_src[i].idpos;
            this->update_w.SMDB.loc_src[iPos].tfpos = this->SepMat.TFDB.loc_src[i].tfpos;
            //更新履歴の更新
            this->update_w.Update[iPos] = SRCID_PREVIOUS_STATE::EXISTS;
            break;
        case UPDATE_METHOD::ID:
        case UPDATE_METHOD::ID_POS:
            this->updated_methods_w[i] = UPDATE_METHOD::ID;
            iPos = Npos_all;
            //std::cout << "UpdateWPos:";
            for(size_t j = 0; j < Npos_all; j++){
                if(this->SepMat.TFDB.loc_src[i].idpos ==
                    this->update_w.SMDB.loc_src[j].idpos){
                    iPos = j;
                    break;
                }
            }
            if(iPos >= Npos_all){
            //std::cout << "Search-->";
                ////DEBUG////std::cout << "this->update_w.Update.size()=" << this->update_w.Update.size() << ", Npos_all=" << Npos_all << std::endl;
                for(size_t j = 0; j < Npos_all; j++){
                    if(this->update_w.Update[j] == SRCID_PREVIOUS_STATE::INVALID){
                        iPos = j;
                        break;
                    }
                }
            }
            //std::cout << iPos << std::endl;
            for(size_t k = 0; k < Nfreq; k++){
                for(size_t m = 0; m < Nmic; m++){
                    this->update_w.SMDB.sm.sep_mat[iPos+m*Npos_all+k*Npos_all*Nmic] =
                        this->SepMat.sm.sep_mat[i+m*Npos_cur+k*Npos_cur*Nmic];
                }
            }
            //音源情報の更新
            this->update_w.SMDB.loc_src[iPos].x[0] = this->SepMat.TFDB.loc_src[i].x[0];
            this->update_w.SMDB.loc_src[iPos].x[1] = this->SepMat.TFDB.loc_src[i].x[1];
            this->update_w.SMDB.loc_src[iPos].x[2] = this->SepMat.TFDB.loc_src[i].x[2];
            this->update_w.SMDB.loc_src[iPos].p[0] = this->SepMat.TFDB.loc_src[i].p[0];
            this->update_w.SMDB.loc_src[iPos].p[1] = this->SepMat.TFDB.loc_src[i].p[1];
            this->update_w.SMDB.loc_src[iPos].p[2] = this->SepMat.TFDB.loc_src[i].p[2];
            this->update_w.SMDB.loc_src[iPos].idpos = this->SepMat.TFDB.loc_src[i].idpos;
            this->update_w.SMDB.loc_src[iPos].tfpos = this->SepMat.TFDB.loc_src[i].tfpos;
            //更新履歴の更新
            this->update_w.Update[iPos] = SRCID_PREVIOUS_STATE::EXISTS;
        break;
        default :
        break;
        }
    }
    this->_SetRemoveSrc(); // A method that is not called.
    return;
}
// ---------------------------------------------------------
std::complex<float> GHDSS_exec::GetUpdateW(int isrc, int imic, int ifreq)
{
  int num_mic = this->SepMat.sm.num_inputs;
  int num_pos_cur = this->SepMat.TFDB.loc_src.size();
  int num_freq = this->SepMat.sm.freq_index.size();
  ////DEBUG////std::cout << "@GetUpdateW(" << isrc << ", " << imic << ", " << ifreq << ")\n" << "num_mic=" << num_mic << ", num_pos_cur=" << num_pos_cur << ", num_freq=" << num_freq << ", this->SepMat.sm.sep_mat.size()=" << this->SepMat.sm.sep_mat.size() << std::endl;
  return this->SepMat.sm.sep_mat[isrc + imic * num_pos_cur + ifreq * num_pos_cur * num_mic];
}
// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------
// 前フレームの音源と比較
int GHDSS_exec::_CheckFrontSrcLoc(Location &cur_loc, UPDATE_METHOD update_method_w)
{
    int index1, index2;
    switch(update_method_w){
    case UPDATE_METHOD::POS:
        //現フレーム音源と１フレーム前音源のPositionを検索
        // ここは，音源があるかどうかが重要なので近くない値でも拾わなければならない
        //index1 = this->GetNearestPos_ALLSearch(cur_loc, this->previous_loc_src);
        // ここは，音源があるかどうかが重要なので近くない値でも拾わなければならない処理を整理
        /*
        index1 = this->GetNearestPos(cur_loc, this->previous_loc_src);
        if(index1 != INVALID_SRCID){    //見つかった場合
            //許容範囲内か確認
            bool btmp = this->_IsThreshPos(cur_loc, this->previous_loc_src[i]);
            if(btmp == true){    //許容範囲内(存在している)ならば，保存値データで検索
                // 1フレーム前の音源位置で検索
                index2 = this->GetNearestPos(this->previous_loc_src[i], this->update_w.SMDB.loc_src);
                if(index2 != INVALID_SRCID){
                    size_t j = static_cast<size_t>(index2);
                    this->update_w.Update[j] = SRCID_PREVIOUS_STATE::EXISTS;
                    //保存値データを現フレーム音源位置に更新
                    this->update_w.SMDB.loc_src[j].x[0] = cur_loc.x[0];
                    this->update_w.SMDB.loc_src[j].x[1] = cur_loc.x[1];
                    this->update_w.SMDB.loc_src[j].x[2] = cur_loc.x[2];
                    this->update_w.SMDB.loc_src[j].p[0] = cur_loc.p[0];
                    this->update_w.SMDB.loc_src[j].p[1] = cur_loc.p[1];
                    this->update_w.SMDB.loc_src[j].p[2] = cur_loc.p[2];
                    this->update_w.SMDB.loc_src[j].idpos = this->previous_loc_src[i].idpos;
                    return static_cast<int>(j);
                }
            }
            //1フレーム前の音源とは異なる場合，以前に出現した位置か？（保存値データで検索）
            for(size_t j = 0; j < this->update_w.SMDB.loc_src.size(); j++){
                if(this->update_w.Update[j] != SRCID_PREVIOUS_STATE::INVALID){
                    btmp = this->_IsThreshPos(cur_loc, this->update_w.SMDB.loc_src[j]);
                    if(btmp == true){    // 保存値データで見つかった場合，更新
                        this->update_w.Update[j] = SRCID_PREVIOUS_STATE::EXISTS; //復活
                        this->update_w.SMDB.loc_src[j].x[0] = cur_loc.x[0];
                        this->update_w.SMDB.loc_src[j].x[1] = cur_loc.x[1];
                        this->update_w.SMDB.loc_src[j].x[2] = cur_loc.x[2];
                        this->update_w.SMDB.loc_src[j].p[0] = cur_loc.p[0];
                        this->update_w.SMDB.loc_src[j].p[1] = cur_loc.p[1];
                        this->update_w.SMDB.loc_src[j].p[2] = cur_loc.p[2];
                        //this->update_w.SMDB.loc_src[j].idpos = cur_loc.idpos;
                        this->update_w.SMDB.loc_src[j].tfpos = cur_loc.tfpos;
                        return static_cast<int>(j);
                    }
                }
            }
            // 1フレーム前にはない音源=CREATEならば、新規に保存する場所の番号を返す
            for(size_t j = 0; j < this->update_w.SMDB.loc_src.size(); j++){
                if(this->update_w.Update[j] == SRCID_PREVIOUS_STATE::INVALID){
                    this->update_w.SMDB.loc_src[j].x[0] = cur_loc.x[0];
                    this->update_w.SMDB.loc_src[j].x[1] = cur_loc.x[1];
                    this->update_w.SMDB.loc_src[j].x[2] = cur_loc.x[2];
                    this->update_w.SMDB.loc_src[j].p[0] = cur_loc.p[0];
                    this->update_w.SMDB.loc_src[j].p[1] = cur_loc.p[1];
                    this->update_w.SMDB.loc_src[j].p[2] = cur_loc.p[2];
                    this->update_w.SMDB.loc_src[j].idpos = cur_loc.idpos;
                    this->update_w.SMDB.loc_src[j].tfpos = cur_loc.tfpos;
                    this->update_w.Update[j] = SRCID_PREVIOUS_STATE::CREATE;
                    return static_cast<int>(j);
                }
            }
        }
        */
        index1 = this->GetNearestPos(cur_loc, this->update_w.SMDB.loc_src);
        if(index1 != INVALID_SRCID){
            // If a sound source location within the acceptable range is found,
            // update the coordinates and return the corresponding index.
            size_t j = static_cast<size_t>(index1);
            // Updates the saved sound source location information with
            // the location information of the current frame.
            this->update_w.SMDB.loc_src[j].x[0] = cur_loc.x[0];
            this->update_w.SMDB.loc_src[j].x[1] = cur_loc.x[1];
            this->update_w.SMDB.loc_src[j].x[2] = cur_loc.x[2];
            this->update_w.SMDB.loc_src[j].p[0] = cur_loc.p[0];
            this->update_w.SMDB.loc_src[j].p[1] = cur_loc.p[1];
            this->update_w.SMDB.loc_src[j].p[2] = cur_loc.p[2];
            this->update_w.SMDB.loc_src[j].idpos = cur_loc.idpos;
            this->update_w.SMDB.loc_src[j].tfpos = cur_loc.tfpos;
            // We don't care if it was revived or was in the previous frame.
            this->update_w.Update[j] = SRCID_PREVIOUS_STATE::EXISTS;
            return static_cast<int>(j);
        }
        else{
            // If no acceptable sound source location is found, search unused index,
            // update the coordinates and return the new stored index.
            // Notes:
            //   If there are no unused index, exit the loop and return INVALID_SRCID.
            for(size_t j = 0; j < this->update_w.SMDB.loc_src.size(); j++){
                if(this->update_w.Update[j] == SRCID_PREVIOUS_STATE::INVALID){
                    this->update_w.SMDB.loc_src[j].x[0] = cur_loc.x[0];
                    this->update_w.SMDB.loc_src[j].x[1] = cur_loc.x[1];
                    this->update_w.SMDB.loc_src[j].x[2] = cur_loc.x[2];
                    this->update_w.SMDB.loc_src[j].p[0] = cur_loc.p[0];
                    this->update_w.SMDB.loc_src[j].p[1] = cur_loc.p[1];
                    this->update_w.SMDB.loc_src[j].p[2] = cur_loc.p[2];
                    this->update_w.SMDB.loc_src[j].idpos = cur_loc.idpos;
                    this->update_w.SMDB.loc_src[j].tfpos = cur_loc.tfpos;
                    this->update_w.Update[j] = SRCID_PREVIOUS_STATE::CREATE;
                    //std::cout << "src CREATE:" << j << std::endl;
                    return static_cast<int>(j);
                }
            }
        }
        break;
    case UPDATE_METHOD::ID:
        for(size_t i = 0; i < this->previous_loc_src.size(); i++){
            if(this->previous_loc_src[i].idpos == cur_loc.idpos){
                for(size_t j = 0; j < this->update_w.SMDB.loc_src.size(); j++){
                    if(this->update_w.SMDB.loc_src[j].idpos == cur_loc.idpos){
                        if(this->update_w.Update[j] == SRCID_PREVIOUS_STATE::CREATE ||
                           this->update_w.Update[j] == SRCID_PREVIOUS_STATE::EXISTS){
                            this->update_w.Update[j] = SRCID_PREVIOUS_STATE::EXISTS;
                            return static_cast<int>(j);
                        }
                    }
                }
            }
        }
        /*
        // This is clearly a bug that exists from the OSS version of HARK.
        // This condition will always be false as long as everything is
        // working properly (i.e. this->update_w is not nullptr).
        if(this->update_w == UPDATE_METHOD::ID){
            // 1フレーム前の音源にはない音源=CREATEならば、新規に保存する場所の番号を返す
            for(size_t j = 0; j < this->update_w.SMDB.loc_src.size(); j++){
                if(this->update_w.Update[j] == SRCID_PREVIOUS_STATE::INVALID){
                    this->update_w.SMDB.loc_src[j].x[0] = cur_loc.x[0];
                    this->update_w.SMDB.loc_src[j].x[1] = cur_loc.x[1];
                    this->update_w.SMDB.loc_src[j].x[2] = cur_loc.x[2];
                    this->update_w.SMDB.loc_src[j].p[0] = cur_loc.p[0];
                    this->update_w.SMDB.loc_src[j].p[1] = cur_loc.p[1];
                    this->update_w.SMDB.loc_src[j].p[2] = cur_loc.p[2];
                    this->update_w.SMDB.loc_src[j].idpos = cur_loc.idpos;
                    this->update_w.SMDB.loc_src[j].tfpos = cur_loc.tfpos;
                    this->update_w.Update[j] = SRCID_PREVIOUS_STATE::CREATE;
                    //std::cout << "src CREATE:" << j << std::endl;
                    return static_cast<int>(j);
                }
            }
        }
        */
        break;
    case UPDATE_METHOD::ID_POS:
        {
            bool found_flag = false;
            for(size_t j = 0; j < this->previous_loc_src.size(); j++){
                // Matches the sound source ID of the previous frame.
                if(this->previous_loc_src[j].idpos == cur_loc.idpos){
                    found_flag = true;
                    break;
                }
            }
            if(found_flag){
                for(size_t j = 0; j < this->update_w.SMDB.loc_src.size(); j++){
                    if(this->update_w.SMDB.loc_src[j].idpos == cur_loc.idpos){
                        this->update_w.Update[j] = SRCID_PREVIOUS_STATE::EXISTS;
                        return static_cast<int>(j);
                    }
                }
            }
        }
        // 1フレーム前の音源にはない音源=CREATEならば、POSで検索
        /*
        bool btmp;
        int i = this->GetNearestPos(cur_loc, this->previous_loc_src);
        //許容範囲内かの確認
        if(i != INVALID_SRCID){
            btmp = _IsThreshPos(cur_loc, this->previous_loc_src[i]);
            if(btmp == true){    //許容範囲内(存在している)ならば，保存値データで検索
                // 1フレーム前の音源位置で検索
                j = this->GetNearestPos(this->previous_loc_src[i], this->update_w.SMDB.loc_src);
                if(j != INVALID_SRCID){
                    this->update_w.Update[j] = SRCID_PREVIOUS_STATE::EXISTS;
                }
                return j;
            }
            // 1フレーム前の音源にはない音源=CREATEならば、新規に保存する場所の番号を返す
            for(size_t j = 0; j < this->update_w.SMDB.loc_src.size(); j++){
                if(this->update_w.Update[j] == SRCID_PREVIOUS_STATE::INVALID){
                    this->update_w.SMDB.loc_src[j].x[0] = cur_loc.x[0];
                    this->update_w.SMDB.loc_src[j].x[1] = cur_loc.x[1];
                    this->update_w.SMDB.loc_src[j].x[2] = cur_loc.x[2];
                    this->update_w.SMDB.loc_src[j].p[0] = cur_loc.p[0];
                    this->update_w.SMDB.loc_src[j].p[1] = cur_loc.p[1];
                    this->update_w.SMDB.loc_src[j].p[2] = cur_loc.p[2];
                    this->update_w.SMDB.loc_src[j].idpos = cur_loc.idpos;
                    this->update_w.SMDB.loc_src[j].tfpos = cur_loc.tfpos;
                    this->update_w.Update[j] = SRCID_PREVIOUS_STATE::CREATE;
                    // std::cout << "source CREATE:" << j << std::endl;
                    return staric_cast<int>(j);
                }
            }
        }
        return INVALID_SRCID;
        */
        index1 = this->GetNearestPos(cur_loc, this->update_w.SMDB.loc_src);
        if(index1 != INVALID_SRCID){
            // If a sound source location within the acceptable range is found,
            // update the coordinates and return the corresponding index.
            size_t j = static_cast<size_t>(index1);
            // Updates the saved sound source location information with
            // the location information of the current frame.
            this->update_w.SMDB.loc_src[j].x[0] = cur_loc.x[0];
            this->update_w.SMDB.loc_src[j].x[1] = cur_loc.x[1];
            this->update_w.SMDB.loc_src[j].x[2] = cur_loc.x[2];
            this->update_w.SMDB.loc_src[j].p[0] = cur_loc.p[0];
            this->update_w.SMDB.loc_src[j].p[1] = cur_loc.p[1];
            this->update_w.SMDB.loc_src[j].p[2] = cur_loc.p[2];
            this->update_w.SMDB.loc_src[j].idpos = cur_loc.idpos;
            this->update_w.SMDB.loc_src[j].tfpos = cur_loc.tfpos;
            // We don't care if it was revived or was in the previous frame.
            this->update_w.Update[j] = SRCID_PREVIOUS_STATE::EXISTS;
            return static_cast<int>(j);
        }
        else {
            // If no acceptable sound source location is found, search unused index,
            // update the coordinates and return the new stored index.
            // Notes:
            //   If there are no unused index, exit the loop and return INVALID_SRCID.
            for(size_t j = 0; j < this->update_w.SMDB.loc_src.size(); j++){
                if(this->update_w.Update[j] == SRCID_PREVIOUS_STATE::INVALID){
                    this->update_w.SMDB.loc_src[j].x[0] = cur_loc.x[0];
                    this->update_w.SMDB.loc_src[j].x[1] = cur_loc.x[1];
                    this->update_w.SMDB.loc_src[j].x[2] = cur_loc.x[2];
                    this->update_w.SMDB.loc_src[j].p[0] = cur_loc.p[0];
                    this->update_w.SMDB.loc_src[j].p[1] = cur_loc.p[1];
                    this->update_w.SMDB.loc_src[j].p[2] = cur_loc.p[2];
                    this->update_w.SMDB.loc_src[j].idpos = cur_loc.idpos;
                    this->update_w.SMDB.loc_src[j].tfpos = cur_loc.tfpos;
                    this->update_w.Update[j] = SRCID_PREVIOUS_STATE::CREATE;
                    //std::cout << "src CREATE:" << j << std::endl;
                    return static_cast<int>(j);
                }
            }
        }
        break;
    }
    return INVALID_SRCID;
}
// ---------------------------------------------------------------------------
//--------------------------------------------------------------------------
// libharkio3となったとき、保存は一回で済ますことが望ましくなったため変更
void GHDSS_exec::AddUpdateW(const std::string &filename, bool export_W)
{
    size_t src, num_freq, num_src, num_mic;
    int idtmp;
    bool bUpdate;
    bool bRtn;
    SMDatabase tmp_w1, tmp_w2;
    SMDatabase *tmp_w_target, *tmp_w_latest;
    int current_target;    // 1->pstmpW, 2->pstmpW2を保存
    //tmp_w1 = NULL;
    //tmp_w2 = NULL;
    num_src = this->update_w.SMDB.loc_src.size();
    num_mic = this->update_w.SMDB.loc_mic.size();
    num_freq = this->update_w.SMDB.sm.freq_index.size();
    // エクスポートする場合は最初にファイルの情報を読み込んでおく
    if(export_W){
        tmp_w1 = SMDatabase(0, 0, 0);
        tmp_w2 = SMDatabase(0, 0, 0);
        src = 0;

        current_target = 1;
    }
    for(size_t i = 0; i < num_src; i++){
        if(this->update_w.Update[i] == SRCID_PREVIOUS_STATE::REMOVE &&
           this->update_w.SMDB.loc_src[i].idpos != INVALID_SRCID){
            if(!export_W){
                //フラグの更新
                // ID_POSの場合、POSでの分離行列情報を後で使用する場合がある
                //if(this->update_method_w == UPDATE_METHOD::ID
                //|| this->update_method_w == UPDATE_METHOD::ID_POS){
                if(this->update_method_w == UPDATE_METHOD::ID){
                    this->update_w.Update[i] = SRCID_PREVIOUS_STATE::INVALID;
//std::cout << "RESET to INVALID_SRCID" << i << std::endl;
                    for(size_t k = 0; k < num_freq; k++){
                        for(size_t m = 0; m < num_mic; m++){
                            this->update_w.SMDB.sm.sep_mat[k * num_mic * num_src
                                                         + m * num_src
                                                         + i] = std::complex<float>(0.0f, 0.0f);
                        }
                    }
                }
            }
            else{ //ExportW ()
//std::cout << "Export src = " << this->update_w.SMDB.loc_src[i].tfpos << std::endl;
                // 同じIDがあれば、更新
                // ポジションで検索してファイルデータベース更新ではないか？？
                bUpdate = false;
                if(current_target == 1){ tmp_w_target = &tmp_w1; }
                else if(current_target == 2){ tmp_w_target = &tmp_w2; }
                // this->update_wにあるもの全てを保存するので、許容範囲内判定はしない
//idtmp = this->GetNearestPos(this->update_w.SMDB.loc_Src[i], &tmp_w_target->loc_src);
                idtmp = this->GetNearestPos(this->update_w.SMDB.loc_src[i], tmp_w_target->loc_src, 1);
                if(idtmp != INVALID_SRCID){    // 許容範囲内
                    //std::cout << "Update Process" << std::endl;
                    /*
                    std::cout << "Current Pos = " << this->update_w.SMDB.loc_src[i].idpos
                              << " , " <<  this->update_w.SMDB.loc_src[i].x[0]
                              << " , " <<  this->update_w.SMDB.loc_src[i].x[1]
                              << " , " <<  this->update_w.SMDB.loc_src[i].x[2] << std::endl;
                    std::cout << "DBfile Pos = " << tmp_w1.loc_src[idtmp].idpos
                              << " , " <<  tmp_w1.loc_src[idtmp].x[0]
                              << " , " <<  tmp_w1.loc_src[idtmp].x[1]
                              << " , " <<  tmp_w1.loc_src[idtmp].x[2] << std::endl;
                    */
                    //分離行列の引継ぎ
                    for(size_t k = 0; k < num_freq; k++){
                        for(size_t m = 0; m < num_mic; m++){
                            tmp_w_target->sm.sep_mat[k * num_mic * src + m * src + idtmp]
                            = this->update_w.SMDB.sm.sep_mat[k * num_mic * num_src
                                                           + m * num_src + i];
                        }
                    }
                    //SrcPosも更新
                    tmp_w_target->loc_src[idtmp].x[0] = this->update_w.SMDB.loc_src[i].x[0];
                    tmp_w_target->loc_src[idtmp].x[1] = this->update_w.SMDB.loc_src[i].x[1];
                    tmp_w_target->loc_src[idtmp].x[2] = this->update_w.SMDB.loc_src[i].x[2];
                    // TFDB中の位置ID
                    tmp_w_target->loc_src[idtmp].tfpos = this->update_w.SMDB.loc_src[i].tfpos;
                    bUpdate = true;
                }
                if(!bUpdate){
                    //std::cout << "Add Process" << std::endl;
                    size_t new_num_src = src + 1;
                    //更新する音源がなければ追加
                    if(current_target == 1){
                        tmp_w2 = SMDatabase(num_freq, new_num_src, num_mic);
                        tmp_w_target = &tmp_w2;
                        tmp_w_latest = &tmp_w1;
                        current_target = 2;
                    }
                    else if(current_target == 2){
                        tmp_w1 = SMDatabase(num_freq, new_num_src, num_mic);
                        tmp_w_target = &tmp_w1;
                        tmp_w_latest = &tmp_w2;
                        current_target = 1;
                    }
                    //マイク座標
                    for(size_t j = 0; j < num_mic; j++){
                        tmp_w_target->loc_mic[j].x[0] = this->update_w.SMDB.loc_mic[j].x[0];
                        tmp_w_target->loc_mic[j].x[1] = this->update_w.SMDB.loc_mic[j].x[1];
                        tmp_w_target->loc_mic[j].x[2] = this->update_w.SMDB.loc_mic[j].x[2];
                        tmp_w_target->loc_mic[j].idpos = this->update_w.SMDB.loc_mic[j].idpos;
                        tmp_w_target->loc_mic[j].tfpos = this->update_w.SMDB.loc_mic[j].tfpos;
                    }
                    //周波数ライン
                    for(size_t j = 0; j < num_freq; j++){
                        tmp_w_target->sm.freq_index[j] =
                            this->update_w.SMDB.sm.freq_index[j];
                    }
                    //音源座標
                    for(size_t j = 0; j < src; j++){
                        tmp_w_target->loc_src[j].x[0] = tmp_w_latest->loc_src[j].x[0];
                        tmp_w_target->loc_src[j].x[1] = tmp_w_latest->loc_src[j].x[1];
                        tmp_w_target->loc_src[j].x[2] = tmp_w_latest->loc_src[j].x[2];
                        tmp_w_target->loc_src[j].idpos = tmp_w_latest->loc_src[j].idpos;
                        tmp_w_target->loc_src[j].tfpos = tmp_w_latest->loc_src[j].tfpos;
                    }
                    tmp_w_target->loc_src[src].x[0] = this->update_w.SMDB.loc_src[i].x[0];
                    tmp_w_target->loc_src[src].x[1] = this->update_w.SMDB.loc_src[i].x[1];
                    tmp_w_target->loc_src[src].x[2] = this->update_w.SMDB.loc_src[i].x[2];
                    tmp_w_target->loc_src[src].idpos = this->update_w.SMDB.loc_src[i].idpos;
                    tmp_w_target->loc_src[src].tfpos = this->update_w.SMDB.loc_src[i].tfpos;
                    /*
                    std::cout << "Add Pos = " << tmp_w2.loc_src[src].idpos
                              << " , " <<  tmp_w2.loc_src[src].x[0]
                              << " , " <<  tmp_w2.loc_src[src].x[1]
                              << " , " <<  tmp_w2.loc_src[src].x[2] << std::endl;
                    */
                    //ファイルデータを読み込み
                    for(size_t k = 0; k < num_freq; k++){
                        for(size_t m = 0; m < num_mic; m++){
                            for(size_t j = 0; j < src; j++){
                                tmp_w_target->sm.sep_mat[k * num_mic * new_num_src
                                                       + m * new_num_src + j]
                                = tmp_w_latest->sm.sep_mat[k * num_mic * src
                                                         + m * src + j];
                            }
                            tmp_w_target->sm.sep_mat[k * num_mic * new_num_src
                                                   + m * new_num_src + src]
                            = this->update_w.SMDB.sm.sep_mat[k * num_mic * num_src
                                                           + m * num_src + i];
                        }
                    }
                    src++; // 追加
                }
                //フラグの更新
                if(this->update_method_w == UPDATE_METHOD::ID){
                    this->update_w.Update[i] = SRCID_PREVIOUS_STATE::INVALID;
                }
            }
        }
    }
    if(export_W){
        std::cout << "Export file : " << filename << std::endl;
        if(current_target == 1){ tmp_w_target = &tmp_w1; }
        else if(current_target == 2){ tmp_w_target = &tmp_w2; }

        this->harkio_w.sampling_rate = this->sampling_rate;
        this->harkio_w.num_fft = this->length;
        //this->harkio_w.srcs = tmp_w_target->loc_src.size();
        //this->harkio_w.mics = tmp_w_target->loc_mic.size();
        //this->harkio_w.freqs = tmp_w_target->sm.freq_index.size();
        //this->harkio_w.iVersion = 1;
        this->harkio_w.file_type = HarkFileIO::FORMAT_TYPE::W;
        // 座標を伝達関数ファイルに対応したものに丸める
        for(size_t j = 0; j < tmp_w_target->loc_src.size(); j++){
            tmp_w_target->loc_src[j].x[0] =
                this->TFDB.loc_src[tmp_w_target->loc_src[j].tfpos].x[0];
            tmp_w_target->loc_src[j].x[1] =
                this->TFDB.loc_src[tmp_w_target->loc_src[j].tfpos].x[1];
            tmp_w_target->loc_src[j].x[2] =
                this->TFDB.loc_src[tmp_w_target->loc_src[j].tfpos].x[2];
        }
        /*
        std::cout << "File info : num_mic = " << tmp_w_target->loc_mic.size()
                  << " num_pos = " << tmp_w_target->loc_src.size()
                  << " num_freq = " << tmp_w_target->sm.freq_index.size() << std::endl;
        */
        std::vector<hark::Position> pos_mic = this->LocationsToCartesianPositions(tmp_w_target->loc_mic);
        std::vector<hark::Position> pos_src = this->LocationsToCartesianPositions(tmp_w_target->loc_src);
        this->harkio_w.SetW(filename,
                            tmp_w_target->sm.freq_index,
                            pos_mic,
                            pos_src,
                            tmp_w_target->sm.sep_mat);
        this->harkio_w.WriteData(filename);
    }
    /*
    {
        std::stringstream ss;
        size_t k = 1;
        for(size_t s = 0; s < num_src; s++){
            if(this->update_w.Update[k] != SRCID_PREVIOUS_STATE::INVALID){
                ss << "this->SepDB(f" << k << "s" << this->update_w.SMDB.loc_src[k].tfpos << "):";
                for(size_t m = 0; m < num_mic; m++){
                    ss << this->update_w.SMDB.sm.sep_mat[k * num_mic * num_src + m * num_src + s];
                }
                ss << "@GHDSS_exec::AddUpdateW\n";
            }
        }
        std::cout << ss.str();
    }
    */
}
//--------------------------------------------------------------------------
void GHDSS_exec::ExportAllUpdateW(const std::string &filename)
{
    size_t num_src;
    num_src = this->update_w.SMDB.loc_src.size();
    //std::cout << "num_src = " << num_src << std::endl;
    for(size_t i = 0; i < num_src; i++){
        if(this->update_w.Update[i] == SRCID_PREVIOUS_STATE::CREATE ||
           this->update_w.Update[i] == SRCID_PREVIOUS_STATE::EXISTS){
            this->update_w.Update[i] = SRCID_PREVIOUS_STATE::REMOVE;
        }
    }
    //
    /*
    {
        std::stringstream ss;
        ss << "UpdateW:\n";
        for(size_t i = 0; i < num_src; i++){
            if(this->update_w.Update[i] != SRCID_PREVIOUS_STATE::INVALID){
                ss << "Update[" << i << "]=" << this->update_w.Update[i]
                   << ", tfpos=" << this->update_w.SMDB.loc_src[i].tfpos << "\n";
            }
        }
        ss << "@GHDSS_exec::ExportAllUpdateW(1)\n";
        std::cout << ss.str();
    }
    */
    this->AddUpdateW(filename, true);
    /*
    {
        std::stringstream ss;
        ss << "UpdateW:\n";
        for(size_t i = 0; i < num_src; i++){
            if(this->update_w.Update[i] != SRCID_PREVIOUS_STATE::INVALID){
                ss << "Update[" << i << "]=" << this->update_w.Update[i]
                   << ", tfpos=" << this->update_w.SMDB.loc_src[i].tfpos << "\n";
            }
        }
        ss << "@GHDSS_exec::ExportAllUpdateW(2)\n";
        std::cout << ss.str();
    }
    */
}
//--------------------------------------------------------------------------
bool GHDSS_exec::SetTfData(const hark::TransferFunction &tfData)
{
    this->m_tfData = tfData;
    if(this->TFDB.tf.empty()){    //Initialフレーム処理（データベースファイルの読み込み)
        if(this->IsTfMatPartial() == ONLINE_TF_UPDATE_TYPE::PARTIAL) {
            std::cerr << "GHDSS: TransferFunction must be all, not partial at initial frame." << std::endl;
            return false;
        }
        this->TFDB = TFDatabase();
    }
    this->SetTransferFunction();
    return true;
}

GHDSS_exec::ONLINE_TF_UPDATE_TYPE GHDSS_exec::IsTfMatPartial()
{
    std::string tfwhat = this->m_tfData.GetWhatIsThis();
    if(tfwhat.compare("transfer function") == 0){
        return GHDSS_exec::ONLINE_TF_UPDATE_TYPE::ALL;
    }
    else if(tfwhat.compare("partial transfer function") == 0){
        return GHDSS_exec::ONLINE_TF_UPDATE_TYPE::PARTIAL;
    }
    return GHDSS_exec::ONLINE_TF_UPDATE_TYPE::INVALID;
}















































































void GHDSS_exec::MakeWMatGHDSS()
{
    size_t s, t, num_freq, num_src, num_mic;
    num_freq = this->SepMat.TFDB.freq_bin.size();
    num_src = this->SepMat.TFDB.loc_src.size();
    num_mic = this->SepMat.TFDB.loc_mic.size();

    if(this->SepMat.sm.sep_mat.size() != this->SepMat.TFDB.tf.size()){
        this->SepMat.sm.resize(num_freq, num_src, num_mic);
    }

    for(size_t i = 0; i < this->SepMat.TFDB.freq_bin.size(); i++){
        this->SepMat.sm.freq_index[i] = this->SepMat.TFDB.freq_bin[i];
    }

    // TF (freq, src, mic) -> Initial-SepMat (freq, mic, src)
    for(size_t k = 0; k < num_freq; k++){
        for(size_t j = 0; j < num_mic; j++){
            for(size_t i = 0; i < num_src; i++){
                s = k * num_mic * num_src + j * num_src + i;
                t = k * num_src * num_mic + i * num_mic + j;
                this->SepMat.sm.sep_mat[s] = this->SepMat.TFDB.tf[t] / static_cast<float>(num_mic);
            }
        }
    }
}

// ---------------------------------------------------------------------------------------------
void GHDSS_exec::MakeWMatGHDSS2(size_t i)
{
    size_t s, t, num_freq, num_src, num_mic;
    num_freq = this->SepMat.TFDB.freq_bin.size();
    num_src = this->SepMat.TFDB.loc_src.size();
    num_mic = this->SepMat.TFDB.loc_mic.size();

    // TF (freq, src, mic) -> Initial-SepMat (freq, mic, src)
    for(size_t k = 0; k < num_freq; k++){
        for(size_t j = 0; j < num_mic; j++){
            s = k * num_mic * num_src + j * num_src + i;
            t = k * num_src * num_mic + i * num_mic + j;
            this->SepMat.sm.sep_mat[s] = this->SepMat.TFDB.tf[t] / static_cast<float>(num_mic);
        }
    }
}












// ---------------------------------------------------------------------------------------------
// SSの計算
void GHDSS_exec::_dCalcSSStepSize_GHDSS()
{
    size_t num_src, num_mic;

    num_mic = this->g_Up_X.size();
    num_src = this->g_Up_Y.size();

    //std::cout << "Y = " << this->g_Up_Y <<std::endl;
    //PY, QY
    //_dGetPQY(this->g_SS_PY, this->g_SS_QY, this->g_Up_Y, this->ss_scale);
    {
        /*
        float absY, absYs, coshYs, tanhYs;
        std::complex<float> expY;

        for(size_t i = 0; i < this->g_Up_Y.size(); i++){
            absY = std::sqrt(this->g_Up_Y(i, 0).real() * this->g_Up_Y(i, 0).real()
                           + this->g_Up_Y(i, 0).imag() * this->g_Up_Y(i, 0).imag())
                           + std::numeric_limits<float>::epsilon();
            absYs = absY * this->ss_scale;

            tanhYs = std::tanh(absYs);

            expY = this->g_Up_Y(i, 0) / absY;

            this->g_SS_PY(i, 0) = tanhYs * expY;

            coshYs = std::cosh(absYs);
            coshYs *= coshYs;
            coshYs = absYs / coshYs * this->ss_scale;
            this->g_SS_QY(i, 0) = (tanhYs + coshYs) * expY;
        }
        */

        Eigen::ArrayXf absY, absYs, coshYs, tanhYs;
        Eigen::ArrayXcf expY;

        absY = this->g_Up_Y.cwiseAbs().real().array()
             + std::numeric_limits<float>::epsilon();
        absYs = absY * this->ss_scale;
        tanhYs = absYs.tanh();
        expY = this->g_Up_Y.array() / absY;
        this->g_SS_PY.noalias() = (tanhYs * expY).matrix();
        coshYs = absYs.cosh().square();
        // Recycle the absYs when it is no longer needed.
        absYs *= this->ss_scale;
        absYs /= coshYs;
        absYs += tanhYs;
        this->g_SS_QY.noalias() = (absYs * expY).matrix();
    }

    // removed : g_SS_Zns1, 
    ////////// Ryy(src*src), PY(src*1), Y'(1*src)
    // Ryy = y * y'
    this->g_SS_Ryy.noalias() = Eigen::MatrixXcf(this->g_SS_PY * this->g_Up_Y.adjoint()).cast<std::complex<double> >();
    //std::cout << "Ryy: " << this->g_SS_Ryy << std::endl;

    // removed : g_SS_Znsns, 
    ////////// E(src*src), Ryy(src*src)
    // E = Ryy - diag(Ryy)
    this->g_SS_E.noalias() = this->g_SS_Ryy - Eigen::MatrixXcd(this->g_SS_Ryy.diagonal().asDiagonal());
    //std::cout << "E: " << this->g_SS_E << std::endl;

    // removed : g_SS_Zns1
    ////////// Jss(src*mic), Zns1(src*1), E(src*src), QY(src*1), X'(1*mic), X(mic*1)
    // Jss = (E * QY) * X'
    // Zns1 = E * QY; Jss = Zns1 * X';
    //this->g_SS_Zns1.noalias() = this->g_SS_E * this->g_SS_QY;
    //this->g_SS_Jss.noalias() = this->g_SS_Zns1 * this->g_Up_X.adjoint();
    this->g_SS_Jss.noalias() = (this->g_SS_E * this->g_SS_QY.cast<std::complex<double> >())
                             * this->g_Up_X.adjoint().cast<std::complex<double> >();

    // Jpri = conj(Jss)
    this->g_SS_Jpri.noalias() = this->g_SS_Jss.conjugate();

    //std::cout << "Eval = " << this->g_SS_E << std::endl;

    double tmp, Yrms;
    if(this->ss_method == SS_METHOD::FIX){ // myu_J_SS = myu * Jss
        tmp = this->ss_myu;
    }
    else if(this->ss_method == SS_METHOD::LCMYU){ // myu_J_SS = LCmyu(SSmyu)*Rxx^-2 *Jss
        // removed : g_SS_Znm1, 
        ////////// Z11(1*1), X'(1*mic), X(mic*1)
        // Rxx = X' * X
        this->g_SS_Z11.noalias() = Eigen::MatrixXcf(this->g_Up_X.adjoint() * this->g_Up_X).cast<std::complex<double> >();
        tmp = this->g_SS_Z11(0, 0).real() * this->g_SS_Z11(0, 0).real();
        if(tmp < std::numeric_limits<float>::epsilon()){
            tmp = std::numeric_limits<float>::epsilon();
        }
        tmp = 1.0 / tmp;
        tmp *= this->ss_myu;
    }
    else if(this->ss_method == SS_METHOD::ADAPTIVE){ // myu_J_SS = tmp * Js
        // removed : g_SS_Znsns, 
        //////////
        // Znsns = abs(E); Znsns = Znsns.*Znsns; tmp = sum(Znsns); ==> tmp = sum(abs2(E));
        //this->g_SS_Znsns.noalias() = this->g_SS_E.cwiseAbs2();
        //tmp = this->g_SS_Znsns.real().sum();
        tmp = this->g_SS_E.cwiseAbs2().real().sum();
        // Znsnm = abs(Jss.*Jpri);
        this->g_SS_Znsnm.noalias() = this->g_SS_Jss.cwiseProduct(this->g_SS_Jpri).cwiseAbs();
        double tmp2 = this->g_SS_Znsnm.real().sum();
        if(std::abs(tmp2) < std::numeric_limits<float>::epsilon()){
            tmp2 += std::numeric_limits<float>::epsilon();
        }
        tmp /= tmp2;
        tmp *= 0.5f;
    }
    else{
        std::stringstream ss;
        ss << "Error: unknown SS_METHOD\n";
        throw hark::RuntimeError(ss.str());
    }

    {
        float lim_val = std::numeric_limits<float>::max() / tmp;
        float max_val = std::max(this->g_SS_Jss.real().cwiseAbs().maxCoeff(),
                                 this->g_SS_Jss.imag().cwiseAbs().maxCoeff());
        if(max_val > lim_val){
            std::stringstream ss;
            ss << "Error: J_SS will contain Inf.\n";
            throw hark::RuntimeError(ss.str());
        }
        this->g_Up_J_SS.noalias() = this->g_SS_Jss.cast<std::complex<float> >() * static_cast<float>(tmp);
    }
    //std::cout << "myuJss = " << this->g_Up_J_SS << std::endl;

    // removed Zns1
    ////////// Zns1(src*1), Y(src*1)
    // noise floor
    // Zns1 = abs(Y); Yrms = sqrt(sum(Zns1.*Zns1) / Zns1.size());
    // ==> Yrms = sqrt(sum(abs2(Y))/Y.size());

    // Yrms
    Yrms = this->g_Up_Y.cwiseAbs2().real().sum() / this->g_Up_Y.size();
    Yrms = std::sqrt(Yrms);

    if(Yrms < this->noise_floor){
        std::cout << "Noise floor reset" << std::endl;
        this->g_Up_J_SS.setZero();
    }
}
// LCの計算
// ---------------------------------------------------------------------------
void GHDSS_exec::_dCalcLCStepSize_GHDSS(size_t num_src, size_t num_mic)
{
    ////////// E(src*src), W(src*mic), Atmp=conj(conj_A)(mic*src) [A is conjugated.]
    // E = W * A
    //this->g_LC_Atmp.noalias() = A.conjugate();
    //this->g_LC_E.noalias() = this->g_Up_W * this->g_LC_Atmp;
    this->g_LC_E.noalias() = Eigen::MatrixXcf(this->g_Up_W * this->g_Up_A.conjugate()).cast<std::complex<double> >();

    // removed : g_LC_Znsns, 
    ////////// E(src*src), Znsns(src*src)
    // Znsns = eye(Nsrc);
    // E = E - Znsns
    //size_t i_max = std::min(this->g_LC_E.rows(), this->g_LC_E.cols());
    //for(size_t i = 0; i < i_max; i++){ this->g_LC_E(i, i) -= 1.0f; }
    //this->g_LC_Znsns = Eigen::MatrixXd::Identity(num_src, num_src);
    //this->g_LC_E -= this->g_LC_Znsns;
    this->g_LC_E -= Eigen::MatrixXd::Identity(num_src, num_src);

    if(this->lc_const == LC_CONST::DIAG){ // FULL uses g_LC_E as is.
        // E = diag(E)
        // !!CAUTION!! Do not use ".noalias()" when writing back to myself matrix
        // as it is a destructive operation. In such cases it return of ".eval()".
        this->g_LC_E = Eigen::MatrixXcd(this->g_LC_E.diagonal().asDiagonal());
    }

    // removed : g_LC_Znsnm, g_LC_Atmp
    ////////// Znsnm(src*mic), Atmp=conj(conj_A)(mic*src) [A is conjugated.]
    // Znsnm = A'
    // adjoint(conj(A)) ==> transpose(conj(conj(A)) ==> transose(A)
    //this->g_LC_Znsnm.noalias() = this->g_LC_Atmp.adjoint();
    //this->g_LC_Znsnm.noalias() = this->g_Up_A.transpose();

    ////////// Jlc(src*mic), E(src*src), Znsnm(src*mic)
    // Jlc = E * Znsnm
    //this->g_LC_Jlc.noalias() = this->g_LC_E * this->g_LC_Znsnm;
    this->g_LC_Jlc.noalias() = this->g_LC_E
                             * this->g_Up_A.transpose().cast<std::complex<double> >();

    //ステップサイズと更新量の決定
    float tmp;
    if(this->lc_method == LC_METHOD::FIX){
        tmp = this->lc_myu;
    }
    else if(this->lc_method == LC_METHOD::ADAPTIVE){
        double fJlcp, foptLCMyu;
        // fJlcp = sum(abs(Jlc)^2)
        fJlcp = this->g_LC_Jlc.cwiseAbs2().real().sum();

        // foptLCMyu = sum(abs(E)^2)
        foptLCMyu = this->g_LC_E.cwiseAbs2().real().sum();

        // zero divide
        if(fJlcp < std::numeric_limits<double>::epsilon()){
            fJlcp += std::numeric_limits<double>::epsilon();
        }
        foptLCMyu /= fJlcp;
        foptLCMyu *= 0.5f;

        tmp = foptLCMyu;
    }
    else{
        std::stringstream ss;
        ss << "Error: unknown LC_METHOD\n";
        throw hark::RuntimeError(ss.str());
    }

    {
        float lim_val = std::numeric_limits<float>::max() / tmp;
        float max_val = std::max(this->g_LC_Jlc.real().cwiseAbs().maxCoeff(),
                                 this->g_LC_Jlc.imag().cwiseAbs().maxCoeff());
        if(max_val > lim_val){
            std::stringstream ss;
            ss << "Error: J_LC will contain Inf.\n";
            throw hark::RuntimeError(ss.str());
        }
        this->g_Up_J_LC.noalias() = this->g_LC_Jlc.cast<std::complex<float> >() * static_cast<float>(tmp);
    }
    //std::cout << "LCmyu = " << this->g_Up_J_LC << std::endl;
}
// -------------------------------------------------------------------------------------------------
// GHDSSの実行
void GHDSS_exec::dUpdateSeparationMatGHDSS(
                                       hark::FreqBins<std::complex<float> > &InputSpec,
                                       hark::FreqBins<std::complex<float> > &OutputSpec)
{
    //std::cout << "start GHDSS" << std::endl;
    size_t num_src, num_freq, num_mic;

    // Currently, the setting value (this->threshold) is fixed, so threshold processing is
    // effectively disabled. In the future, we have improved the processing so that it will
    // be as lightweight as possible when changing the setting value.
    if(this->prev_threshold != this->threshold){
        constexpr double int16max = static_cast<double>(std::numeric_limits<int16_t>::max());
        constexpr double dbmax = 10.0 * std::log10(int16max);
        double dtmp = (dbmax - static_cast<double>(this->threshold)) / 10.0;
        dtmp = std::pow(10.0, dtmp);
        if(dtmp < static_cast<double>(std::numeric_limits<float>::min())){
            dtmp = static_cast<double>(std::numeric_limits<float>::min());
        }
        this->x_threshold = static_cast<float>(dtmp);
        this->prev_threshold = this->threshold;
    }

    num_mic = InputSpec.num_ch; //same as// num_mic = InputSpec.bins.rows();

    //num_freq = InputSpec.upper_freq - InputSpec.lower_freq + 1;
    //num_freq = InputSpec.bins.size() / num_mic;
    num_freq = InputSpec.bins.cols();

    num_src = OutputSpec.num_ch; //same as// num_src = OutputSpec.bins.rows();

    //周波数帯域は一致していなければならない
    if(this->SepMat.sm.freq_index.size() != num_freq
     || this->SepMat.TFDB.freq_bin.size() != num_freq){
        std::stringstream ss;
        ss << "InputSpec size " << num_freq
           << " and Separation matrix " << this->SepMat.sm.freq_index.size()
           << " (TF " << this->SepMat.TFDB.freq_bin.size()
           << " ) size is mismatch.\n";
        throw hark::RuntimeError(ss.str());
    }

    if(this->g_SepMat_Nsrc != num_src){
        // SS
        this->g_SS_Ryy.resize(num_src, num_src);
        //this->g_SS_Zns1.resize(num_src, 1);
        this->g_SS_E.resize(num_src, num_src);
        this->g_SS_Jss.resize(num_src, num_mic);
        this->g_SS_Jpri.resize(num_src, num_mic);
        //this->g_SS_Znsns.resize(num_src, num_src);
        this->g_SS_Znsnm.resize(num_src, num_mic);
        this->g_SS_PY.resize(num_src, 1);
        this->g_SS_QY.resize(num_src, 1);
        this->g_SS_Z11.resize(1, 1);

        // LC
        this->g_LC_E.resize(num_src, num_src);
        //this->g_LC_Znsns.resize(num_src, num_src);
        //this->g_LC_Znsnm.resize(num_src, num_mic);
        this->g_LC_Jlc.resize(num_src, num_mic);
        //this->g_LC_Atmp.resize(num_src, num_mic);

        // Update
        this->g_Up_A.resize(num_mic, num_src);
        this->g_Up_W.resize(num_src, num_mic);
        this->g_Up_Y.resize(num_src, 1);
        this->g_Up_Ryy.resize(num_src, num_src);
        this->g_Up_J_SS.resize(num_src, num_mic);
        this->g_Up_J_LC.resize(num_src, num_mic);

        this->g_SepMat_Nsrc = num_src;
    }
    if(this->g_SepMat_Nmic != num_mic){
        // SS
        //this->g_SS_Znm1.resize(num_mic, 1);

        // Update
        this->g_Up_X.resize(num_mic, 1);
        this->g_Up_Rxx.resize(num_mic, num_mic);

        this->g_SepMat_Nmic = num_mic;
    }

    for(size_t ifreq = InputSpec.lower_freq; ifreq <= InputSpec.upper_freq; ifreq++){
        if(ifreq == 0){ continue; } // D.C.
        //std::cout << "freq=" << ifreq << std::endl;

        if(ifreq != this->SepMat.TFDB.freq_bin[ifreq]
         || ifreq != this->SepMat.sm.freq_index[ifreq]){
            //std::cout << "ifreq = " << ifreq
            //          << ",TF = " << this->SepMat.TFDB.freq_bin[ifreq]
            //          << ", W = " << this->SepMat.sm.freq_index[ifreq]
            //          << " -> continue\n";
            continue;
        }

        //--- Copy :: Spec array -> Complex array ---//
        // Transfer function matrix A (Notes: TFDB.tf and g_Up_A are conjugated.)
        // SepMat.TFDB.tf [num_freq * num_mic * num_src]
        // =extract ifreq=> g_Up_A [num_mic, num_src]
        //for(size_t i = 0; i < num_mic; i++){
        //    for(size_t j = 0; j < num_src; j++){
        //        this->g_Up_A(i, j) = this->SepMat.TFDB.tf[ifreq * num_mic * num_src + i * num_src + j];
        //    }
        //}
        std::complex<float> *a = this->SepMat.TFDB.tf.data() + ifreq*num_mic*num_src;
        this->g_Up_A.noalias() = Eigen::Map<Eigen::MatrixXcf>(a, num_mic, num_src);

        // Input spectrum X
        // InputSpec.bins [num_mic, num_freq]
        // =extract ifreq=> g_Up_X [num_mic, 1]
        // (注意!!) spectrum data:frame->ch->freqの順に並んでいる
        //for(size_t i = 0; i < num_mic; i++){
        //    this->g_Up_X(i, 0) = InputSpec.bins(i, ifreq);
        //}
        this->g_Up_X.noalias() = InputSpec.bins.col(ifreq);

        // Separation matrix W
        // this->SepMat.sm.sep_mat [num_freq * num_src * num_mic]
        // =extract ifreq=> g_Up_W [num_src, num_mic]
        //for(size_t i = 0; i < num_src; i++){
        //    for(size_t j = 0; j < num_mic; j++){
        //        this->g_Up_W(i, j) = this->SepMat.sm.sep_mat[ifreq * num_src * num_mic + i * num_mic + j];
        //    }
        //}
        std::complex<float> *w = this->SepMat.sm.sep_mat.data() + ifreq*num_src*num_mic;
        this->g_Up_W.noalias() = Eigen::Map<Eigen::MatrixXcf>(w, num_src, num_mic);
        
        // Correlation matrix Rxx
        // Rxx [num_mic * num_mic, num_freq]
        // =extract ifreq=> g_Up_Rxx [num_mic, num_mic]
        this->g_Up_Rxx.resize(num_mic, num_mic);
        for(size_t i = 0; i < num_mic; i++){
            for(size_t j = 0; j < num_mic; j++){
                ////DEBUG////std::cout << "this->Rxx.bin(" << this->Rxx.bins.rows() << ", " << this->Rxx.bins.cols() << ")\ni=" << i << ", j=" << j << ", num_mic=" << num_mic << "\n";
                std::complex<double> tmp_val = this->Rxx.bins(i * num_mic + j, ifreq);
                this->g_Up_Rxx(i, j) = tmp_val;
            }
        }
        // Eigen 3.4 or later (Ubuntu 22.04 only)
        //this->g_Up_Rxx.noalias() = this->Rxx.bins.col(ifreq).reshaped<AutoOrder>(num_mic, num_mic);
        //NG//this->g_Up_Rxx.noalias() = Eigen::Map<Eigen::MatrixXcd>(Eigen::VectorXcd(this->Rxx.bins.col(ifreq)).data(), static_cast<int>(num_mic), static_cast<int>(num_mic));

        // Correlation matrix Ryy
        // Ryy [num_src * num_src, num_freq]
        // =extract ifreq=> g_Up_Ryy [num_src, num_src]
        this->g_Up_Ryy.resize(num_src, num_src);
        for(size_t i = 0; i < num_src; i++){
            for(size_t j = 0; j < num_src; j++){
                std::complex<double> tmp_val = this->Ryy.bins(i * num_src + j, ifreq);
                this->g_Up_Ryy(i, j) = tmp_val;
            }
        }
        // Eigen 3.4 or later (Ubuntu 22.04 only)
        //this->g_Up_Ryy.noalias() = this->Ryy.bins.col(ifreq).reshped<AutoOrder>(num_src, num_src);
        //NG//this->g_Up_Ryy.noalias() = Eigen::Map<Eigen::MatrixXcd>(Eigen::VectorXcd(this->Ryy.bins.col(ifreq)).data(), static_cast<int>(num_src), static_cast<int>(num_src));

        // ------------------------------------------//
        // ---------- Execute GHDSS --------------- //
        ////////// g_Up_Y(src*1), g_Up_W(src*mic), g_Up_X(mic*1)
        this->g_Up_Y.noalias() = this->g_Up_W * this->g_Up_X;
        //std::cout << "Y_" << ifreq << " = \n" << this->g_Up_Y << std::endl;

        // --- Copy : Y -> Separated spectrum(src->freqの順) --- //
        //for(size_t i = 0; i < num_src; i++){
        //    OutputSpec.bins(i, ifreq) = this->g_Up_Y(i, 0);
        //}
        OutputSpec.bins.col(ifreq) = this->g_Up_Y.col(0);
        // --------------------------------------- //

        // ------ Update separated matrix --------- //
        // Threshold
        //float tmp = this->g_Up_X.cwiseAbs().maxCoeff();
        //bool need_updating = (tmp < this->x_threshold) ? false : true;

        // more faster?
        float tmp = this->g_Up_X.cwiseAbs2().maxCoeff();
        tmp = std::sqrt(tmp);
        bool need_updating = (tmp < this->x_threshold) ? false : true;

        // Currently, the setting value (this->threshold) is fixed,
        // so threshold processing is effectively disabled.
        // In the future, we have improved the processing so that
        // it will be as lightweight as possible when changing the setting value.
        // 'need_updating' will always be 'true'.
        if(need_updating){
            //std::cout << "SS" << std::endl;
            if(this->ss_method == SS_METHOD::LCMYU){ this->ss_myu = this->lc_myu; }

            this->_dCalcSSStepSize_GHDSS();

            if(this->update == UPDATE_TYPE::STEP){
                ////////// g_Up_W(src*mic), g_Up_J_SS(src*mic)
                this->g_Up_W -= this->g_Up_J_SS;
            }

            //std::cout << "LC" << std::endl;
            this->_dCalcLCStepSize_GHDSS(num_src, num_mic);

            if(this->update == UPDATE_TYPE::STEP){
                ////////// g_Up_W(src*mic), g_Up_J_LC(src*mic)
                this->g_Up_W -= this->g_Up_J_LC;
                //this->g_Up_W *= (1.0 - this->w_myu);
            }
            else if(this->update == UPDATE_TYPE::TOTAL){
                //this->g_Up_W *= (1.0 - this->w_myu);
                ////////// g_Up_W(src*mic), g_Up_J_SS(src*mic), g_Up_J_LC(src*mic)
                this->g_Up_W -= (this->g_Up_J_SS + this->g_Up_J_LC);
            }
        }
        
        // --------------------------------------- //
        // if(ifreq == 1){ std::cout << "W1 = " << W << std::endl; }

        //--- Update Spec array ---//
        // Separation matrix W[num_src * num_mic] std::complex<float>
        for(size_t i = 0; i < num_src; i++){
            for(size_t j = 0; j < num_mic; j++){
                this->SepMat.sm.sep_mat[ifreq * num_src * num_mic + i * num_mic + j] =
                    this->g_Up_W(i, j);
            }
        }
        
        // Correlation matrix Rxx [num_mic * num_mic] std::complex<double>
        for(size_t i = 0; i < num_mic; i++){
            for(size_t j = 0; j < num_mic; j++){
                this->Rxx.bins(i * num_mic + j, ifreq) = this->g_Up_Rxx(i, j);
            }
        }
        // Eigen 3.4 or later (Ubuntu 22.04 only)
        //this->Rxx.bins.col(ifreq) = this->g_Up_Rxx.reshaped<AutoOrder>(num_mic * num_mic, 1);
        //NG//this->Rxx.bins.col(ifreq) = Eigen::Map<Eigen::MatrixXcd>(Eigen::VectorXcd(this->g_Up_Rxx.array()).data(), num_mic * num_mic, 1);

        // Correlation matrix Ryy [num_src * num_src] std::complex<double>
        for(size_t i = 0; i < num_src; i++){
            for(size_t j = 0; j < num_src; j++){
                this->Ryy.bins(i * num_src + j, ifreq) = this->g_Up_Ryy(i, j);
            }
        }
        // Eigen 3.4 or later (Ubuntu 22.04 only)
        //this->Ryy.bins.col(ifreq) = this->g_Up_Ryy.reshaped<AutoOrder>(num_src * num_src, 1);
        //NG//this->Ryy.bins.col(ifreq) = Eigen::Map<Eigen::MatrixXcd>(Eigen::VectorXcd(this->g_Up_Ryy.array()).data(), num_src * num_src, 1);
        // ------------------------------------------//
    }
}












// ---------------------------------------------------------------------
// GHDSSの実行（分離行列の更新) 
void GHDSS_exec::ExecuteGHDSS(hark::FreqBins<std::complex<float> > &InputSpec,
                              hark::FreqBins<std::complex<float> > &OutputSpec)
{
    //constexpr bool debug = false;
    size_t lower, upper;
    lower = this->SepMat.TFDB.freq_bin[0];
    upper = this->SepMat.TFDB.freq_bin.size() + lower - 1;

    /*
    if(debug){
        std::stringstream ss;
        if(InputSpec.lower_freq != lower){ ss << "Lower freq not matched!\n"; }
        if(InputSpec.upper_freq != upper){ ss << "Upper freq not matched!\n"; }
        if(!ss.str().empty()){
            ss << "GHDSS: Fail to execute GHDSS.\n"
               << "Exception caught in .ExecuteGHDSS() "
               << __FILE__ << ", " << __FILE__ << "\n";
            throw hark::RuntimeError(ss.str());
        }
    }
    */

    if(InputSpec.num_ch != this->SepMat.sm.num_inputs){
        std::stringstream ss;
        ss << "InputSpec ChNum and TFdata ChNum not matched!\n"
           << "InputSpec ChNum = " << InputSpec.num_ch << "\n"
           << "TFdata ChNum = " << this->SepMat.sm.num_inputs << "\n";
        throw hark::RuntimeError(ss.str());
    }

    if(OutputSpec.lower_freq != lower || OutputSpec.upper_freq != upper ||
       OutputSpec.num_ch != this->SepMat.sm.num_outputs)
    {
        OutputSpec.resize(this->SepMat.sm.num_outputs, lower, upper);
    }

    /*
    if(debug && OutputSpec.num_ch == 2){
        std::stringstream ss;
        ss << "this->SepMat(size:" << this->SepMat.sm.sep_mat.size() << "):";
        for(size_t i = 0; i < this->SepMat.sm.sep_mat.size(); i++){
            ss << this->SepMat.sm.sep_mat[i] << ",");
        }
        std::cout << ss.str() << std::endl;
    }
    */

    // GHDSSの実行と分離行列の更新
    this->dUpdateSeparationMatGHDSS(InputSpec, OutputSpec);
}

void GHDSS_exec::Initialize_GHDSS(int num_ch, int num_pos, int lower, int upper)
{
    this->Rxx.resize(num_ch * num_ch, lower, upper);
    //for(size_t i = 0; i < this->Rxx.bins.size(); i++){
    //    this->Rxx.bins[i] = std::complex<double>(1.0, 0.0);
    //}
    this->Rxx.bins.setOnes();
    ////DEBUG////std::cout << "this->Rxx.bins(" << this->Rxx.bins.rows() << ", " << this->Rxx.bins.cols() << ")\n" << std::endl;

    this->Ryy.resize(num_pos * num_pos, lower, upper);
    //for(size_t i = 0; i < this->Ryy.bins.size(); i++){
    //    this->Ryy[i] = std::complex<double>(1.0, 0.0);
    //}
    this->Rxx.bins.setOnes();
    ////DEBUG////std::cout << "this->Ryy.bins(" << this->Ryy.bins.rows() << ", " << this->Ryy.bins.cols() << ")\n" << std::endl;

    this->initialized = true;
}

}
}

