# To Formants.
# To be called from Dutch/English/Chinese/DataFormants.script.


form "F0-controlled Formant Estimation"
  real left_Frequency_range 0
  positive right_Frequency_range 5000
  comment Measurement points must have been defined in Matrix "syllables".
  comment Pitch contour should have been stored in Pitch "filtered".
  positive Frequency_resolution_(Hz) 10
  positive Local_bandwidth_(B/F0) 1.1
  boolean Pre_emphasize 1
  comment 
  natural Maximum_number_of_formants_per_column 10
  comment For noisy sounds, raise Formant display threshold.
  real Formant_display_threshold_(0..25) 9
endform

# Shorten variables.
fMin = left_Frequency_range
fMax = right_Frequency_range
pe = pre_emphasize
fStep = frequency_resolution
locB = local_bandwidth
fdt = formant_display_threshold
if fdt < 0
  pause Formant threshold should not be negative. Please restart.
  exit
endif
maxFor = maximum_number_of_formants_per_column

Copy... chunk
if pe = 1
  Pre-emphasize (in-place): 50
endif

len = Get total duration

# Define f grid resolution.
fSteps = floor(fMax/fStep)

select Pitch filtered
mPitch = Get quantile: 0, 0, 0.5, "Hertz"
voice = 1
if mPitch = undefined
  mPitch = 150
  voice = 0
endif

selectObject: "Matrix syllables"
nCols = Get number of columns

# Set up formants matrices.
Create simple Matrix: "Formants", maxFor, nCols, "0"
Create simple Matrix: "q", maxFor, nCols, "0"

totalCountF = 0

for mp to nCols
  selectObject: "Matrix syllables"
  tc = Get value in cell: 1, mp
  v = Get value in cell: 2, mp

  # Get local pitch.
  select Pitch filtered
  f0 = Get value at time: tc, "Hertz", "nearest"
  if v = -1
    f0 = mPitch
    voice = 0
  else
    voice = 1
  endif
  
  # Extract 5 periods. Avoids vertical F0 stripes.
  select Sound chunk
# win = 5/f0
# t2 = tc+2.5/f0
# pause win = 'win' f0 = 'f0'

  part =  Extract part: tc-2.5/f0, tc+2.5/f0, "Hanning", 1, "no"

  select part
  spec = To Spectrum (resampled): 50
  Formula... sqrt(self[1,col]^2+self[2,col]^2)
  matrix = To Matrix
  To Sound (slice): 1

  # Limit range to fMax + marge for detection eventual peak at right edge.
  range = Extract part: 0, 1.02*fMax, "rectangular", 1, "no"

  # Resample to round value of bins because queried value can be
  #   different from actual value in the 14th pos. after the decimal point!
  Resample... 1/5 2
  Rename... spectrum

  # Make Gaussian smoothing filter i.r., its bandwidth adapted to
  # local f0, avoiding F0 ripple.
  # If unvoiced the bandwidth is set high.
  b = locB*f0
  if voice = 1
    Create Sound from formula: "filter", 1, -2*f0, 2*f0, 1/5,
    ... "e^(-(2*ln(2)/('b'^2)*x^2))"
  else
    b = 4*f0
    Create Sound from formula: "filter", 1, -5*f0, 5*f0, 1/5,
    ... "e^(-(2*ln(2)/('b'^2)*x^2))"
    # Restore b to former value.
    b = locB*f0
  endif

  nSamp = Get number of samples

  # Get smoothed spectrum.
  plus Sound spectrum
  con = Convolve: "sum", "zero"
  Extract part: 0, fMax, "rectangular", 1, "no"

  # Make result of convolution sum independent of i.r. length.
  Formula... self/nSamp
  Rename... specData

  select Sound filter
  plus con
  Remove

  select Sound specData
  # Convert to power.
  Formula... self^2
  max = Get maximum: 0, 0, "parabolic"

  To PointProcess (extrema): 1, "yes", "no", "Sinc70"
  nPeaks = Get number of points

  select Sound specData
  mean = Get mean: 0, 0, 0

  Copy: "specDataDiff"
  # 'dt' = sample duration = 5
  Formula: "(self[col+1]-self[col])*5"
  # Correct shift caused by differentiation (half sample time).
  Shift times by: 2.5

  peaks = 0
  localCountF = 0

  fTh = fdt*(30/9-0.12/9*f0)
  if fTh<0
    fTh=0
  endif

  for forIndex to nPeaks
    select PointProcess specData
    t = Get time from index... 'forIndex'

    if forIndex > 1
      tPrev = Get time from index... 'forIndex'-1
    else
      tPrev = 0
    endif
    if t > fMax
      goto SKIP
    endif

    # Preceding peak in derivative acts as a measure for protrudence.
    select Sound specDataDiff
    maxPrev = Get maximum: tPrev, t, "sinc70"
    select Sound specData
    mag = Get value at time: 0, t, "Sinc70"
    prot = maxPrev/mag

    # Select only peaks sufficiently protruding.

    if voice = 1
      # Long periods cause steep slopes and vv.
      threshold = fTh/b
    else
      threshold = 4/b
        if mag < mean/8
          goto SKIP
        endif
    endif

    if prot < threshold
      goto SKIP
    endif

    totalCountF += 1
    time'totalCountF' = tc
    freq'totalCountF' = t
    localCountF +=1

    if forIndex > maxFor
      goto SKIP
    endif

    # Add to formant and q Matrices.
    select Matrix Formants
    Set value: localCountF, mp, round(t)
    select Matrix q
    Set value: localCountF, mp, prot

    label SKIP

  endfor ;fIndex

  select PointProcess specData
  plus Sound specDataDiff
  Remove

  label FILL

  select part
  plus spec
  plus matrix
  plus Sound specData
  plus Sound chunk_part
  plus range
  plus Sound spectrum
  Remove

endfor ;mp

exit