unit ProcMath;

interface

uses Windows,LibV, tgen;

const
  SIZE_POWER = 13;
  INBLK_SIZE = 8192;

var
  use_pad   : boolean = true;
  use_fixup : boolean = true;
  pad_len   : integer = 512;
  data_len  : integer;

{$DEFINE HAVE_REALFT}

// {$DEFINE DEBF}

function BeginProcessing(Frequencies,Amplitudes,
  Channel1Window,Channel2Window : vector_p;pad,fixup:vboolean):vboolean;

procedure EndProcessing;

procedure IdleProc(Processing:vboolean;var Done:Boolean);

implementation

var
  waveout : vhandle_t = LIBV_INVALID_HANDLE;

  Freq, Amp, WinLeft, WinRight : vector_p;


  Samples : array[0..INBLK_SIZE-1] of vec_t;

  InReal: array[0..INBLK_SIZE-1] of vec_t;

  FreqReal : array[0..INBLK_SIZE-1] of vec_t;
  FreqReal1 : array[0..INBLK_SIZE-1] of vec_t;
  FreqReal2 : array[0..INBLK_SIZE-1] of vec_t;

{$IFNDEF HAVE_REALFT}
  Dummy : array[0..INBLK_SIZE-1] of vec_t;
  FreqImag : array[0..INBLK_SIZE-1] of vec_t;
  FreqImag1 : array[0..INBLK_SIZE-1] of vec_t;
  FreqImag2 : array[0..INBLK_SIZE-1] of vec_t;
{$ENDIF} {!HAVE_REALFT}

  Samples1 : array[0..INBLK_SIZE-1] of vec_t;
  Samples2 : array[0..INBLK_SIZE-1] of vec_t;

  OutSamples : array[0..2*INBLK_SIZE-1] of s16_t;

(*
 * This routine stores floating-point data from 'Samples1' and 'Samples2'
 * in 'OutSamples' using packed 16-bit PCM format.
 *
 * BUGS: depends on FPU rounding mode.
 *)
procedure Collect2Channels;assembler;
const
  upper_limit : vec_t = MAXSHORT;
asm
  push ebx
  push esi

  mov esi, data_len
  mov eax, pad_len
  shl eax, 2
  mov ebx, offset Samples1
  mov ecx, offset Samples2
  add ebx, eax
  add ecx, eax
  dec esi
  mov edx, offset OutSamples

@@next_pair:
  fld dword ptr[ebx + esi * 4]
  fld upper_limit
  fmulp
  fcom upper_limit
  fstsw ax
  sahf
  jbe @@_1_ok
  ffree st(0)
  mov word ptr[edx + esi * 4], MAXSHORT
  jmp @@_load_2
@@_1_ok:
  fistp word ptr[edx + esi * 4]
@@_load_2:
  fld dword ptr[ecx + esi * 4]
  fld upper_limit
  fmulp
  fcom upper_limit
  fstsw ax
  sahf
  jbe @@_2_ok
  ffree st(0)
  mov word ptr[edx + esi * 4 + 2], MAXSHORT
  jmp @@_check_end
@@_2_ok:
  fistp word ptr[edx + esi * 4 + 2]
@@_check_end:
  dec esi
  jge @@next_pair

  pop esi
  pop ebx
end;

{$IFDEF HAVE_REALFT}
procedure fftRealNip(dest,src:vec_t_array_p;size_power:u32_t;forw:vboolean);
  stdcall;external 'LibV.dll' name '_fftRealNip@16';

procedure FFTForward;
begin
  fftRealNip(@FreqReal,@InReal,SIZE_POWER,True);
end;

procedure ApplyWindows;
var
  wlen,i : integer;
begin
  wlen := INBLK_SIZE-1;
  for i:=0 to wlen do
  begin
    FreqReal1[i] := FreqReal[i] * WinLeft^.elements^[i shr 1];
    FreqReal2[i] := FreqReal[i] * WinRight^.elements^[i shr 1];
  end;
end;

procedure FFTInverse;
begin
  fftRealNip(@Samples1,@FreqReal1,SIZE_POWER,False);
  fftRealNip(@Samples2,@FreqReal2,SIZE_POWER,False);
end;

{$ELSE} {!HAVE_REALFT}

procedure FFTForward;
begin
  memZero(@Dummy,INBLK_SIZE * sizeof(vec_t));
  fftCalcNip(@FreqReal,@FreqImag,@InReal,@Dummy,SIZE_POWER,1);
end;

procedure ApplyWindows;
var
  wlen,i : integer;
begin
  wlen := INBLK_SIZE-1;
  for i:=0 to wlen do
  begin
    FreqReal1[i] := FreqReal[i] * WinLeft^.elements^[i];
		FreqImag1[i] := FreqImag[i] * WinLeft^.elements^[i];
    FreqReal2[i] := FreqReal[i] * WinRight^.elements^[i];
		FreqImag2[i] := FreqImag[i] * WinRight^.elements^[i];
  end;
end;

procedure FFTInverse;
begin
  fftCalcNip(@Samples1,@Dummy,@FreqReal1,@FreqImag1,SIZE_POWER,-1);
  fftCalcNip(@Samples2,@Dummy,@FreqReal2,@FreqImag2,SIZE_POWER,-1);
end;
{$ENDIF) {!HAVE_REALFT}

{$IFDEF DEBF}
var
  wfile : vhandle_t;
{$ENDIF}

procedure WriteStereoOutput;
begin
  LibV.fileWrite(waveout,@OutSamples,data_len * 4);
{$IFDEF DEBF}
  LibV.fileWrite(wfile,@OutSamples,data_len * 4);
{$ENDIF}
end;

procedure FFTPad;
var
  i, start, limit : integer;
begin
  limit := pad_len shl 1;
  { First block: PAD_LEFT + PAD_RIGHT copies of last samples }
  i := 0;
  start := data_len;
  while i <> limit do
  begin
    InReal[i] := InReal[start + i];
    i := i + 1;
  end;
  { Rest of data: new samples }
  repeat
    InReal[i] := Samples[i];
    i := i + 1;
  until i = INBLK_SIZE;
end;

var
  u : vec_t;

procedure FFTFixUp;
var
  i : integer;
  delta, du : vec_t;
begin
  delta := u - Samples1[pad_len];
  if {v_abs(delta) < 0.2} true then
  begin
    du := delta / data_len;
    delta := du;
    i := PAD_len+data_len-2;
    repeat
      Samples1[i] := Samples1[i]+delta;
      delta := delta + du;
      i := i - 1;
    until i < pad_len;
  end;
  u := Samples1[pad_len+data_len-1];
end;

procedure ProcessNextBlock;
begin
  if use_pad then
  begin
    toneGenerate(@Samples[pad_len shl 1],DATA_LEN);
    FFTPad;
  end
  else
  begin
    toneGenerate(@InReal[pad_len],data_len);
  end;
  FFTForward;
  ApplyWindows;
  FFTInverse;
  if use_fixup then
    FFTFixUp;
  Collect2Channels;
  WriteStereoOutput;
end;

procedure IdleProc(Processing:vboolean;var Done:Boolean);
const
  max_qlen = 44100 * 2 * 2;
var
  qlen : u32_t;
begin
  if not Processing then
  begin
    Done := True;
    Exit;
  end;
  qlen := 0;
  if fileControl(waveout,pctl_waveout_get_pending_samples,qlen) and
    (qlen < max_qlen) then
    ProcessNextBlock
  else
    timeDelay(5);
  Done := False;
end;

function BeginProcessing(Frequencies,Amplitudes,
  Channel1Window,Channel2Window : vector_p;pad,fixup:vboolean):vboolean;
begin
  Result := False;
  use_pad := pad;
  use_fixup := fixup;
  memZero(@InReal,INBLK_SIZE * sizeof(vec_t));
  memZero(@Samples,INBLK_SIZE * sizeof(vec_t));
  if not toneInit(Frequencies,Amplitudes) then
    Exit;
  if waveout = LIBV_INVALID_HANDLE then
  begin
    Windows.MessageBox(NULL,
      'Multiband filter','Unable to open waveform output',MB_ICONHAND);
    Exit;
  end;
  Freq := Frequencies;
  Amp := Amplitudes;
  WinLeft := Channel1Window;
  WinRight := Channel2Window;
  Result := True;
end;

procedure EndProcessing;
begin
  wavePlaybackReset(waveout);
end;

initialization
  LibV.logStartConsoleLog();
  LibV.logUpperLevel(LOG_TRACE);
  data_len := INBLK_SIZE - (pad_len shl 1);
  waveout := LibV.wavePlaybackOpen(2,44100,16);
{$IFDEF DEBF}
  wfile := LibV.wavOpenWrite('//d/out.wav',2,44100,16);
{$ENDIF}
finalization
  LibV.fileClose(waveout);
{$IFDEF DEBF}
  LibV.fileClose(wfile);
{$ENDIF}
end.

