// 1st order UHJ transcoder/decoder based on
// Gerzon, Michael A. (November 1985). 
// "Ambisonics in Multichannel Broadcasting and Video". 
// Journal of the Audio Engineering Society. AES. 33 (11): 859–871.
// http://decoy.iki.fi/dsound/ambisonic/motherlode/source/11730.pdf
// Allpass based, 90 degree phase shift filters are based on:
// https://www.mikrocontroller.net/attachment/33904/hilbert_Olli_Niemitalo.pdf

desc: WigWare UHJ Decoder/Transcoder

//include Ambisonic Filter Functions
import WigAmbFilter.jsfx-inc

//include decoder filenames
//filename:0,Wig_3o_Oct2_pic_1.png
//filename:1,Wig_3o_Oct2_pic_2.png
//filename:2,Wig_3o_Oct2_pic_3.png
filename:0,UoD_Logo_Small.png
filename:1,LoudspeakerClipArt.png

slider1:k_=0.0<0.0,0.7,0.05>Forward Preference
slider2:OutType=0<0,2,1{Quad,FuMa,AmbiX}>Output Quad Decode or B Format
slider3:spang=45.0<20,70,1>Speaker Angle (45 is square)
slider4:shelfenable=1<0,1,1{Off,On}>Shelf Filters
slider5:trf=400.0<100,3000,20>Transistion Frequency (Hz)
slider6:LFGain=0.0<-6.0,6.0,0.05>LF Decoder Gain (dB)
slider7:HFGain=0.0<-6.0,6.0,0.05>HF Decoder Gain (dB)
slider8:DistFiltEnable=0<0,2,1{Off,Speaker Distance Comp (SDC),SDC & NFC}>Distance Filtering
slider9:SpDist=4<0.5,4,0.05>Speaker Distance
slider10:NFCDist=4<0.5,4,0.05>Near-Field Comp Distance

in_pin:Left
in_pin:Right
in_pin:T (not implemented yet)
in_pin:Q (not implemented yet)
out_pin:FL
out_pin:FR
out_pin:BL
out_pin:BR

@init
bpos=0;
sum_0 = 0.0;
dif_0 = 0.0;

length_of_csps = 4;
F1coef = 1024;
end_of_csps = F1coef + length_of_csps;
F2coef = end_of_csps;
end_of_csps = F2coef + length_of_csps;
csp1 = end_of_csps;
end_of_csps = csp1 + length_of_csps;
csp2 = end_of_csps;
end_of_csps = csp2 + length_of_csps;
csp3 = end_of_csps;
end_of_csps = csp3 + length_of_csps;
csp4 = end_of_csps;
end_of_csps = csp4 + length_of_csps;
spangar = end_of_csps;
end_of_csps = spangar + length_of_csps;
k1 = end_of_csps;
k2 = k1+2;
k3 = k2+2;

F1coef[0] = 0.6923878;      F1coef[1] = 0.9360654322959;F1coef[2] = 0.9882295226860;F1coef[3] = 0.9987488452737;
F2coef[0] = 0.4021921162426;F2coef[1] = 0.8561710882420;F2coef[2] = 0.9722909545651;F2coef[3] = 0.9952884791278;

csp1[0] = 1.0;csp1[1] = csp1[2] = csp1[3] = 0.0;
csp2[0] = 1.0;csp2[1] = csp2[2] = csp2[3] = 0.0;
csp3[0] = 1.0;csp3[1] = csp3[2] = csp3[3] = 0.0;
csp4[0] = 1.0;csp4[1] = csp4[2] = csp4[3] = 0.0;

spangar[0] = spangar[1] = spangar[2] = spangar[3] = 0.0;

//k1[0] = 1.000;k1[1] = 1.000;
//k2[0] = 1.000;k2[1] = 1.000;
//k3[0] = 1.000;k3[1] = 1.000;
k1[0] = 0.646;k1[1] = 1.000;
k2[0] = 1.263;k2[1] = 1.000;
k3[0] = 0.775;k3[1] = 1.000;

F1a.SOS_INIT();
F1b.SOS_INIT();
F1c.SOS_INIT();
F1d.SOS_INIT();
F2a.SOS_INIT();
F2b.SOS_INIT();
F2c.SOS_INIT();
F2d.SOS_INIT();
F3a.SOS_INIT();
F3b.SOS_INIT();
F3c.SOS_INIT();
F3d.SOS_INIT();
F4a.SOS_INIT();
F4b.SOS_INIT();
F4c.SOS_INIT();
F4d.SOS_INIT();


function SetDistCoef(dNFC,dspeak) local(w1,w2)
(
	NFCDist == 4.0? w1 = 0 :(
		w1 = 342.0/(dNFC*srate);
	);
	w2 = 342.0/(dspeak*srate);
	NF1.NF_Filt1Init(w1,w2,1.0);
	NF2.NF_Filt1Init(w1,w2,1.0);
	NF3.NF_Filt1Init(w1,w2,1.0);
);

SF0.shelfinit(srate,400.0,1.0,1.0,1);
SF1.shelfinit(srate,400.0,1.0,1.0,1);
SF2.shelfinit(srate,400.0,1.0,1.0,1);
SF3.shelfinit(srate,400.0,1.0,1.0,1);
NF1.NF_Filt1Init(342.0/(NFCDist*srate),342.0/(SpDist*srate),1.0);
NF2.NF_Filt1Init(342.0/(NFCDist*srate),342.0/(SpDist*srate),1.0);
NF3.NF_Filt1Init(342.0/(NFCDist*srate),342.0/(SpDist*srate),1.0);

sum_0 = dif_0 = 0.0;

@slider
//Filter Gains
LFGainLin = 2 ^(LFGain/6);
HFGainLin = 2 ^(HFGain/6);
//Do shelf filtering
SF0.shelfinit(srate,trf,k1[0]*LFGainLin,k1[1]*HFGainLin,0);
SF1.shelfinit(srate,trf,k2[0]*LFGainLin,k2[1]*HFGainLin,0);
SF2.shelfinit(srate,trf,k2[0]*LFGainLin,k2[1]*HFGainLin,0);
SF3.shelfinit(srate,trf,k3[0]*LFGainLin,k3[1]*HFGainLin,0);
SetDistCoef(NFCDist,SpDist);
//Recalculate decoder
spangr = spang*$pi/180.0;     //convert to rad
spangar[0] = spangr;          //FL
spangar[1] = -spangr;         //FR
spangar[2] = $pi-spangr;      //BL
spangar[3] = -$pi+spangr;     //BR
dbg = spangar[3];

//dbg = spangar[3];

csp1[1] = 1/(sqrt(2.0)*cos(spangar[0]));
csp2[1] = 1/(sqrt(2.0)*cos(spangar[1]));
csp3[1] = 1/(sqrt(2.0)*cos(spangar[2]));
csp4[1] = 1/(sqrt(2.0)*cos(spangar[3]));

csp1[2] = 1/(sqrt(2.0)*sin(spangar[0]));
csp2[2] = 1/(sqrt(2.0)*sin(spangar[1]));
csp3[2] = 1/(sqrt(2.0)*sin(spangar[2]));
csp4[2] = 1/(sqrt(2.0)*sin(spangar[3]));

@sample
sum = 0.5*(spl0+spl1);
dif = 0.5*(spl0-spl1);

sum0 = sum;
sum90 = sum;
dif0 = dif;
dif90 = dif;
//cnt = 0;
//loop(4,
//     sum0 = F1.SOS_AP(sum0,F1c[cnt]);
//     sum90 = F2.SOS_AP(sum90,F2c[cnt]);
//     dif0 = F3.SOS_AP(dif0,F1c[cnt]);
//     dif90 = F4.SOS_AP(dif90,F2c[cnt]);
//     cnt = cnt + 1;
//);
//Allpass 0 and +90 degree filters
	sum0 = F1a.SOS_AP(sum0,F1coef[0]);
	sum0 = F1b.SOS_AP(sum0,F1coef[1]);
	sum0 = F1c.SOS_AP(sum0,F1coef[2]);
	sum0 = F1d.SOS_AP(sum0,F1coef[3]);
	sum90 = F2a.SOS_AP(sum90,F2coef[0]);
	sum90 = F2b.SOS_AP(sum90,F2coef[1]);
	sum90 = F2c.SOS_AP(sum90,F2coef[2]);
	sum90 = F2d.SOS_AP(sum90,F2coef[3]);
	dif0 = F3a.SOS_AP(dif0,F1coef[0]);
	dif0 = F3b.SOS_AP(dif0,F1coef[1]);
	dif0 = F3c.SOS_AP(dif0,F1coef[2]);
	dif0 = F3d.SOS_AP(dif0,F1coef[3]);
	dif90 = F4a.SOS_AP(dif90,F2coef[0]);
	dif90 = F4b.SOS_AP(dif90,F2coef[1]);
	dif90 = F4c.SOS_AP(dif90,F2coef[2]);
	dif90 = F4d.SOS_AP(dif90,F2coef[3]);

W =  0.982*sum_0 + 0.164*dif90;
X =  0.419*sum_0 - 0.828*dif90; 
Y =  0.385*sum90 + 0.763*dif_0;
B = -0.694*sum90 + 0.116*dif_0;

//delay by one sample
sum_0 = sum0;
dif_0 = dif0;

//FuMa B-Format as per Gerzon Paper
shelfenable ? (
BF[0] = SF0.shelf(W);
BF[1] = SF1.shelf(X);
BF[2] = SF2.shelf(Y) + k_ * SF3.shelf(B);
BF[3] = 0.0; 
dbg = 1;
) : (
BF[0] = W;
BF[1] = X;
BF[2] = Y + k_ * B;
BF[3] = 0.0;
dbg=0;
);

DistFiltEnable!=0? (
	DistFiltEnable==1?(
		BF[1]=NF1.NF_Filt1ProcessSpDist(BF[1]);
		BF[2]=NF2.NF_Filt1ProcessSpDist(BF[2]);
		BF[3]=NF3.NF_Filt1ProcessSpDist(BF[3]);
	) : (
		BF[1]=NF1.NF_Filt1Process(BF[1]);
		BF[2]=NF2.NF_Filt1Process(BF[2]);
		BF[3]=NF3.NF_Filt1Process(BF[3]);
	);
);

OutType==0 ? 
(
	//quad decode
	spl0=spl1=spl2=spl3=0.0;
	
	cnt = 0;
	loop(3,
		spl0+=BF[cnt]*csp1[cnt]*0.5;
		spl1+=BF[cnt]*csp2[cnt]*0.5;
		spl2+=BF[cnt]*csp3[cnt]*0.5;
		spl3+=BF[cnt]*csp4[cnt]*0.5;
		cnt = cnt+1;
	);
) : (
	OutType==1 ? 
	(
		//FuMa
		spl0 = BF[0]*0.70712;  //W -3dB
		spl1 = BF[1];  //X
		spl2 = BF[2];  //Y
		spl3 = BF[3];  //Z
	) : (
		//AmbiX
		spl0 = BF[0];    //W
		spl1 = BF[2];    //Y
		spl2 = BF[3];    //Z
		spl3 = BF[1];    //X
	);
);

@gfx 480 220
gfx_getimgdim(1, spgfx_w, spgfx_h);
gfx_r = 1.0;gfx_g =1.0;gfx_b = 1.0;
gfx_x = 3;
gfx_y = 3;
gfx_drawstr("WigWare Stereo UHJ to Loudspeakers...");
gfx_x = 3;
gfx_y += gfx_texth+3;
gfx_drawstr("...or B-Format decoder/transcoder");

//gfx_x = 0;
//gfx_y+=gfx_texth+3;
//gfx_blit(0, 1.0, 0.0);
//gfx_x+=245;
//gfx_blit(1,1.0,0.0);
//gfx_x+=245;
//gfx_blit(2,1.0,0.0);
gfx_x = 350;
gfx_y = 5;
gfx_blit(0, 1.0, 0.0);

spcentx = 120;
spcenty = 100;
sprad = 80;

gfx_line(spcentx+21,spcenty-sprad+21,spcentx+21,spcenty+sprad+21);
gfx_line(spcentx-sprad+21,spcenty+21,spcentx+sprad+21,spcenty+21);
gfx_x = spcentx;
gfx_y = spcenty-sprad+12;
gfx_drawstr("Front");
ban = 0;
loop(4,
	gfx_y = spcenty-(cos(-spangar[ban])*sprad);
	gfx_x = spcentx+(sin(-spangar[ban])*sprad);
	gfx_blit(1,1.0,-spangar[ban]);
	ban+=1;
);


