# To Formant spectrogram.

form "To F0-controlled Formant Spectrogram"
  real left_Frequency_range 0
  positive right_Frequency_range 5000
  positive Time_resolution_of_display_(ms) 5
  positive Frequency_resolution_of_display_(Hz) 20
  positive Local_bandwidth_(B/F0) 1.2
  boolean Pre_emphasize 1
  comment 
  natural Maximum_number_of_formants_per_column 10
  choice Mark_formant_peaks 1
    option Include voiceless parts
    option Exclude voiceless parts
    option No
  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
tStep = time_resolution_of_display/1000
fStep = frequency_resolution_of_display
locB = local_bandwidth
fTh = formant_display_threshold
if fTh < 0
  pause Formant threshold should not be negative. Please restart.
  exit
endif
maxFor = maximum_number_of_formants_per_column

obj$ = selected$("Sound")
Copy... sound

# Pad with zeros when Sound is too short.
length = Get total duration
if length < 0.05
  fSamp = Get sampling frequency
  Create Sound from formula: "zeros", 1, 0, 0.05, fSamp, "0"
  plus Sound sound
  Concatenate
  select Sound sound
  plus Sound zeros
  Remove
  select Sound chain
  Rename... sound
endif

To Pitch: tStep, 75, 500
mPitch = Get quantile: 0, 0, 0.5, "Hertz"
voice = 1
if mPitch = undefined
  mPitch = 150
  voice = 0
endif

select Sound sound
if pe = 1
  Pre-emphasize (in-place): 50
endif

len = Get total duration
tSteps = floor(len/tStep)

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

# Set up spectrogram matrix.
Create Matrix... 'obj$' 0 'len' 'tSteps' 'tStep' 'tStep'/2
... 0 'fMax' 'fSteps' 'fStep' 0 0

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

if mark_formant_peaks != 3
  # Set up result table.
  Create Table with column names: "Formants", 'tSteps', { "'obj$'", "time", "F0"}
  for column to maxFor
    Append column: "F'column'"
    Append column: "q'column'"
  endfor
endif

totalCountF = 0
for tIndex to tSteps-1
  tc = tIndex*tStep
  # Valid pitch?
  select Pitch sound
  f0 = Get value at time: tc, "Hertz", "nearest"
  if f0 = undefined
    f0 = mPitch
    voice = 0
  else
    voice = 1
  endif
  if mark_formant_peaks != 3
    select Table Formants
    Set numeric value: 'tIndex', "time", 'tc:3'
    if voice = 0
      Set string value: 'tIndex', "F0", "u"
      else
      Set numeric value: 'tIndex', "F0", 'f0:0'
    endif
  endif

  # Extract 5 periods. Avoids vertical F0 stripes.
  select Sound sound
  part =  Extract part: tc-2.5/f0, tc+2.5/f0, "Hanning", 1, "no"

  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 Gauss 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"

  if mark_formant_peaks = 3
    goto FILL
  else
    if mark_formant_peaks = 2
      if voice = 0
        goto FILL
        endif
      endif
  endif

  To PointProcess (extrema): 1, "yes", "no", "Sinc70"
  n = 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
  for fIndex to n
    select PointProcess specData
    t = Get time from index... 'fIndex'

    if fIndex > 1
      tPrev = Get time from index... 'fIndex'-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 fIndex > maxFor
      goto SKIP
    endif

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

    label SKIP

  endfor ;fIndex

  select PointProcess specData
  plus Sound specDataDiff
  Remove

  label FILL

  for row to fSteps-1
    select Sound specData
    f = row*fStep
    v = Get value at time: 0, f, "cubic"

    select Matrix 'obj$'
    Set value: row, tIndex, v
  endfor

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

  select Matrix Formants
  column = tIndex
  # Exclude column number 0.
  if column = 0
    column = 1
  endif
  freq = 1
  row = 0

  for count to maxFor
    select Matrix Formants
    row += 1
    freq = Get value in cell: row, column
    if freq = 0
      goto AGAIN
    endif

    select Matrix q
    q = Get value in cell: row, column
    q = round(q*1000)/10

    select Table Formants
    Set numeric value: 'tIndex', "F'count'", 'freq:0'
    Set numeric value: 'tIndex', "q'count'", 'q:1'

    label AGAIN
  endfor ;count

endfor ;tIndex

select Matrix 'obj$'
To Spectrogram

select Sound sound
plus Pitch sound
plus Matrix 'obj$'
plus Matrix Formants
plus Matrix q
Remove

select Spectrogram 'obj$'
Paint: 0, 0, fMin, fMax, 100, "yes", 50, 0, 0, "yes"

if mark_formant_peaks != 3
  # Display formants.
  for i to totalCountF
    time = time'i'
    freq = freq'i'
    if freq < fMin
      goto NEXT
    endif
    radius = len/225
    Paint circle: "{0,0.9,0}", time, freq, radius
    label NEXT
  endfor
endif