/' (c) 2022 cepstrum.co.jp           '/
/' This is not free software.        '/
'
' GUI program, command line option : fbc -s gui
'
' adaptive howling canceller for ASIO audio device
'
' *** caution! : limiter position is different from standard adaptive howling canceller cofiguration ***
'
'  +------------------------------------------------------------------------------------------------------------------------------------------+
'  |                                                                                                                                          |
'  |                                                                                                           4th/ 250Hz                     |
'  |     deemphasis                                                                                   emphasis 2nd/3960Hz                     |
'  |      +-------+   +---------+   +---------+   +-----+                      +-----+   +---------+   +-----+   +-----+ d                    |
'  |  +-->| deemp +-->| fs conv +-->| limiter +-->| D/A +-->> out      in >>-->| A/D +-->| fs conv +-->| emp +-->| BPF +--+                   |
'  |  |   +-------+   +---------+   +---------+   +-----+                      +-----+   +---------+   +-----+   +-----+  |                   |
'  |  |             8kHz ---> 48kHz                                                    48kHz ---> 8kHz                    |           5Hz     |
'  |  |                                                                                                                +  v  e     +-------+  |
'  +--+ x                                                                                                                (+)---+-->| shift +--+
'     |                                                                                                          ^     -  ^    |   +-------+  
'     |                                                                                                          |        |    |   frequncy
'     |                                                                                                       +-----+  y  |    |     shift
'     +------------------------------------------------------------------------------------------------------>| ADP +-----+    |
'                                                                                                             +-----+          |
'                                                                                                                | 2048tap     |
'                                                                                                                +-------------+
'                           

#inclib "openblas"
#inclib "fsconv"

#include once "crt.bi"
#include once "portaudio.bi"
#include once "fbblas.bi"
#include once "fsconv.bi"
#include once "hilbert.bi"
#include once "graphics.bi"

#define FS             48000
#define PA_BUFLEN      6             ' 6=48kHz/8kHz

#define LMSLEN         2048
#define MU             0.00003 
#define POWER_BETA     0.999         
#define SHIFT_PERIOD   1600          ' 8000/1600=5[Hz]
#define EMP_DEEMP_COEF 0.8

dim shared as single  wcopy(LMSLEN)
dim shared as integer wcopy_wt_busy = 0
dim shared as integer wcopy_rd_busy = 1

type User_Data_Type
  frame_count as integer
end type

dim user_data        as User_Data_Type
dim outputParameters as PaStreamParameters 
dim inputParameters  as PaStreamParameters 
dim stream           as any ptr 
dim errcode          as integer<32>

dim now_time         as string
dim past_time        as string
dim past_frame_count as integer

dim asio_api_index   as integer
dim host_api_info    as PaHostApiInfo ptr
dim device_index     as integer

dim inchar           as string
dim keycode          as integer

dim i                as integer
dim k                as integer

dim cmd_opt_val      as integer

#define FFTLEN 2048
dim ar(FFTLEN) as single
dim ai(FFTLEN) as single
#include "fft.bi"

'==========================================================================

function atan_limiter(x as single, beta as single) as single
  return beta*atan_(x/beta)         ' call atan in crt.bi
end function

'==========================================================================

function emphasis(x as single, c as single) as single
  static zx as single = 0.0
  dim    y  as single

  y=x-c*zx
  zx=x
  return y
end function

'==========================================================================

function deemphasis(x as single, c as single) as single
  static y as single = 0.0

  y=x+c*y
  return y
end function

'==========================================================================

function hpf4th250hz(x as single) as single
  static zzzzx as single = 0.0
  static zzzx  as single = 0.0
  static zzx   as single = 0.0
  static zx    as single = 0.0
  static zzzzy as single = 0.0
  static zzzy  as single = 0.0
  static zzy   as single = 0.0
  static zy    as single = 0.0
  dim    y     as single

  y=7.733467891606214950e-01*(x-4.0*zx+6.0*zzx-4.0*zzzx+zzzzx)+3.487307741549900086e+00*zy-4.589291232078407390e+00*zzy+2.698884391340754973e+00*zzzy-5.980652616008872435e-01*zzzzy
  zzzzx=zzzx
  zzzx =zzx
  zzx  =zx
  zx   =x
  zzzzy=zzzy
  zzzy =zzy
  zzy  =zy
  zy   =y
  return y
end function

'==========================================================================

function lpf2nd3960hz(x as single) as single
  static zzx as single = 0.0
  static zx  as single = 0.0
  static zzy as single = 0.0
  static zy  as single = 0.0
  dim    y   as single

  y=9.780304792065596330E-1*(x+2.0*zx+zzx)-1.955578240315035470E0*zy-9.565436765112032450E-1*zzy
  zzx=zx
  zx =x
  zzy=zy
  zy =y
  return y
end function

'==========================================================================

function callbackfunc cdecl(                   _
  pread      as single ptr,                    _
  pwrite     as single ptr,                    _
  rwcount    as uinteger<32>,                  _
  timeInfo   as PaStreamCallbackTimeInfo ptr,  _
  flag       as uinteger<32>,                  _
  user_data  as User_Data_Type ptr             _
) as integer

  dim ad48k(6) as LrData
  dim da48k(6) as LrData
  dim ad16k0   as LrData
  dim ad16k1   as LrData
  dim da16k0   as LrData
  dim da16k1   as LrData
  dim ad8k     as LrData
  dim da8k     as LrDAta

  dim x        as single
  dim zx       as single
  dim y        as single
  dim d        as single
  dim i        as integer

  static xbuf(LMSLEN*2) as single
  static w(LMSLEN)      as single
  static power          as single  = 100.0
  static xbuf_ix        as longint = 0
  static init_flag      as integer = 1
  static frame_counter  as integer = 0

  static e              as single  = 0.0
  static sincos_ix      as integer = 0
  dim    ei             as double
  dim    eq             as double
  dim    si             as single
  dim    co             as single

  if init_flag=1 then
    init_flag=0
    for i=0 to LMSLEN-1
      xbuf(i)=0.0
      xbuf(i+LMSLEN)=0.0
      w(i)=0.0
    next
    return 0
  end if

  '-------------------------------------------------------------------------------------
  ' read A/D and Fs conversion (48kHz ---> 8kHz)

  for i=0 to 5
    ad48k(i).l=*(pread+2*i+0)
    ad48k(i).r=*(pread+2*i+1)
  next

  decimate6to2(ad48k(0), ad48k(1), ad48k(2), ad48k(3), ad48k(4), ad48k(5), ad16k0, ad16k1)    ' 6:2 decimation, fs=48kHz --> 16kHz
  decimate2iir_lpc(ad16k0, ad16k1, ad8k)                                                      ' 2:1 decimation, fs=16kHz --> 8kHz

  '-------------------------------------------------------------------------------------

  x=e

  da8k.l=deemphasis(x, EMP_DEEMP_COEF)
  da8k.r=0.0

  d=emphasis(ad8k.l, EMP_DEEMP_COEF) 
  d=hpf4th250hz(d)
  d=lpf2nd3960hz(d)
 
  y=fbblas_fir(x, xbuf(), w(), LMSLEN, xbuf_ix)
  e=d-y
  power=POWER_BETA*power+(1.0-POWER_BETA)*x*x
  fbblas_lms(w(), 2.0*MU*e/power, xbuf(), LMSLEN, xbuf_ix)

  hilbert8th(e, ei, eq)
  sincos_ix=(sincos_ix+1) mod SHIFT_PERIOD
  si=sin(2.0*M_PI*csng(sincos_ix)/csng(SHIFT_PERIOD))
  co=cos(2.0*M_PI*csng(sincos_ix)/csng(SHIFT_PERIOD))
  e=ei*si+eq*co

  '-------------------------------------------------------------------------------------
  ' Fs conversion (8kHz ---> 48kHz) amd wrote D/A

  interpolate2iir_lpc(da8k, da16k0, da16k1)                                                    ' 1:2 interpolation, fs=8kHz --> 16kHz
  interpolate2to6(da16k0, da16k1, da48k(0), da48k(1), da48k(2), da48k(3), da48k(4), da48k(5))  ' 2:6 interpolation, fs=16kHz --> 48kHz

  for i=0 to 5
    *(pwrite+2*i+0)=atan_limiter(da48k(i).l, 0.99)
    *(pwrite+2*i+1)=da48k(i).r
  next

  '-------------------------------------------------------------------------------------

  if wcopy_rd_busy=0 then
    wcopy_wt_busy=1
    for i=0 to LMSLEN-1
      wcopy(i)=w(i)
    next
    wcopy_wt_busy=0
  end if

  frame_counter=frame_counter+1
  (user_data->frame_count)=frame_counter

  return 0

end function

'==========================================================================

function cmd_opt_int(optstr as string) as integer
  dim s       as string
  dim i       as integer
  dim opt_val as integer
  
  opt_val=-1
  i=1
  while command(i)<>""
    s=lcase(command(i)) 
    if instr(s, "/"+optstr)<>0 or instr(s, "-"+optstr) then
       s=mid(s, 3)
       opt_val=valuint(s)
    end if  
    i=i+1
  wend
  return opt_val
end function

'==========================================================================

sub plot_thread(ar() as single)
  static zploty as integer
  dim plotx as integer
  dim ploty as integer
  dim i     as integer
  dim wmax  as single = 0.0
  
  g_fillbox(0, 0, 1024+1, 560+1, BLACK_COLOR)              ' clear screen

  g_dotline(128, 160, 128, 160+400, DARK_GRAY_COLOR, &haaaaaaaa)
  g_line   (256, 160, 256, 160+400, DARK_GRAY_COLOR)
  g_dotline(384, 160, 384, 160+400, DARK_GRAY_COLOR, &haaaaaaaa)
  g_line   (512, 160, 512, 160+400, DARK_GRAY_COLOR)
  g_dotline(640, 160, 640, 160+400, DARK_GRAY_COLOR, &haaaaaaaa)
  g_line   (768, 160, 768, 160+400, DARK_GRAY_COLOR)
  g_dotline(896, 160, 896, 160+400, DARK_GRAY_COLOR, &haaaaaaaa)

  g_dotline(128, 0, 128, 160, DARK_GRAY_COLOR, &haaaaaaaa)
  g_line   (256, 0, 256, 160, DARK_GRAY_COLOR)
  g_dotline(384, 0, 384, 160, DARK_GRAY_COLOR, &haaaaaaaa)
  g_line   (512, 0, 512, 160, DARK_GRAY_COLOR)
  g_dotline(640, 0, 640, 160, DARK_GRAY_COLOR, &haaaaaaaa)
  g_line   (768, 0, 768, 160, DARK_GRAY_COLOR)
  g_dotline(896, 0, 896, 160, DARK_GRAY_COLOR, &haaaaaaaa)

  g_line(0, 160+320, 1024, 160+320, DARK_GRAY_COLOR)
  g_line(0, 160+240, 1024, 160+240, RED_COLOR)             ' 0dB line
  g_line(0, 160+160, 1024, 160+160, DARK_GRAY_COLOR)
  g_line(0, 160+ 80, 1024, 160+ 80, DARK_GRAY_COLOR)

  for i=0 to FFTLEN/2-1 
    plotx=i
    ploty=ar(i)*8+160+240
    if ploty<160.0 then
      ploty=160.0
    end if
    if i=0 then
      zploty=ploty
    end if
    g_line(plotx, zploty, plotx+1, ploty, GREEN_HIGH_COLOR)
    zploty=ploty
  next

  g_line(0, 160, 1024, 160, LIGHT_GRAY_COLOR)
  g_line(0,  80, 1024,  80, DARK_GRAY_COLOR)

  for i=0 to 1024
    if wmax<abs(wcopy(i)) then
      wmax=abs(wcopy(i))
    end if
  next
  for i=0 to 1024
    g_line(i, 80+0.9*wcopy(i)/wmax*80, i+1, 80+0.9*wcopy(i+1)/wmax*80, GREEN_HIGH_COLOR)
  next

  g_string_bg(4, 549, " 20dB", BLACK_COLOR) 
  g_string   (4, 549, " 20dB", WHITE_COLOR) 
  g_string_bg(4, 479, " 10dB", BLACK_COLOR) 
  g_string   (4, 479, " 10dB", WHITE_COLOR) 
  g_string_bg(4, 396, "  0dB", BLACK_COLOR) 
  g_string   (4, 396, "  0dB", WHITE_COLOR) 
  g_string_bg(4, 316, "-10dB", BLACK_COLOR) 
  g_string   (4, 316, "-10dB", WHITE_COLOR) 
  g_string_bg(4, 236, "-20dB", BLACK_COLOR) 
  g_string   (4, 236, "-20dB", WHITE_COLOR) 
  g_string_bg(4, 163, "-30dB", BLACK_COLOR) 
  g_string   (4, 163, "-30dB", WHITE_COLOR) 

  g_string_bg(241, 163, "1kHz", BLACK_COLOR) 
  g_string   (241, 163, "1kHz", WHITE_COLOR) 
  g_string_bg(497, 163, "2kHz", BLACK_COLOR) 
  g_string   (497, 163, "2kHz", WHITE_COLOR) 
  g_string_bg(753, 163, "3kHz", BLACK_COLOR) 
  g_string   (753, 163, "3kHz", WHITE_COLOR) 
  g_string_bg(991, 163, "4kHz", BLACK_COLOR) 
  g_string   (991, 163, "4kHz", WHITE_COLOR) 

  g_string_bg(  4, 149, "0",   BLACK_COLOR) 
  g_string   (  4, 149, "0",   WHITE_COLOR) 
  g_string_bg(246, 149, "256", BLACK_COLOR) 
  g_string   (246, 149, "256", WHITE_COLOR) 
  g_string_bg(502, 149, "512", BLACK_COLOR) 
  g_string   (502, 149, "512", WHITE_COLOR) 
  g_string_bg(758, 149, "768", BLACK_COLOR) 
  g_string   (758, 149, "768", WHITE_COLOR) 
  g_string_bg(991, 149, "1024", BLACK_COLOR) 
  g_string   (991, 149, "1024", WHITE_COLOR) 

  g_string_bg(  4, 2, "0ms",   BLACK_COLOR) 
  g_string   (  4, 2, "0ms",   WHITE_COLOR) 
  g_string_bg(241, 2, "32ms",  BLACK_COLOR) 
  g_string   (241, 2, "32ms",  WHITE_COLOR) 
  g_string_bg(497, 2, "64ms",  BLACK_COLOR) 
  g_string   (497, 2, "64ms",  WHITE_COLOR) 
  g_string_bg(753, 2, "96ms",  BLACK_COLOR) 
  g_string   (753, 2, "96ms",  WHITE_COLOR) 
  g_string_bg(984, 2, "128ms", BLACK_COLOR) 
  g_string   (984, 2, "128ms", WHITE_COLOR) 

  g_string_bg(820, 549, "hit ESC or ctrl-c to exit", BLACK_COLOR) 
  g_string   (820, 549, "hit ESC or ctrl-c to exit", YELLOW_COLOR) 

end sub

'==========================================================================

windowtitle " howling canceller demo program for Windows PC with ASIO audio device, (c) 2022 cepstrum.co.jp"
screenres 1024+1, 560+1

fft(ar(), ai(), -1, 1)

cmd_opt_val=cmd_opt_int("d")          ' read /d, -d, /D, -D option

errcode=Pa_Initialize()
if errcode<>0 then
  Pa_Terminate()
  print "!!! PortAudio initialize error"
  sleep 3000
  system
end if

if cmd_opt_val<0 then
  asio_api_index=Pa_HostApiTypeIdToHostApiIndex(3)     ' (index val 3)= asio
  host_api_info=Pa_GetHostApiInfo(asio_api_index)
  if (host_api_info->deviceCount)=0 then
    Pa_Terminate()
    print "!!! no ASIO device"
    sleep 3000
    system   
  end if
  device_index=Pa_HostApiDeviceIndexToDeviceIndex(asio_api_index, 0)   ' find 1st ASIO device
else
  device_index=cmd_opt_val
end if

outputParameters.device                   =device_index
outputParameters.channelCount             =2
outputParameters.sampleFormat             =paFloat32  
outputParameters.suggestedLatency         =paLatencyFs48k64Samples
outputParameters.hostApiSpecificStreamInfo=NULL

inputParameters.device                    =device_index
inputParameters.channelCount              =2
inputParameters.sampleFormat              =paFloat32  
inputParameters.suggestedLatency          =paLatencyFs48k64Samples
inputParameters.hostApiSpecificStreamInfo =NULL

errcode=Pa_OpenStream(  _  
  @stream,              _  ' PaStream  ptr ptr
  @inputParameters,     _  ' input parameters
  @outputParameters,    _  ' output parameters
  FS,                   _  ' Fs
  PA_BUFLEN,            _  ' frames per buffer
  0,                    _  ' stream flags
  @callbackfunc,        _  ' callback function
  @user_data            _  ' user data
) 
if errcode<>0 then
  Pa_Terminate()
  print "!!! PortAudio stream setting error"
  sleep 3000
  system
end if

errcode=Pa_StartStream(stream)
if errcode<>0 then
  Pa_Terminate()
  print "!!! PortAudio can't start stream"
  sleep 3000
  system
end if

past_time=""
past_frame_count=0
do

  if wcopy_wt_busy=0 then
    wcopy_rd_busy=1

    for i=0 to FFTLEN-1
      ar(i)=wcopy(i)
      ai(i)=0.0
    next

    fft(ar(), ai(), -1, 0)

    for i=0 to FFTLEN/2-1
      ar(i)=ar(i)*ar(i)+ai(i)*ai(i)   ' calc power
      ar(i)=10.0*log10(ar(i)+1e-19)   ' cals power spectrum
    next

    dim thread_id as any ptr = threadcall plot_thread(ar())

    sleep 800, 1
    wcopy_rd_busy=0
  end if

  sleep 200, 1
  inchar=inkey$

  keycode=asc(inchar)
  if keycode=3 or keycode=27 then           ' if ctrl-C or ESC
    errcode=Pa_StopStream(stream)
    errcode=Pa_CloseStream(stream)
    Pa_Terminate()

    open "out.txt" for output as #1
    for i=0 to LMSLEN-1
      print #1, i, wcopy(i)
    next
    close #1

    system
  end if

  now_time=time
  if now_time<>past_time then
    past_frame_count=user_data.frame_count
  end if
  past_time=now_time
loop