#ifndef UDO_CHORDDETECT
#define UDO_CHORDDETECT ##
/*
	Polyphonic note tracking

	This file is part of the SONICS UDO collection by Richard Knight 2023
		License: GPL-2.0-or-later
		http://1bpm.net
*/



gichorddetect_ampthresh = 0.01

/*
	Internal opcode
	Set up pvsbin recursively for each bin up to imaxbin, and add a confirmation to inotefn for the nearest MIDI note detected

	_polydetect_bin fsig, imaxbin, inotefn [, ibin=1]

	fsig		the signal from pvsanal to perform pitch detection on
	imaxbin		maximum number of bins in the fsig which denotes the level of recursion required
	inotefn		ftable to store note confirmations in
	ibin		current bin for pvsbin to examine
*/
opcode _polydetect_bin, 0, fiip
	fsig, imaxbin, inotefn, ibin xin
	kamp, kfreq pvsbin fsig, ibin
	
	if (kamp > gichorddetect_ampthresh) then
		knote = ftom(kfreq, 1)
		tablew table:k(knote, inotefn) + 1, knote, inotefn
	endif

	if (ibin + 1 < imaxbin) then
		_polydetect_bin fsig, imaxbin, inotefn, ibin + 1
	endif
endop


/*
	Internal opcode
	Rank the notes in inotefn and output the detected MIDI notes along with a certainty ratio

	kout[], kcertainty[] _polydetect_ranknotes inotefn, imaxnotes

	kout[]			array of detected MIDI note numbers, of size imaxnotes
	kcertainty[]	array of detected note certainty ratios, of size imaxnotes
	inotefn			ftable with note confirmations
	imaxnotes		maximum number of notes to look for, ie polyphony level
*/
opcode _polydetect_ranknotes, k[]k[], ii
	inotefn, imaxnotes xin
	ilen = ftlen(inotefn)
	kout[] init imaxnotes
	
	kcertainty[] init imaxnotes
	ktotal = 0
	kindex = 0
	kmax = 0
	knonzeronum = 0
	while (kindex < ilen) do
		kval = tab:k(kindex, inotefn)
		if (kval > 0) then
			ktotal += kval
			knonzeronum += 1
		endif
		kindex += 1
	od

	if (knonzeronum != 0) then
		kavg = ktotal / knonzeronum
		knum = 0
		kindex = 0
		while (kindex < ilen) do
			kval = tab:k(kindex, inotefn)
			kmax = max:k(kmax, kval)
			if (kval > kavg) then
				knum += 1
				if (knum >= imaxnotes) then
					goto writeout
				endif
			endif
			kindex += 1
		od
	else
		goto complete
	endif

writeout:
	kindex = 0
	kwriteindex = 0
	while (kindex < ilen) do
		kval = tab:k(kindex, inotefn)
		if (kval > kmax/2) then ;kavg) then
			kout[kwriteindex] = kindex
			kcertainty[kwriteindex] = kval / kmax
			kwriteindex += 1
			if (kwriteindex >= imaxnotes) then
				goto complete
			endif
		endif
		kindex += 1
	od
complete:
	xout kout, kcertainty
endop


/*
	Detect the nearest MIDI notes in an audio signal to an arbitrary level of polyphony

	kchanged, kout[], kcertainty[] polydetect ain, imaxnotes, iupdateksmps
	
	kchanged		trigger output as 1 if the detected notes has changed from the last output
	kout[]			array of detected MIDI note numbers, of size imaxnotes
	kcertainty[]	array of detected note certainty ratios, of size imaxnotes
	ain				input audio to examine
	imaxnotes		maximum number of notes to look for, ie polyphony level
	iupdateksmps	analysis window size in ksmps
*/
opcode polydetect, kk[]k[], aii
	ain, imaxnotes, iupdateksmps xin
	ir = 1024
	inotefn ftgentmp 0, 0, 128, 2, 0
	ktimek timeinstk

	fsig pvsanal ain, ir, ir/4, ir, 1
	_polydetect_bin fsig, ir/8, inotefn ; quarter of spectrum

	klast[] init imaxnotes
	kchanged = 0

	if (ktimek % iupdateksmps == 0) then
		kout[], kcertainty[] _polydetect_ranknotes inotefn, imaxnotes
		
		kcompare[] cmp klast, "==", kout

		if (sumarray(kcompare) == lenarray(kcompare)) then
			kchanged = 0
		else
			kchanged = 1
			klast = kout
		endif

		ftset inotefn, k(0)
	endif
	xout kchanged, kout, kcertainty
endop




#end