/*== source from adaptBF.m

  % [1] O. L. FrostIII, 'An Algorithm for Lineary Constrained Adaptive
  % array processing', Proc. of the IEEE, Vol. 60, No.8, 1972
  % The difference between this program and the paper are only two points: 
  % 1: This uses complex. Transpose is processed with complex conjugation.
  % 2: This uses row vectors for BF coefficient. (Paper uses column vectors)
      if isempty(Adata.F) % Initialization process
        Adata.F = zeros(NSrc, NMic, NFreq); % LCMV parameter 1 (F)
        Adata.P = zeros(NMic, NMic, NSrc, NFreq); % LCMV parameter 2 (P)
        for nf = 1:NFreq
          for ns = 1:NSrc % for each source
            Fi = 1; % Constrained target value
            C = Adata.H(:, ns, nf); % Constraned base vector
            Adata.F(ns, :, nf) = Fi*inv(C'*C)*C'; % = Fi*pinv(C) = WDS-BF
            Adata.P(:, :, ns, nf) = eye(NMic) - C*inv(C'*C)*C';
          end
        end
        if isempty(G), G = Adata.F; end
        return
        
      else % update process
        for nf = 1:NFreq
          for ns = 1:NSrc % for each source
              g = G(ns, :, nf);
              F = Adata.F(ns, :, nf);
              P = Adata.P(:, :, ns, nf);
              myu = Adata.myu;
              y = Adata.y(ns, nf);
              x = Adata.x(:, nf);
              gn = (g - myu*y*x') * P + F; % Update Eq. (22) in [1]
              G(ns, :, nf) = gn;
          end
        end
        return
      end % For if initialization
      
*/

//numFreq = num_freq;
//muAdp   = lc_myu_adaptive;
//mu      = lc_myu;

void LCMVupdateSources(int num_src, BF_SourceLocation currentSourcePositions[ARY_NUM_SRC])
{
    int s, ps, m, pm, f;
    // 直近フレームでの音源と、このフレームでの音源との対応をとる
    int lastSourceIndices[ARY_NUM_SRC]; // i 番めの要素は、このフレームでの i 番目の音源（配列の i 番目の要素という意味）が、直近フレームでの何番目の音源（同）に相当するかを表す。
    for(s=0; s<num_src; s++){ // 直近フレームの音源から、同じ ID を持つものを探す
        lastSourceIndices[s] = -1; // 見つからなかった場合は-1とする
        for(ps=0; ps<num_pre_src; ps++){
            if(currentSourcePositions[s].source_id == sourcePositions[ps].source_id){
                lastSourceIndices[s] = ps; // 見つかった場合は直近フレームで見つけた音源の配列要素番号を代入する
                break;
            }
        }
    }
    //直近フレームの音源情報バックアップ
    num_pre_src = num_src;
    for(s=0; s<num_src; s++){
        sourcePositions[s].x[0]      = currentSourcePositions[s].x[0];
        sourcePositions[s].x[1]      = currentSourcePositions[s].x[1];
        sourcePositions[s].x[2]      = currentSourcePositions[s].x[2];
        sourcePositions[s].source_id = currentSourcePositions[s].source_id;
        sourcePositions[s].is_signal = currentSourcePositions[s].is_signal;
    }

    // 伝達関数を再計算する
    calculateTransformFunctions(num_src);

    // 分離行列の必要な要素を再初期化する
    HA_Complex Fbk[ARY_NUM_SRC][ARY_NUM_MIC];
    HA_Complex Gbk[ARY_NUM_SRC][ARY_NUM_MIC];
    HA_Complex Pbk[ARY_NUM_SRC][ARY_NUM_MIC][ARY_NUM_MIC];

    // num_mic_pos :: 伝達関数のマイク数を用いる
    for(f=0; f<num_freq; f++){
        for(s=0; s<num_src; s++){ //直近フレームに同一IDがあるケースのみ結果をバックアップ
            const int lastSourceIndex = lastSourceIndices[s];
            if(lastSourceIndex >= 0){
                for(m=0; m<num_mic_pos; m++){
                    Fbk[s][m].re = F[f][lastSourceIndex][m].re;
                    Fbk[s][m].im = F[f][lastSourceIndex][m].im;
                    Gbk[s][m].re = separationMatrices[f][lastSourceIndex][m].re;
                    Gbk[s][m].im = separationMatrices[f][lastSourceIndex][m].im;
                    for(pm=0; pm<num_mic_pos; pm++){
                        Pbk[s][m][pm].re = P[f][lastSourceIndex][m][pm].re;
                        Pbk[s][m][pm].im = P[f][lastSourceIndex][m][pm].im;
                    }
                }
            }
        }
        for(s=0; s<num_src; s++){
            // 入力の j 番目の音源と同じIDの音源が、直近フレームに見当たらない？
            if(lastSourceIndices[s] >= 0){
                for(m=0; m<num_mic_pos; m++){
                    F[f][s][m].re = Fbk[s][m].re;
                    F[f][s][m].im = Fbk[s][m].im;
                    separationMatrices[f][s][m].re = Gbk[s][m].re;
                    separationMatrices[f][s][m].im = Gbk[s][m].im;
                    for(pm=0; pm<num_mic_pos; pm++){
                        P[f][s][m][pm].re = Pbk[s][m][pm].re;
                        P[f][s][m][pm].im = Pbk[s][m][pm].im;
                    }
                }
            }
            else{
                //C[ARY_NUM_MIC]=transformFunctions[f][s][ARY_NUM_MIC];
                //CNorm2 = CT*C : CT == H[s], H == Hs[f], H == transformFunctions
                HA_Complex CNorm2; CNorm2.re = 0.0f; CNorm2.im = 0.0f;
                for(m=0; m<num_mic_pos; m++){
                    CNorm2.re += transformFunctions[f][s][m].re * transformFunctions[f][s][m].re - transformFunctions[f][s][m].im * transformFunctions[f][s][m].im;
                    CNorm2.im += transformFunctions[f][s][m].re * transformFunctions[f][s][m].im * 2.0f;
                }
                if(sqrtf(CNorm2.re * CNorm2.re + CNorm2.im * CNorm2.im) < 1.0e-6){ // if(abs(CNorm2) < 1.0e-6)
                    CNorm2.re = 1.0e-6;
                    CNorm2.im = 0.0f;
                }
                float tmp = 1.0f / (CNorm2.re * CNorm2.re + CNorm2.im * CNorm2.im);
                HA_Complex CNorm2i; // CNorm2i = 1.0f / CNorm2;
                CNorm2i.re =  CNorm2.re * tmp;
                CNorm2i.im = -CNorm2.im * tmp;
                //LOGD("transformFunctions[%d][%d][m] : ", f, s); for(m=0; m<num_mic; m++){ LOGD("(%.6f, %.6f) ", transformFunctions[f][s][m].re, transformFunctions[f][s][m].im); }
                //LOGD("CNorm2i : %f", CNorm2i);
                
                //F = CNorm2i * CT; G = CNorm2i * CT;
                for(m=0; m<num_mic_pos; m++){
                    F[f][s][m].re = CNorm2i.re * transformFunctions[f][s][m].re - CNorm2i.im * transformFunctions[f][s][m].im;
                    F[f][s][m].im = CNorm2i.re * transformFunctions[f][s][m].im + CNorm2i.im * transformFunctions[f][s][m].re;
                    separationMatrices[f][s][m].re = F[f][s][m].re;
                    separationMatrices[f][s][m].im = F[f][s][m].im;
                }
                //LOGD("separationMatrices[%d][%d][m] : ", f, s); for(m=0; m<num_mic; m++){ LOGD("(%.6f, %.6f) ", separationMatrices[f][s][m].re, separationMatrices[f][s][m].im); }
                
                //P = eye(NMic) - C * CNorm2i * CT;
                HA_Complex mul_tmp;
                for(pm=0; pm<num_mic_pos; pm++){
                    //mul_tmp.re = transformFunctions[f][s][pm].re * CNorm2i.re - transformFunctions[f][s][pm].im * CNorm2i.im;
                    //mul_tmp.im = transformFunctions[f][s][pm].re * CNorm2i.im + transformFunctions[f][s][pm].im * CNorm2i.re;
                    mul_tmp.re =  transformFunctions[f][s][pm].im * CNorm2i.im - transformFunctions[f][s][pm].re * CNorm2i.re;
                    mul_tmp.im = -transformFunctions[f][s][pm].re * CNorm2i.im - transformFunctions[f][s][pm].im * CNorm2i.re;
                    for(m=0; m<num_mic_pos; m++){
                        //P[f][s][pm][m].re = -(mul_tmp.re * transformFunctions[f][s][m].re - mul_tmp.im * transformFunctions[f][s][m].im);
                        //P[f][s][pm][m].im = -(mul_tmp.re * transformFunctions[f][s][m].im + mul_tmp.im * transformFunctions[f][s][m].re);
                        P[f][s][pm][m].re = mul_tmp.re * transformFunctions[f][s][m].re - mul_tmp.im * transformFunctions[f][s][m].im;
                        P[f][s][pm][m].im = mul_tmp.re * transformFunctions[f][s][m].im + mul_tmp.im * transformFunctions[f][s][m].re;
                    }
                    P[f][s][pm][pm].re += 1.0f;
                }
                //LOGD("P[%d][%d][pm][m] : ", f, s); for(pm=0; pm<num_mic; pm++){ for(m=0; m<num_mic; m++){ LOGD("(%.6f, %.6f) ", P[f][s][pm][m].re, P[f][s][pm][m].im); } }
            }
        }
        //if(f == 7){ F.dump(stdout, "F[7]"); }
        //LOGD("separationMatrices<G>[%d][s][m] : ", f); for(s=0; s<num_src; s++){ for(m=0; m<num_mic; m++){ LOGD("(%.6f, %.6f) ", separationMatrices[f][s][m].re, separationMatrices[f][s][m].im); } }
    }
    //LOGD("transformFunction<H>[f][s][m] : "); for(f=0; f<num_freq; f++){ for(s=0; s<num_src; s++){ for(m=0; m<num_mic; m++){ LOGD("(%.6f, %.6f) ", transformFunctions[f][s][m].re, transformFunctions[f][s][m].im); } } }
    //LOGD("separationMatrices<G>[f][s][m] : "); for(f=0; f<num_freq; f++){ for(s=0; s<num_src; s++){ for(m=0; m<num_mic; m++){ LOGD("(%.6f, %.6f) ", separationMatrices[f][s][m].re, separationMatrices[f][s][m].im); } } }
}

void LCMVadaptSeparationMatrices(num_src)
{
    //参考
    //HA_Complex transformFunctions[ARY_NUM_FFT][ARY_NUM_SRC][ARY_NUM_MIC]    ////Hs
    //HA_Complex separationMatrices[ARY_NUM_FFT][ARY_NUM_SRC][ARY_NUM_MIC]    ////Gs
    //HA_Complex P[ARY_NUM_FFT][ARY_NUM_SRC][ARY_NUM_MIC][ARY_NUM_MIC]
    //HA_Complex F[ARY_NUM_FFT][ARY_NUM_SRC][ARY_NUM_MIC]
    HA_Complex inputSpectrum[ARY_NUM_MIC][ARY_NUM_FFT];
    HA_Complex outputSpectrum[ARY_NUM_SRC][ARY_NUM_FFT];
    HA_Complex gn[ARY_NUM_MIC];
    int f, s, m, pm;
    float n, lmu;
    lmu = lc_myu;
    for(f=0; f<num_freq; f++){
        if(lc_myu_adaptive){ //if enabled adaptive myu, update lmu variable.
            n = 0.0f;
            for(m=0; m<num_mic_pos; m++){
                n += inputSpectrum[m][f].re * inputSpectrum[m][f].re + inputSpectrum[m][f].im * inputSpectrum[m][f].im;
            }
            lmu = lc_myu / (2.0f * n + (n < BF_EPS ? BF_EPS : 0.0f));
        }
        for(s=0; s<num_src; s++){
#if DEBUG0
            float Jc = (outputSpectrum[s][f].re * outputSpectrum[s][f].re + outputSpectrum[s][f].im * outputSpectrum[s][f].im) * 0.5f;
            if(j == 0){ LOGD("lmu=%.6f Jc=%.6f ", lmu, Jc); }
#endif //DEBUG0
            for(m=0; m<num_mic_pos; m++){
                gn[m].re = separationMatrices[f][s][m].re - lmu * (outputSpectrum[s][f].re * inputSpectrum[m][f].re - outputSpectrum[s][f].im * inputSpectrum[m][f].im);
                gn[m].im = separationMatrices[f][s][m].im - lmu * (outputSpectrum[s][f].re * inputSpectrum[m][f].im + outputSpectrum[s][f].im * inputSpectrum[m][f].re);
            }
            for(m=0; m<num_mic_pos; m++){
                separationMatrices[f][s][m].re = 0.0f;
                separationMatrices[f][s][m].im = 0.0f;
                for(pm=0; pm<num_mic_pos; pm++){
                    separationMatrices[f][s][m].re += gn[pm].re * P[f][s][pm][m].re - gn[pm].im * P[f][s][pm][m].im;
                    separationMatrices[f][s][m].im += gn[pm].re * P[f][s][pm][m].im + gn[pm].im * P[f][s][pm][m].re;
                }
                separationMatrices[f][s][m].re += F[f][s][m].re;
                separationMatrices[f][s][m].im += F[f][s][m].im;
            }
        }
    }
}

