
/************************************************************
**  SemiBlindICA.c
**
**  Object name :
**    SemiBlindICA
**
**  History :
**     2013.05.14 M.Takigahira Build有無指定に対応
**     2013.01.10 K.Takagi サンプルネットワークとデータを比較し結果を確認
**     2012.12.18 K.Takagi 既知の値にて結果を確認
**     2012.12.17 K.Takagi 処理部分を実装、ビルドの確認
**     2012.12.16 K.Takagi 新規作成
**
**  Description
**  -----------
**  セミブラインドICA
**
**  Copyright (c) 2012 - 2013
**  System In Frontier,Inc.
**  All rights reserved.
**
**  当社の許可なしに複製および転載を禁ず
**
*/

///////////////////////////////////////////////////////////////
//  Include Files
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "build_modules.h"
#include "HA_log.h"
#include "android_hark.h"

#include "MatrixMemoryAllocate.h"
#include "SemiBlindICA.h"

///////////////////////////////////////////////////////////////
//  Define Macros
#define FLOOR_VALUE (1.0E-5)

#if !defined BUILD_MODULE_SEMI_BLIND_ICA
///////////////////////////////////////////////////////////////
//  Dummy Functions for build empty methods.
int HASetParamSemiBlindICA(Param_SemiBlindICA_t *param){ return 0; }
int HAInitSemiBlindICA(void){ return 0; }
int HAReleaseSemiBlindICA(void){ return 0; }
int SemiBlindICA(int frame_count, COMPLEX_FLOAT **out_data, COMPLEX_FLOAT **in_data){ return 0; }
#ifdef STANDALON_TEST
int main(int argc, char **argv){ return 0; }
#endif // STANDALON_TEST
#else // #if defined BUILD_MODULE_SEMI_BLIND_ICA
///////////////////////////////////////////////////////////////
//  Parameter

/*=========================================================*/
/*                 Structure Declaration                   */
/*=========================================================*/
typedef struct _SBICA_INFO_t_
{
    int taps;
    int idx;
    int itvl;
    unsigned long mask;
    float n_var;
    float r_var;
}SBICA_INFO_t;

/*=========================================================*/
/*                    Private Valiable                     */
/*=========================================================*/
static Param_SemiBlindICA_t semi_blind_ica_info;
static SBICA_INFO_t **sbica_info;

//static COMPLEX_FLOAT ***mu_f;
static float ***mu_f;
static COMPLEX_FLOAT ***buf_s;
static COMPLEX_FLOAT ***buf_ns;
static COMPLEX_FLOAT ***w;

static float mu_s;
static float mu_n;

///////////////////////////////////////////////////////////////
//  Local Buffer

///////////////////////////////////////////////////////////////
//  Clear
/************************************************************
**  History :
**    2012.12.16 K.Takagi 新規作成
*/
// Public : バッファクリアなどの解放処理
int HAReleaseSemiBlindICA(void)
{
    if(sbica_info != NULL){
        free(sbica_info[0]);
        free(sbica_info);
    }
    sbica_info = NULL;

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

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

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

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

///////////////////////////////////////////////////////////////
//  Initialize
/************************************************************
**  History :
**    2012.12.16 K.Takagi 新規作成
*/
// Public : 初期化
int HAInitSemiBlindICA(void)
{
    int i,j;
    int t;

    int channel_num;
    int fft_length;
    
    int taps;
    int tap_max;
    int tap_lower;
    int tap_upper;
    
    //float mu_s;
    float mu_v;
    //float mu_n;
    
    float lambda;
    
    int itvl;
    int size;
    int size_max;
    
    channel_num = semi_blind_ica_info.channel;
    if((channel_num <= 1) || (channel_num > 16)){
        return -1;
    }
    
    fft_length = semi_blind_ica_info.length;
    if(fft_length <= 0){
        return -1;
    }
    fft_length = semi_blind_ica_info.length / 2 + 1;

    itvl = semi_blind_ica_info.interval;

    mu_s = semi_blind_ica_info.mu_s / 10000.0;
    mu_v = semi_blind_ica_info.mu_v / 10000.0;
    mu_n = semi_blind_ica_info.mu_n / 10000.0;

    lambda = semi_blind_ica_info.lambda;

    tap_max = semi_blind_ica_info.tap_lower;
    tap_lower = semi_blind_ica_info.tap_lower;
    tap_upper = semi_blind_ica_info.tap_upper;

    /* バッファ確保 */
    sbica_info = (SBICA_INFO_t **)MemoryAllocate2D(channel_num, fft_length, sizeof(SBICA_INFO_t));
    if(sbica_info == NULL){
        return -1;
    }
    
    mu_f = (float ***)MemoryAllocate3D(channel_num, fft_length, tap_max, sizeof(float));
    if(mu_f == NULL){
        return -1;
    }
    
    w = (COMPLEX_FLOAT ***)MemoryAllocate3D(channel_num, fft_length, tap_max, sizeof(COMPLEX_FLOAT));
    if(w == NULL){
        return -1;
    }
    
    for(j=0; j<channel_num; j++){
        for(i=0; i<fft_length; i++){
            for(t=0; t<tap_max; t++){
                mu_f[j][i][t] = 0; 
                
                w[j][i][t].real = 0; 
                w[j][i][t].imag = 0; 
            }
        }
    }

    size_max = 0;
    for(j=0; j<channel_num; j++){
        for(i=0; i<fft_length; i++){
            taps = (int)((double)tap_lower + (double)(tap_upper - tap_lower) * (double)i / (double)fft_length);
            sbica_info[j][i].taps = taps;
            
            sbica_info[j][i].idx = 0;
            sbica_info[j][i].itvl = itvl;
            //sbica_info[j][i].mu_s = mu_s;
            //sbica_info[j][i].mu_n = mu_n;
            
            sbica_info[j][i].n_var = 1.0;
            sbica_info[j][i].r_var = 1.0;
            
            for(t=0; t<taps; t++){
                mu_f[j][i][t] = pow(lambda, t) * mu_v;
            }
            
            for(t=0; t<32; t++){
                size = 1 << t;
                if((taps * itvl) <= size){
                    break;
                }
            }
            sbica_info[j][i].mask = size -1;
            
            if(size_max < size){
                size_max = size;
            }
        }
    }
    
    buf_s = (COMPLEX_FLOAT ***)MemoryAllocate3D(channel_num, fft_length, size_max, sizeof(COMPLEX_FLOAT));
    if(buf_s == NULL){
        return -1;
    }
    
    buf_ns = (COMPLEX_FLOAT ***)MemoryAllocate3D(channel_num, fft_length, size_max, sizeof(COMPLEX_FLOAT));
    if(buf_ns == NULL){
        return -1;
    }
    
    for(j=0; j<channel_num; j++){
        for(i=0; i<fft_length; i++){
            for(t=0; t<size_max; t++){
                buf_s[j][i][t].real = 0; 
                buf_s[j][i][t].imag = 0; 
                
                buf_ns[j][i][t].real = 0; 
                buf_ns[j][i][t].imag = 0; 
            }
        }
    }
    return 0;
}

///////////////////////////////////////////////////////////////
//  SetParam
/************************************************************
**  History :
**    2012.12.16 K.Takagi 新規作成
*/
// Public : パラメータ設定
int HASetParamSemiBlindICA(Param_SemiBlindICA_t *param)
{
    int channel_num;
    channel_num = param->channel;
    if((channel_num <= 1) || (channel_num > 16)){
        return -1;
    }
    semi_blind_ica_info.channel     = param->channel;
    semi_blind_ica_info.length      = param->length;
    semi_blind_ica_info.interval    = param->interval;
    semi_blind_ica_info.tap_lower   = param->tap_lower;
    semi_blind_ica_info.tap_upper   = param->tap_upper;
    semi_blind_ica_info.lambda      = param->lambda;
    semi_blind_ica_info.mu_v        = param->mu_v;
    semi_blind_ica_info.mu_s        = param->mu_s;
    semi_blind_ica_info.mu_n        = param->mu_n;
    semi_blind_ica_info.is_zero     = param->is_zero;
    semi_blind_ica_info.output_freq = param->output_freq;
    return 0;
}

///////////////////////////////////////////////////////////////
//  MAIN
/************************************************************
**  History :
**    2012.12.18 K.Takagi 同じ入力データでg,sum,buf_ns,n_var,r_varの結果を確認
**    2012.12.17 K.Takagi 新規作成
*/
// Public : SemiBlindICA処理
int SemiBlindICA(int frame_count, COMPLEX_FLOAT **out_data, COMPLEX_FLOAT **in_data)
{
    int i;
    
    int idx;
    int itvl;
    int mask;
    int taps;
    
    float n_var;
    float r_var;
    
    int ch;
    int nchannel;
    int noise_ch;
    int freq;
    int f;
    
    float f_real;
    float f_imag;
    float cf_abs;
    
    // 入力データの最後のチャネルをノイズとする
    nchannel = semi_blind_ica_info.channel;
    noise_ch = semi_blind_ica_info.channel;
    freq = semi_blind_ica_info.length/2+1;
    
    for(ch=0; ch<nchannel; ch++){
        for(f=0; f<freq; f++){
            //complex<float> update(complex<float> x, complex<float> s)
            {
                COMPLEX_FLOAT d, sum, c1, n_d;
                int midx;
                float ry, v1, e1, rx, v2, e2;

                taps = sbica_info[ch][f].taps;

                idx  = sbica_info[ch][f].idx;
                itvl = sbica_info[ch][f].itvl;
                mask = sbica_info[ch][f].mask;

                n_var = sbica_info[ch][f].n_var;
                r_var = sbica_info[ch][f].r_var;

                //デバック
                //LOGI("ch = %d f = %d\n", ch, f);
                //LOGI("%d, %d\n", ch, f);
                //LOGI("x = (%g, %g) s = (%g, %g)\n", in_data[ch][f].real, in_data[ch][f].imag, in_data[noise_ch][f].real, in_data[noise_ch][f].imag);
                
                buf_s[ch][f][idx] = in_data[noise_ch][f];
                
                cf_abs = sqrt(in_data[noise_ch][f].real * in_data[noise_ch][f].real + in_data[noise_ch][f].imag * in_data[noise_ch][f].imag);
                
                if(cf_abs < semi_blind_ica_info.is_zero){
                    out_data[ch][f] = in_data[ch][f];
                    continue;
                }

                sum.real = 0;
                sum.imag = 0;
                for(i=0; i<taps; i++){
                    midx = (idx - i * itvl) & mask;
                    //sum += w[ch][f][i] * buf_s[ch][f][midx];
                    f_real = w[ch][f][i].real * buf_s[ch][f][midx].real - w[ch][f][i].imag * buf_s[ch][f][midx].imag;
                    f_imag = w[ch][f][i].real * buf_s[ch][f][midx].imag + w[ch][f][i].imag * buf_s[ch][f][midx].real;
                    sum.real += f_real;
                    sum.imag += f_imag;
                }
                d.real = in_data[ch][f].real - sum.real;
                d.imag = in_data[ch][f].imag - sum.imag;

                //デバック
                //LOGI("sum = (%g,%g) d = (%g,%g)\n", sum.real, sum.imag, d.real, d.imag);

                n_d.real = n_var * d.real;
                n_d.imag = n_var * d.imag;
                buf_ns[ch][f][idx].real = r_var * buf_s[ch][f][idx].real; 
                buf_ns[ch][f][idx].imag = r_var * buf_s[ch][f][idx].imag; 

                //デバック
                //LOGI("idx = %d buf_ns = (%g,%g)\n", idx, buf_ns[ch][f][idx].real, buf_ns[ch][f][idx].imag);

                cf_abs = sqrt(n_d.real * n_d.real + n_d.imag * n_d.imag);
                ry = cf_abs + FLOOR_VALUE * rand()/ (double)RAND_MAX;
                v1 = tanh(ry);
                e1 = ry;
                c1.real = v1 * n_d.real / ry;
                c1.imag = v1 * n_d.imag / ry;

                //
                cf_abs = sqrt(buf_ns[ch][f][idx].real * buf_ns[ch][f][idx].real + buf_ns[ch][f][idx].imag * buf_ns[ch][f][idx].imag);
                rx = cf_abs + FLOOR_VALUE * rand()/(double)RAND_MAX;
                v2 = tanh(rx);
                e2 = rx;

                // filter update
                for(i=0; i<taps; i++){
                    midx = (idx - i * itvl) & mask;
                    //w[i] += mu_f[ch][f][i] * (c1 * conj(buf_ns[ch][f][midx]));
                    f_real =   c1.real * buf_ns[ch][f][midx].real + c1.imag * buf_ns[ch][f][midx].imag;
                    f_imag = - c1.real * buf_ns[ch][f][midx].imag + c1.imag * buf_ns[ch][f][midx].real;
                    w[ch][f][i].real += mu_f[ch][f][i] * f_real;
                    w[ch][f][i].imag += mu_f[ch][f][i] * f_imag;
                }
                
                // variance estimation
                n_var += mu_n * (1 - v1 * e1) * n_var;
                if (n_var < 0) n_var *= -1;
                
                r_var += mu_s * (1 - v2 * e2) * r_var;
                if (r_var < 0) r_var *= -1;
                
                idx = (idx + 1) & mask;

                out_data[ch][f] = d;
                
                //更新
                sbica_info[ch][f].idx   = idx;
                sbica_info[ch][f].n_var = n_var;
                sbica_info[ch][f].r_var = r_var;
                
                //デバック
                //LOGI("n_var = %g r_var = %g\n", n_var, r_var);
            }
        }
    }
    return 0;
}

#ifdef STANDALON_TEST
/************************************************************
**  History :
**    2012.12.18 K.Takagi 新規作成
*/
// Public : 単体テストのメイン　固定値を使って結果が同じになることを確認
int main(int argc, char **argv)
{
    int hark_count;
    int stat;
    int channels;
    int ch;
    int f;
    
    int length;
    
    COMPLEX_FLOAT **in_data;
    COMPLEX_FLOAT **out_data;
        
    Param_SemiBlindICA_t semi_blind_ica_param;
    
    channels = 8;
    length   = 512;
    
    semi_blind_ica_param.channel     = channels;
    semi_blind_ica_param.length      = length;
    semi_blind_ica_param.interval    = 1;
    semi_blind_ica_param.tap_lower   = 8;
    semi_blind_ica_param.tap_upper   = 4;
    semi_blind_ica_param.lambda      = 0.8;
    semi_blind_ica_param.mu_v        = 0.01;
    semi_blind_ica_param.mu_s        = 0.01;
    semi_blind_ica_param.mu_n        = 0.01;
    semi_blind_ica_param.is_zero     = 0.001;
    semi_blind_ica_param.output_freq = 150;
    
    /* パラメータ設定 */
    HASetParamSemiBlindICA(&semi_blind_ica_param);
    
    /* 初期化 */
    stat = HAInitSemiBlindICA();
    if(stat < 0){
        LOGI("Init Error.\n");
        return -1;
    }
    //バッファクリア
    in_data  = NULL;
    out_data = NULL;
    in_data = (COMPLEX_FLOAT **)MemoryAllocate2D(channels+1, length/2+1, sizeof(COMPLEX_FLOAT));
    if(in_data == NULL){
        goto ErrEnd;
    }
    out_data = (COMPLEX_FLOAT **)MemoryAllocate2D(channels, length/2+1, sizeof(COMPLEX_FLOAT));
    if(out_data == NULL){
        goto ErrEnd;
    }
    //バッファ初期化（固定値）
    for(ch=0; ch<channels; ch++){
        for(f=0; f<length/2+1; f++){
            in_data[ch][f].real = 2.0;
            in_data[ch][f].imag = 1.1;
        }
    }
    //ノイズ部の初期化
    for(f=0; f<length/2+1; f++){
        in_data[channels][f].real = 0.2;
        in_data[channels][f].imag = 0.11;
    }
    for(hark_count=0; hark_count<100; hark_count++){
        LOGI("hark count = %d\n", hark_count);
        
        SemiBlindICA(hark_count, out_data, in_data);
    }
ErrEnd:
    if(in_data != NULL){
        free(in_data[0]);
        free(in_data);
    }
    if(out_data != NULL){
        free(out_data[0]);
        free(out_data);
    }
    HAReleaseSemiBlindICA();
    return 0;
}
#endif // STANDALON_TEST
#endif // BUILD_MODULE_SEMI_BLIND_ICA

/* EOF */

