// (c) 2022 cepstrum.co.jp
// howling canceller simulation program for Scilab
//
//  +------------------------------------------------------------+
//  |                                                            |
//  |                                 s (speech signal)          |
//  |                                 |                          |
//  |                       +---+ d   v                          |
//  |                  +--->| h +--->(+)---+ ds                  |
//  |                  |    +---+          |                     |
//  |    +---------+   |                 + v         +-------+   |
//  +--->| limiter +---+ x    ^           (+)---+--->| delay +---+
//       +---------+   |      |          - ^    | e  +-------+ ze
//                     |    +---+          |    |
//                     +--->| w +----------+    |
//                          +---+ y             |
//                            |                 |
//                            +-----------------+

clear;                  // clear all variables

INPUT_WAVEFILE_NAME='ford8k.wav';
OUTPUT_WAVEFILE_NAME='out.wav';

DATLEN=200000;          // data file length (even number)
FS=8000;                // sampling frequency [Hz]
FIRLEN=256;             // adaptive filter legnth
DLYLEN=256;             // delay length
GAIN=4.0;               // maximum gain of acoustic system
MARGIN=2.0;             // dynamic range margin
MU=0.05;                // step size parameter of adaptive filter

indata=loadwave(PWD+'\'+INPUT_WAVEFILE_NAME);

outdata=zeros(1:DATLEN);
missalignment=zeros(1:DATLEN);
err=zeros(1:DATLEN);

// generate impulse response of acoustice system (h)
rand('normal');
rand('seed', 912);
h=[zeros(1:5), rand(1:FIRLEN-5)];
h=h.*(0.97^(1:FIRLEN)); 
h=h/max(abs(fft(h, -1)));
h=GAIN*h;

buf=zeros(1:FIRLEN);
w=zeros(1:FIRLEN);
delay=zeros(1:DLYLEN);

ze=0;
for i=1:DATLEN 
  x=ze;

  // limiter
  if (1.0/GAIN)<abs(x)
    x=(1.0/GAIN)*sign(x);
  end

  buf=[x, buf(1:FIRLEN-1)];
  d=h*buf';
  s=indata(i)/(GAIN*MARGIN);
  ds=d+s;
  y=w*buf';
  e=ds-y;
  
  ze=delay(DLYLEN);
  delay=[e, delay(1:DLYLEN-1)];

  w=w+2*MU*e*buf;       // LMS algorithm

  outdata(i)=x;
  missalignment(i)=10.0*log10(sum(abs(w-h).^2)/sum(h.^2)+1e-10);
  err(i)=e-s;
end

savewave(PWD+'\'+OUTPUT_WAVEFILE_NAME, 0.99*GAIN*outdata, FS);

// plot output signal of limiter (x)
scf(1);
clf;
title('limiter output (x)', 'fontsize', 3);
plot2d(1:DATLEN, outdata, style=2);

// plot missalignment of adaptive filter coefficient [dB]
scf(2);
clf;
title('miss alignment [dB]', 'fontsize', 3);
plot2d(1:DATLEN, missalignment, style=2, rect=[0, -20, DATLEN, 0]);

// plot impulse response (h, w)
scf(3);
clf;
title('imuplse response (blue-->h, red-->w)', 'fontsize', 3);
plot2d(1:FIRLEN, h, style=2);       // blue
plot2d(1:FIRLEN, w, style=5);       // red

// calculate frequency response of h, w
hspec=fft([h, zeros(1:FS-FIRLEN)], -1);
hspec=hspec(1:FS/2);
[hdb, hphi]=dbphi(hspec);
wspec=fft([w, zeros(1:FS-FIRLEN)], -1);
wspec=wspec(1:FS/2);
[wdb, wphi]=dbphi(wspec);

// plot amplitude response of h, w [dB]
scf(4);
clf;
title('amplitude response [dB] (blue-->h  red-->w)', 'fontsize', 3);
plot2d(1:FS/2, hdb, style=2);       // blue
plot2d(1:FS/2, wdb, style=5);       // red

// plot phase response of h, w 
scf(5);
clf;
title('phase response (blue-->h  red-->w)', 'fontsize', 3);
plot2d(1:FS/2, hphi/180, style=2);  // blue 
plot2d(1:FS/2, wphi/180, style=5);  // red