text:
module Test_afdsyn

using FaultDetectionTools
using DescriptorSystems
using LinearAlgebra
using Polynomials
using Random
using Test

# Testing examples for EFDSYN
println("Test_afdsyn")
@testset "afdsyn" begin
rand(2);

## Example without control and disturbance inputs
p = 3; mf = 2;
sysf = fdimodset(rss(1,p,mf,stable=true),faults = 1:mf);

# solve an EFDP as an exact AFDP (S provided)
S = fditspec(sysf)
@time Q, Rf, info = afdredsyn(sysf; exact = true, atol = 1.e-7, minimal = false, rdim = 2, S); info

R = fdIFeval(Q, sysf);
@test iszero(Rf.sys[:,Rf.faults]-R.sys[:,R.faults],atol=1.e-7) &&
      info.HDesign == [1.0 0.0 0.0; 0.0 1.0 0.0] && info.gap == Inf &&
      info.gap ≈ fdif2ngap(R)[1] && isstable(Q.sys) && fdimmperf(R,Rf) < 1.e-7

# solve an EFDP as an exact AFDP (S internally computed)
@time Q, Rf, info = afdredsyn(sysf; exact = true, atol = 1.e-7, minimal = false, rdim = 2); info

R = fdIFeval(Q, sysf);
@test iszero(Rf.sys[:,Rf.faults]-R.sys[:,R.faults],atol=1.e-7) &&
      info.HDesign == [1.0 0.0 0.0; 0.0 1.0 0.0] && info.gap == Inf &&
      info.gap ≈ fdif2ngap(R)[1] && isstable(Q.sys)

# solve an EFDP as an exact AFDP
@time Q, Rf, info = afdsyn(sysf; exact = true, atol = 1.e-7, minimal = false, rdim = 2); info

R = fdIFeval(Q, sysf);
@test iszero(Rf.sys[:,Rf.faults]-R.sys[:,R.faults],atol=1.e-7) &&
      info.HDesign2 == [0.0 1.0 0.0; 0.0 0.0 1.0] && info.gap == Inf &&
      info.gap ≈ fdif2ngap(R)[1] && isstable(Q.sys)

## Example with fault and noise inputs: Rw non full-row rank
p = 3; mf = 2; mw = 2
sysf = fdimodset(rss(1,p,mf+mw,stable=true),noise = 1:mw, faults = mw+1:mw+mf);
S = fditspec_(sysf.sys[:,sysf.faults])

# solve an EFDP with noise inputs as an exact AFDP: rdim = 2
@time Q, Rfw, info = afdredsyn(sysf; exact = true, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 2, S); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.HDesign ==  [1.0 0.0 0.0; 0.0 1.0 0.0] &&
      info.gap ≈ fdif2ngap(R)[1] && isstable(Q.sys) && order(Q.sys) == 1 && fdimmperf(R,Rfw) < 1.e-7 &&
      fdimmperf(FDFilterIF(Rfw.sys[:,Rfw.noise],0,0,0,length(Rfw.noise))) ≈ fdimmperf(R,FDFilterIF(Rfw.sys[:,Rfw.faults],0,0,length(Rfw.faults)))

@time Q, Rfw, info = afdredsyn(sysf; exact = false, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 2, S); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.HDesign ==  [1.0 0.0 0.0; 0.0 1.0 0.0] &&
      info.gap ≈ fdif2ngap(R)[1] && isstable(Q.sys) && order(Q.sys) == 1

@time Q, Rfw, info = afdsyn(sysf; exact = true, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 2); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.HDesign ==  [0.0 1.0 0.0; 0.0 0.0 1.0] &&
      info.gap ≈ fdif2ngap(R)[1] && isstable(Q.sys) && order(Q.sys) == 0

@time Q, Rfw, info = afdsyn(sysf; exact = false, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 2); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.HDesign ==  [0.0 1.0 0.0; 0.0 0.0 1.0] &&
      info.gap ≈ fdif2ngap(R)[1] && isstable(Q.sys) && order(Q.sys) == 0

## Example with fault and noise inputs: Rw full-row rank
p = 3; mf = 2; mw = 3
sysf = fdimodset(rss(1,p,mf+mw,stable=true),noise = 1:mw, faults = mw+1:mw+mf);
S = fditspec_(sysf.sys[:,sysf.faults])

# solve an EFDP with noise inputs as an exact AFDP: rdim = 1
@time Q, Rfw, info = afdredsyn(sysf; exact = true, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 1, S); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.gap ≈ fdif2ngap(R)[1]
te1 = info.gap

# solve an EFDP with noise inputs as an exact AFDP: rdim = 1
@time Q, Rfw, info = afdsyn(sysf; exact = true, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 1); info
R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.gap ≈ fdif2ngap(R)[1]

# solve an EFDP with noise inputs as an exact AFDP: rdim = 2
@time Q, Rfw, info = afdredsyn(sysf; exact = true, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 2, S); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.gap ≈ fdif2ngap(R)[1]
te2 = info.gap

# solve an EFDP with noise inputs as an exact AFDP: rdim = 2
@time Q, Rfw, info = afdsyn(sysf; exact = true, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 2); info
R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.gap ≈ fdif2ngap(R)[1]

# solve an EFDP with noise inputs as an exact AFDP: rdim = 3
@time Q, Rfw, info = afdredsyn(sysf; exact = true, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 3, S); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.HDesign == [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0] &&
      info.gap ≈ fdif2ngap(R)[1]
te3 = info.gap

# solve an AFDP with noise inputs: rdim = 1
@time Q, Rfw, info = afdredsyn(sysf; exact = false, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 1, S); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.HDesign == [1.0 0.0 0.0] &&
      info.gap ≈ fdif2ngap(R)[1]
ta1 = info.gap

# solve an AFDP with noise inputs: rdim = 2
@time Q, Rfw, info = afdredsyn(sysf; exact = false, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 2, S); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.HDesign == [1.0 0.0 0.0; 0.0 1.0 0.0] &&
      info.gap ≈ fdif2ngap(R)[1]
ta2 = info.gap

# solve an AFDP with noise inputs: rdim = 3
@time Q, Rfw, info = afdredsyn(sysf; exact = false, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 3, S); info

R = fdIFeval(Q, sysf)
@test iszero(Rfw.sys-R.sys,atol=1.e-7) &&
      info.HDesign == [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0] &&
      info.gap ≈ fdif2ngap(R)[1]
ta3 = info.gap
@test (te1 <= ta1 || te1 ≈ ta1) && te2 <= ta2 && te3 <= ta3

# tests for non-standard cases

s = rtf('s')
Rf = (s+1)/(s+2); Rw = 1/(s+2); # low pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
α = 3;
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 1, sdeg = -α, atol = 1.e-7); info
@test info.gap ≈ α ≈ fdif2ngap(Rfw)[1] && gpole(Q.sys) ≈ [-α] &&
      ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = (s+1)/(s+2); Rw = 1/(s+2); # low pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
epsreg = 0.3;
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 2, epsreg, atol = 1.e-7); info
@test info.gap ≈ fdif2ngap(Rfw)[1] && gpole(Q.sys) ≈ [-info.gap]  &&
      ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = (s+1)/(s+2); Rw = 1/(s+2); # low pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 3, atol = 1.e-7); info
@test info.gap ≈ fdif2ngap(Rfw)[1] && gpole(Q.sys) ≈ gpole(sysf.sys) &&
      ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = (s+1)/(s+2); Rw = 1/(s+2); # low pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
sdegzer = -0.1;
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 4, sdegzer, atol = 1.e-7); info
@test info.gap ≈ fdif2ngap(Rfw)[1] && sort(gpole(Q.sys)) ≈ sort([gpole(sysf.sys); [1/sdegzer]]) &&
      ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = (s+1)/(s+2); Rw = 1/(s+2); # low pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
epsreg = 0.3;
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 5, epsreg, atol = 1.e-7); info
@test info.gap ≈ fdif2ngap(Rfw)[1] && gpole(Q.sys) ≈ [-info.gap] &&
      ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = (s+1)/(s+2); Rw = s/(s+2); # high pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
ϵ = 0.3;
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 1, sdeg = -ϵ, atol = 1.e-7); info
@test info.gap ≈ 1/ϵ ≈ fdif2ngap(Rfw)[1] && gpole(Q.sys) ≈ [-ϵ] &&
      ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = (s+1)/(s+2); Rw = s/(s+2); # high pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
epsreg = 0.3;
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 2, epsreg, atol = 1.e-7); info
@test info.gap ≈ fdif2ngap(Rfw)[1] && ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = (s+1)/(s+2); Rw = s/(s+2); # high pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 3, atol = 1.e-7); info
@test info.gap ≈ fdif2ngap(Rfw)[1] && gpole(Q.sys) ≈ gpole(sysf.sys) &&
      ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = (s+1)/(s+2); Rw = s/(s+2); # high pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
sdegzer = -0.1;
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 4, sdegzer, atol = 1.e-7); info
@test info.gap ≈ fdif2ngap(Rfw)[1] && sort(gpole(Q.sys)) ≈ sort([gpole(sysf.sys); [sdegzer]]) &&
      ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = (s+1)/(s+2); Rw = s/(s+2); # high pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1, noise = 2)
epsreg = 0.3;
@time Q, Rfw, info = afdredsyn(sysf; nonstd = 5, epsreg, atol = 1.e-7); info
@test info.gap ≈ fdif2ngap(Rfw)[1] && ghinfnorm(Rfw.sys[:,2])[1] ≈ 1

s = rtf('s')
Rf = [(s+1)/(s+2) 1]; Rw = 1/(s+2); # low pass filter
sysf = fdimodset(dss([Rf Rw]; minimal = true),faults = 1:2, noise = 3)
α = 3;
@time Q, Rfw, info = afdredsyn(sysf; sdeg = -α, atol = 1.e-7); info
@test info.gap ≈ α  && ghinfnorm(Rfw.sys[:,3])[1] ≈ 1

## Example with fault and noise inputs: Rw non full-row rank
p = 3; mf = 2; mw = 2
sysf = fdimodset(rss(1,p,mf+mw,stable=true),noise = 1:mw, faults = mw+1:mw+mf);

# solve an EFDP with noise inputs as an exact AFDP: rdim = 2
@time Q1, Rfw1, info1 = efdsyn(sysf; FDtol = 0.00001, atol = 1.e-7, minimal = false); info1
@time Q2, Rfw, info = afdredsyn(Rfw1; exact = false, FDtol = 0.00001, atol = 1.e-7, minimal = false, rdim = 2, S = info1.S, degs = reverse(info1.degs)); info

R = fdIFeval(Q2*Q1, sysf);
@test iszero(Rfw.sys[:,Rfw.faults]-R.sys[:,R.faults],atol=1.e-7) &&
      info.HDesign == [0.0 1.0 0.0; 0.0 0.0 1.0]  &&
      info.gap ≈ fdif2ngap(R)[1]

# solve an EFDP with noise inputs as an exact AFDP: rdim = 2
@time Q1, Rfw1, info1 = efdsyn(sysf; FDtol = 0.00001, atol = 1.e-7, minimal = false); info1
@time Q2, Rfw, info = afdredsyn(Rfw1; exact = false, FDtol = 0.00001, atol = 1.e-7, minimal = true, rdim = 2, S = info1.S, degs = reverse(info1.degs)); info

R = fdIFeval(Q2*Q1, sysf);
@test iszero(Rfw.sys[:,Rfw.faults]-R.sys[:,R.faults],atol=1.e-7) &&
      info.HDesign == [0.0 1.0 0.0; 0.0 0.0 1.0] &&
      info.gap ≈ fdif2ngap(R)[1]

# Example 5.6c - Solution of an AFDP using AFDSYN

# define s as an improper transfer function
s = rtf('s');
# define Gu(s), Gf(s) and Gw(s)
Gu = [(s+1)/(s+2); (s+2)/(s+3)];     # enter Gu(s)
Gw = [1/(s+2); 0];                   # enter Gw(s)
Gf = [(s+1)/(s+2) 0; 0 1];           # enter Gf(s)
p = 2; mu = 1; mw = 1; mf = 2;       # enter dimensions

# setup the synthesis model
sysf = fdimodset(dss([Gu Gf Gw],minimal = true),c =1:mu, f = mu .+ (1:mf), n = (mu+mf) .+ (1:mw));

# call of AFDSYN with the options for stability degree -3 and the synthesis
# of a scalar output filter
Q, Rfw, info = afdsyn(sysf, atol = 1.e-7, minimal = false, nullspace = false, smarg = -2, sdeg = -3,
                      rdim = 1, HDesign = [1 1]); info

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7)
@test iszero(R.sys[:,[R.controls; R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,[R.faults;R.noise]]-Rfw.sys[:,[Rfw.faults;Rfw.noise]]) &&
      fdisspec(Rfw, block = true) == Bool[1 1] &&
      maximum(real(gpole(Q))) >= -3 && minimum(real(gpole(Q))) <= -3 &&
      fdif2ngap(R)[1] ≈ info.gap

# alternative least order synthesis
# call of AFDSYN with the options for stability degree -3 and the synthesis
# of a scalar output filter; the choice of HDesign preserves the achieved gap of 3
Q, Rfw, info = afdsyn(sysf, atol = 1.e-7, smarg = -3, sdeg = -3,
         HDesign = [0.1 1]); info

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7)
@test iszero(R.sys[:,[R.controls; R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,[R.faults;R.noise]]-Rfw.sys[:,[Rfw.faults;Rfw.noise]]) &&
      fdisspec(Rfw, block = true) == Bool[1 1] &&
      gpole(Q) ≈ [-3] &&
      fdif2ngap(R)[1] ≈ info.gap

# Example 5.9 - Solution of an AFDP using AFDSYN with rdm = 2

# define s as an improper transfer function
s = rtf('s');
# define Gu(s), Gf(s) and Gw(s)
Gu = [(s+1)/(s+2); (s+2)/(s+3)];     # enter Gu(s)
Gw = [1/(s+2); 0];                   # enter Gw(s)
Gf = [(s+1)/(s+2) 0; 0 1];           # enter Gf(s)
p = 2; mu = 1; mw = 1; mf = 2;       # enter dimensions

# setup the synthesis model
sysf = fdimodset(dss([Gu Gf Gw],minimal = true),c =1:mu, f = mu .+ (1:mf), n = (mu+mf) .+ (1:mw));

# call of AFDSYN with the options for stability degree -3 and the synthesis
# of a scalar output filter
Q, Rfw, info = afdsyn(sysf, atol = 1.e-7, minimal = true, nullspace = false, smarg = -2, sdeg = -2, poles = [-2,-3],
                      rdim = 2, HDesign = [1 0]); info

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7)
@test iszero(R.sys[:,[R.controls; R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,[R.faults;R.noise]]-Rfw.sys[:,[Rfw.faults;Rfw.noise]]) &&
      fdisspec(Rfw, block = false) == [info.S; info.S2] &&
      fdif2ngap(Rfw)[1] ≈ info.gap

## Example with solvable strong synthesis (frequencies are also zeros)
#
s = rtf('s');
# define 2x1 Gf(s) without zeros
Gf = [s/(s+1) 0; (s^2+4)/(s+3)^2 1]; # enter Gf(s)

# build minimal realzation of the model with faults
sysf = fdimodset(dss(Gf; minimal = true, atol = 1.e-7),faults = 1);

@time Q, Rf, info = afdsyn(sysf; sdeg = -3, rdim = 1, FDfreq = [0, 2]); info

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0, 2])[:] == Bool[1]

# the same results  must result using the resulting design matrix H
Q1, Rf1, info1 = afdsyn(sysf; sdeg = -3, rdim = 1, FDfreq = [0, 2], FDGainTol = 0.001, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys,atol=1.e-7) && iszero(Rf.sys-Rf1.sys,atol=1.e-7)

sysf = fdimodset(dss(Gf; minimal = true, atol = 1.e-7),faults = 1:2);
@time Q, Rf, info = afdsyn(sysf; sdeg = -3, rdim = 2, FDfreq = [0, 2]); info
# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0, 2]; block = false) == Bool[0 1; 0 0] &&
      fdisspec(Rf, [0]; block = false) == Bool[1 1; 0 0] &&
      fdisspec(Rf, [2]; block = false) == Bool[0 1; 1 0] &&
      fdisspec(Rf, [0, 2]; block = true) == Bool[1 1]

## Example with solvable strong synthesis (frequencies are also zeros)
#  Using simple basis based synthesis
#
s = rtf('s');
# define 2x1 Gf(s) without zeros
Gf = [s/(s+1) 0; (s^2+4)/(s+3)^2 1]; # enter Gf(s)

# build minimal realzation of the model with faults
sysf = fdimodset(dss(Gf; minimal = true, atol = 1.e-7),faults = 1);

@time Q, Rf, info = afdsyn(sysf; sdeg = -3, rdim = 1, FDfreq = [0, 2], simple = true); info

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0, 2])[:] == Bool[1]

Q1, Rf1, info1 = afdsyn(sysf; sdeg = -3, rdim = 1, FDfreq = [0, 2], FDGainTol = 0.001, simple = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys,atol=1.e-7) && iszero(Rf.sys-Rf1.sys,atol=1.e-7)

# example with a weak AFDIP
s = rtf('s');
# define 1x2 Gf(s) without zeros
Gf = [1/s 1/(s^2+4)]; # enter Gf(s)

# build minimal realzation of the model with faults
sysf = fdimodset(dss(Gf; minimal = true, atol = 1.e-7), faults = 1:2);

@time Q, Rf, info = afdsyn(sysf, Bool[1, 0]; sdeg = -3, rdim = 1, FDfreq = [0]); info
# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0])[:] == Bool[1, 0] &&
      fdif2ngap(Rf,Bool[1, 0],0)[1] ≈ info.gap ≈ fdif2ngap(R,Bool[1, 0],0)[1] &&
      fdimmperf(Rf) == 0 && fdimmperf(Rf,Bool[1,0]) ≈ 1

Q1, Rf1, info1 = afdsyn(sysf, Bool[1, 0]; sdeg = -3, rdim = 1, FDfreq = [0], FDGainTol = 0.001, HDesign = info.HDesign); info1

@test iszero(Q.sys-Q1.sys,atol=1.e-7) && iszero(Rf.sys-Rf1.sys,atol=1.e-7)

## Example with solvable strong synthesis (frequencies are also poles)
#
s = rtf('s');
# define 2x1 Gf(s) without zeros
Gf = [1/s; 1/(s^2+4)]; # enter Gf(s)

# build minimal realzation of the model with faults
sysf = fdimodset(dss(Gf; minimal = true, atol = 1.e-7),faults = 1);

@time Q, Rf, info = afdsyn(sysf; sdeg = -3, rdim = 1, FDfreq = [0]); info
# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0, 2])[:] == Bool[1]

Q1, Rf1, info1 = afdsyn(sysf; sdeg = -3, rdim = 1, FDfreq = [0, 2], FDGainTol = 0.001, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys,atol=1.e-7) && iszero(Rf.sys-Rf1.sys,atol=1.e-7)

## Example with solvable strong synthesis (frequencies are also poles)
#  Using simple basis based synthesis
s = rtf('s');
# define 2x1 Gf(s) without zeros
Gf = [1/s; 1/(s^2+4)]; # enter Gf(s)

@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, smarg = -3, sdeg = -3, rdim = 1, FDfreq = [0, 2], simple = true); info
# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0, 2])[:] == Bool[1]

Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, smarg = -3, sdeg = -3, rdim = 1, FDfreq = [0, 2], FDGainTol = 0.001, simple = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys,atol=1.e-7) && iszero(Rf.sys-Rf1.sys,atol=1.e-7)

## Example with solvable strong synthesis (frequencies are also zeros)
#
s = rtf('s');
# define 2x1 Gf(s) without zeros
Gf = [s; s^2+4]; # enter Gf(s)

# build minimal realzation of the model with faults
sysf = fdimodset(dss(Gf; minimal = true, atol = 1.e-7),faults = 1);

@time Q, Rf, info = afdsyn(sysf; sdeg = -3, smarg = -3, rdim = 1, FDfreq = [0, 2]); info

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0, 2])[:] == Bool[1]

Q1, Rf1, info1 = afdsyn(sysf; sdeg = -3, smarg = -3, rdim = 1, FDfreq = [0, 2], HDesign2 = info.HDesign2); info1
@test iszero(Q.sys-Q1.sys,atol=1.e-7) && iszero(Rf.sys-Rf1.sys,atol=1.e-7)

@time Q, Rf, info = afdsyn(sysf; sdeg = -3, smarg = -3, rdim = 1, FDfreq = [0, 2], simple = true); info

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0, 2])[:] == Bool[1]

Q1, Rf1, info1 = afdsyn(sysf; sdeg = -3, smarg = -3, rdim = 1, FDfreq = [0, 2], simple = true, HDesign2 = info.HDesign2); info1
@test iszero(Q.sys-Q1.sys,atol=1.e-7) && iszero(Rf.sys-Rf1.sys,atol=1.e-7)

## Example with solvable strong synthesis (frequencies are also zeros)
#  Using simple basis based synthesis
#
s = rtf('s');
# define 2x1 Gf(s) without zeros
Gf = [s; s^2+4]; # enter Gf(s)

# build minimal realzation of the model with faults
sysf = fdimodset(dss(Gf; minimal = true, atol = 1.e-7),faults = 1);

@time Q, Rf, info = afdsyn(sysf; sdeg = -3, smarg = -3, rdim = 1, FDfreq = [0, 2], simple = true); info

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0, 2])[:] == Bool[1]

Q1, Rf1, info1 = afdsyn(sysf; sdeg = -3, smarg = -3, rdim = 1, FDfreq = [0, 2], simple = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys,atol=1.e-7) && iszero(Rf.sys-Rf1.sys,atol=1.e-7)

# Example 5.3 - Solution of an EFDP using AFDSYN

# define s as an improper transfer function
s = rtf('s');
# define Gu(s) and Gd(s)
Gu = [(s+1)/(s+2); (s+2)/(s+3)];     # enter Gu(s)
Gd = [(s-1)/(s+2); 0];               # enter Gd(s)
p = 2; mu = 1; md = 1; mf = 2;       # set dimensions

# setup the synthesis model with additive faults with Gf(s) = [ Gu(s) [0;1]]
sysf = fdimodset(dss([Gu Gd]),c =1,d = 2,f = 1,fs = 2);

# call of EFDSYN with the options for stability degree -3 and the synthesis
# of a scalar output filter
@time Q, Rf, info = afdsyn(sysf, smarg =-3, sdeg = -3, rdim = 1);

# normalize Q and Rf to match example
scale = evalfr(Rf.sys[1,1],Inf)[1,1];
dss2rm(Q.sys/scale, atol = 1.e-7)
dss2rm(Rf.sys/scale, atol = 1.e-7)

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0]) == Bool[1 1]

# Example 5.4m - Solution of an EFDP using AFDSYN

# define s as an improper transfer function
s = rtf('s');
# define Gu(s) and Gd(s)
Gu = [(s+1)/(s-2); (s+2)/(s-3)];     # enter Gu(s)
Gd = [(s-1)/(s+2); 0];               # enter Gd(s)
Gf = [(s+1)/(s-2) 0; (s+2)/(s-3) 1]; # enter Gf(s)
p = 2; mu = 1; md = 1; mf = 2;       # set dimensions

# setup the synthesis model with faults with Gf(s) = [ Gu(s) [0;1]]
sysf = fdimodset(dss([Gu Gd]),c =1,d = 2,f = 1,fs = 2);

# call of EFDSYN with the options for stability degree -3 and the synthesis
# of a scalar output filter
@time Q, Rf, info = afdsyn(sysf, sdeg = -3, rdim = 1);

# normalize Q and Rf to match example
scale = evalfr(Rf.sys[1,1],Inf)[1,1];
dss2rm(Q.sys/scale, atol = 1.e-7)
dss2rm(Rf.sys/scale, atol = 1.e-7)

# check synthesis results
R = fdIFeval(Q, sysf; minimal = true, atol = 1.e-7);
@test iszero(R.sys[:,[R.controls;R.disturbances]], atol = 1.e-7) &&
      iszero(R.sys[:,R.faults]-Rf.sys) &&
      fdisspec(Rf, [0]) == Bool[1 1]

## Test covering design matrices
##
p = 3; mf = 2;
sysf = fdimodset(rss(1,p,mf),faults = 1:mf);

# solve an EFDP using the nullspace based approach
@time Q, Rf, info = efdsyn(sysf; atol = 1.e-7, minimal = false, rdim = 2); info
@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, minimal = false, rdim = 2); info
R = fdIFeval(Q, sysf);
@test iszero(Rf.sys-R.sys[:,R.faults],atol=1.e-7) && info.S2 == Bool[1 1; 1 1; 1 1] &&
      info.HDesign2 == [0.0 1.0 0.0; 0.0 0.0 1.0]

# the same results  must result using the resulting design matrix H2
Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = false, simple = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rf.sys-Rf1.sys, atol = 1.e-7)

## solve an EFP using the nullspace based approach
@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, minimal = false, rdim = 2, simple = true); info
R = fdIFeval(Q, sysf);
@test iszero(Rf.sys-R.sys[:,R.faults],atol=1.e-7) && info.S2 == Bool[1 1; 1 1; 1 1] &&
         info.HDesign2 == [0.0 1.0 0.0; 0.0 0.0 1.0]

# the same results  must result using the resulting design matrix H
Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = false, simple = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rf.sys-Rf1.sys, atol = 1.e-7)

## solve an EFP using the nullspace based approach
@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, minimal = false, rdim = 1, simple = true); info
R = fdIFeval(Q, sysf);
@test iszero(Rf.sys-R.sys[:,R.faults],atol=1.e-7) && info.S2 == Bool[1 1; 1 1; 1 1] && info.HDesign2 == [0.0 1.0 0.0]

# the same results  must result using the resulting design matrix H
Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = false, simple = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rf.sys-Rf1.sys, atol = 1.e-7)

## solve an EFD using the nullspace based approach
@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, minimal = true, simple = true); info
#@time Q, Rf, info = efdsyn(sysf; atol = 1.e-7, minimal = true, simple = true); info
R = fdIFeval(Q, sysf; atol = 1.e-7);
@test iszero(Rf.sys-R.sys[:,R.faults],atol=1.e-6) && iszero(R.sys[:,R.controls],atol=1.e-7) &&
      info.S2 == Bool[1 1; 1 1; 1 1] && info.HDesign2 == [0.0 1.0 0.0]

# the same results  must result using the resulting design matrix H
Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = false, simple = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rf.sys-Rf1.sys, atol = 1.e-7)

## solve an EFD using the nullspace based approach
@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, minimal = true, simple = false); info
R = fdIFeval(Q, sysf; atol = 1.e-7);
@test iszero(Rf.sys-R.sys[:,R.faults],atol=1.e-7) && iszero(R.sys[:,R.controls],atol=1.e-7) &&
      info.S2 == Bool[1 1; 1 1; 1 1] && info.HDesign2 == [0.0 1.0 0.0]

# the same results  must result using the resulting design matrix H
Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = false, simple = false, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rf.sys-Rf1.sys, atol = 1.e-7)

## Example with fault and noise inputs
p = 3; mf = 2; mw = 2; mu = 0
sysf = fdimodset(rss(1,p,mf+mw), noise = (mu+mf).+ (1:mw), faults = mu+1:mu+mf);

## solve an AMMP using the nullspace based approach
Q, Rfw, info = afdsyn(sysf; atol = 1.e-7, minimal = false, simple = false); info
R = fdIFeval(Q, sysf; atol = 1.e-7);
@test iszero(Rfw.sys[:,Rfw.faults]-R.sys[:,R.faults],atol=1.e-7) && iszero(R.sys[:,R.controls],atol=1.e-7) &&
      iszero(Rfw.sys[:,Rfw.noise]-R.sys[:,R.noise],atol=1.e-7) &&
      info.S == Bool[1 1; 1 1; 1 1] && info.S2 == Bool[1 1] &&
      info.HDesign ==[0.0 1.0 0.0; 0.0 0.0 1.0] &&  info.HDesign2[:] == [1.0] &&
      info.gap ≈ fdif2ngap(Rfw)[1]

# the same results  must result using the resulting design matrix H
Q1, Rfw1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = false, simple = false, HDesign = info.HDesign, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rfw.sys-Rfw1.sys, atol = 1.e-7)

## special cases which require forming rdim combinations of nout vecotors, with nout > rdim

# Gf = [ I;0]
p = 4; mf = 2;
sys = dss([eye(2);zeros(2,2)]);
sysf = fdimodset(sys,faults = 1:mf);

# solve an EFDP using the nullspace based approach
@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, minimal = true, rdim = 1); info
R = fdIFeval(Q, sysf);
@test iszero(Rf.sys-R.sys[:,R.faults],atol=1.e-7) && info.S2 == Bool[1 0; 0 1; 0 0; 0 0]

# the same results  must result using the resulting design matrix H
Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rf.sys-Rf1.sys, atol = 1.e-7)

# solve an EFDP using the nullspace based approach
@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, minimal = false, rdim = 1); info
R = fdIFeval(Q, sysf);
@test iszero(Rf.sys-R.sys[:,R.faults],atol=1.e-7) && info.S2 == Bool[1 0; 0 1; 0 0; 0 0]

# the same results  must result using the resulting design matrix H
Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = false, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rf.sys-Rf1.sys, atol = 1.e-7)

# solve an EFDP using the nullspace based approach
@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, minimal = true, rdim = 1, simple = true); info
R = fdIFeval(Q, sysf);
@test iszero(Rf.sys-R.sys[:,R.faults],atol=1.e-7) && info.S2 == Bool[1 0; 0 1; 0 0; 0 0]

# the same results  must result using the resulting design matrix H
Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = true, simple = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rf.sys-Rf1.sys, atol = 1.e-7)

# solve an EFDP using the nullspace based approach
@time Q, Rf, info = afdsyn(sysf; atol = 1.e-7, minimal = false, rdim = 1, simple = true); info
R = fdIFeval(Q, sysf);
@test iszero(Rf.sys-R.sys[:,R.faults],atol=1.e-7) && info.S2 == Bool[1 0; 0 1; 0 0; 0 0]

# the same results  must result using the resulting design matrix H
Q1, Rf1, info1 = afdsyn(sysf; atol = 1.e-7, minimal = false, simple = true, HDesign2 = info.HDesign2); info1

@test iszero(Q.sys-Q1.sys, atol = 1.e-7) && iszero(Rf.sys-Rf1.sys, atol = 1.e-7)

## Model Niemann 1998, Optim. Appl. Meth.
n = 5; mu = 2; md = 1; mf = 2; m = mu+md+mf; p = 5;
a = [
-0.0782 0.2939 0.0220 0.0208 -0.0291
0.0077 -0.0278 0.0015 -0.0015 0.0026
-1.3251 5.32608 -0.5263 0.2214 -0.4777
1.0809 -4.4452 0.3770 -0.4631 0.4032
2.1532 -8.6386 0.7811 -0.5745 0.7816]*1000;
bd = [
    0.0443
   -0.0042
    0.7910
   -0.6598
   -1.2881]*1000;
bu = [
    -0.007197 0.003005
0.003473 0.000284
1.218502 -0.032866
1.322502 0.020147
-0.082265 0.024388 ];
bf = bu;
c = eye(n); dd = zeros(n,md); du = zeros(n,mu); df = zeros(n,mf);

sys = dss(a,[bu bd bf],c,[du dd df]);
sysf = fdimodset(sys, c = 1:mu, d = mu .+ (1:md) , f = (mu+md) .+ (1:mf));

# opt = struct('tol',1.e-7,'FDGainTol',.01,...
#     'FDFreq',0,'sdeg',-0.05,'m1',mu+md);
# apply fdigenspec to [Gu Gd Gf; I 0 0]
#S_strong = fdigenspec([sys; eye(mu,mu+md+mf)],opt)
S_strong = Bool[1 0;0 1;1 1]
SFDI = S_strong

nb = size(SFDI,1)
orders = zeros(Int,nb)

Qtot = dss(zeros(0,p+mu)); Rftilde = dss(zeros(0,mf));
info = Any[]; HDesign2 = Any[]
for i = 1:nb
   # set input groups for i-th design
   indd = Vector(1:mf)[SFDI[i,:] .== false]
   indf = Vector(1:mf)[SFDI[i,:] .== true]
   sysc = fdimodset(sysf.sys, c = 1:mu, d = [Vector(mu .+ (1:md)); (mu+md) .+ indd],
                    f = (mu+md) .+ indf, aux = (mu+md) .+ (1:mf));
   #
   Qi, Rfi, infoi =afdsyn(sysc, atol = 1.e-7, sdeg =-5, smarg = -0.05, minimal = true,
                           FDfreq = 0, FDGainTol = 0.0001, rdim = 1, simple = false);  infoi
   info = [info; infoi]
   HDesign2 = [HDesign2; [infoi.HDesign2]]
   Qtot = [Qtot; Qi.sys];
   Rftilde = [Rftilde; Rfi.sys[:,Rfi.aux]];
   orders[i] = order(Qi.sys)
end

# checks: Q[Gu Gd;I 0] = 0 and Q[Gf; 0] = Rftilde
syse = [sysf.sys; eye(mu,mu+md+mf)];
@test iszero(Qtot*syse[:,[sysf.controls; sysf.disturbances]], atol = 1.e-7)
@test iszero(Qtot*syse[:,sysf.faults]-Rftilde,atol=1.e-7)

@test all(orders .== 1)
#step(Rftilde,10)

S, gains = fdisspec_(Rftilde, 0;  FDGainTol = 0.0001)
@test isequal(SFDI,S[:,:,1])
S1 = fditspec_(Rftilde; atol = 1.e-7,FDStol = 0.0001, FDfreq = 0)
@test isequal(SFDI,S1[:,:,1])

## Example 1, Xiong & Saif 2000, Int. J. Robust Nonlinear Control
n = 4; mu = 2; md = 2; mf = 2; m = mu+md+mf; p = 4;
a = [ -9.9477 -0.7476 0.2632 -5.0337
      52.1659 2.7452 5.5532 -24.4221
      26.0922 2.6361 -4.1975 -19.2774
      0.0 0.0 1.0 0.0];
bu = [ 0.4422 0.1761
       3.5446 -7.5922
        -5.52 4.49
        0.0 0.0];
bd = [ 0 0; 0 1 ; 1 0; 0 0 ];
bf = [bu[:,1] zeros(n,1)];
c = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 1 1 1];
dd = zeros(n,md); du = zeros(n,mu); df = [zeros(n,mf-1) [0 0 1 0]'];

sys = dss(a,[bu bd bf],c,[du dd df]);
sysf = fdimodset(sys, c = 1:mu, d = mu .+ (1:md) , f = (mu+md) .+ (1:mf));

# opt = struct('tol',1.e-7,'FDGainTol',.01,...
#     'FDFreq',0,'sdeg',-0.05,'m1',mu+md);
# apply fdigenspec to [Gu Gd Gf; I 0 0]
#S_strong = fdigenspec([sys; eye(mu,mu+md+mf)],opt)
S_strong = Bool[1 0;0 1;1 1]
SFDI = S_strong

nb = size(SFDI,1)
orders = zeros(Int,nb)

Qtot = dss(zeros(0,p+mu)); Rftilde = dss(zeros(0,mf));
info = Any[]; HDesign2 = Any[]
for i = 1:nb
   # set input groups for i-th design
   indd = Vector(1:mf)[SFDI[i,:] .== false]
   indf = Vector(1:mf)[SFDI[i,:] .== true]
   sysc = fdimodset(sysf.sys, c = 1:mu, d = [Vector(mu .+ (1:md)); (mu+md) .+ indd],
                    f = (mu+md) .+ indf, aux = (mu+md) .+ (1:mf));
   #
   Qi, Rfi, infoi = afdsyn(sysc, atol = 1.e-7, sdeg =-5, smarg = -0.05, minimal = true,
                           FDfreq = 0, FDGainTol = 0.0001, rdim = 1, simple = false);  infoi
   info = [info; infoi]
   HDesign2 = [HDesign2; [infoi.HDesign]]
   Qtot = [Qtot; Qi.sys];
   Rftilde = [Rftilde; Rfi.sys[:,Rfi.aux]];
   orders[i] = order(Qi.sys)
end

# checks: Q[Gu Gd;I 0] = 0 and Q[Gf; 0] = Rftilde
syse = [sysf.sys; eye(mu,mu+md+mf)];
@test iszero(Qtot*syse[:,[sysf.controls; sysf.disturbances]], atol = 1.e-7)
@test iszero(Qtot*syse[:,sysf.faults]-Rftilde,atol=1.e-7)

@test orders == [2, 1, 1]
#step(Rftilde,10)

S, gains = fdisspec_(Rftilde, 0;  FDGainTol = 0.0001)
@test isequal(SFDI,S[:,:,1])
S1 = fditspec_(Rftilde; atol = 1.e-7,FDStol = 0.0001, FDfreq = 0)
@test isequal(SFDI,S1[:,:,1])

## Example 2, Xiong & Saif 2000, Int. J. Robust Nonlinear Control
n = 5; mu = 3; md = 2; mf = 2; m = mu+md+mf; p = 3;
a = [
0 0 -0.0034 0 0
0 -0.041 0.0013 0 0
0 0 -1.1471 0 0
0 0 -0.0036 0 0
0 0.094 0.0057 0 -0.051];
bu = [
    -1 0 0
     0 0 0
     0 0 0.948
     0.916 -1 0
    -0.598 0 0];
bd = [
     0 1
     0.062 -0.132
     0 -7.189
     0 0
     0 0 ];
bf = bu[:,1:2];
c = [
1 0 0 0 0
0 0 1 0 0
0 0 0 1 0];
dd = zeros(p,md); du = zeros(p,mu); df = zeros(p,mu-1);

sys = dss(a,[bu bd bf],c,[du dd df]);

sysf = fdimodset(sys, c = 1:mu, d = mu .+ (1:md) , f = (mu+md) .+ (1:mf));

# opt = struct('tol',1.e-7,'FDGainTol',.01,...
#     'FDFreq',0,'sdeg',-0.05,'m1',mu+md);
# apply fdigenspec to [Gu Gd Gf; I 0 0]
#S_strong = fdigenspec([sys; eye(mu,mu+md+mf)],opt)
S_strong = Bool[1 0;0 1;1 1]
SFDI = S_strong

nb = size(SFDI,1)
orders = zeros(Int,nb)

Qtot = dss(zeros(0,p+mu)); Rftilde = dss(zeros(0,mf));
info = Any[]; HDesign = Any[]
for i = 1:nb
   # set input groups for i-th design
   indd = Vector(1:mf)[SFDI[i,:] .== false]
   indf = Vector(1:mf)[SFDI[i,:] .== true]
   sysc = fdimodset(sysf.sys, c = 1:mu, d = [Vector(mu .+ (1:md)); (mu+md) .+ indd],
                    f = (mu+md) .+ indf, aux = (mu+md) .+ (1:mf));
   #
   Qi, Rfi, infoi = afdsyn(sysc, atol = 1.e-7, sdeg =-5, smarg = -0.05, minimal = true,
                           FDfreq = 0, FDGainTol = 0.0001, rdim = 1, simple = false);  infoi
   info = [info; infoi]
   HDesign = [HDesign; [infoi.HDesign2]]
   Qtot = [Qtot; Qi.sys];
   Rftilde = [Rftilde; Rfi.sys[:,Rfi.aux]];
   orders[i] = order(Qi.sys)
end

# checks: Q[Gu Gd;I 0] = 0 and Q[Gf; 0] = Rftilde
syse = [sysf.sys; eye(mu,mu+md+mf)];
@test iszero(Qtot*syse[:,[sysf.controls; sysf.disturbances]], atol = 1.e-7)
@test iszero(Qtot*syse[:,sysf.faults]-Rftilde,atol=1.e-7)

@test orders == [1, 1, 1]
#step(Rftilde,10)

S, gains = fdisspec_(Rftilde, 0;  FDGainTol = 0.0001)
@test isequal(SFDI,S[:,:,1])
S1 = fditspec_(Rftilde; atol = 1.e-7,FDStol = 0.0001, FDfreq = 0)
@test isequal(SFDI,S1[:,:,1])

# use individual synthesis with specified structure vectors
nb = size(SFDI,1)
orders = zeros(Int,nb)

Qtot = dss(zeros(0,p+mu)); Rftilde = dss(zeros(0,mf));
info = Any[]; HDesign = Any[]
for i = 1:nb
   Qi, Rfi, infoi = afdsyn(sysf, SFDI[i,:]; atol = 1.e-7, sdeg =-5, smarg = -0.05, minimal = true,
                           FDfreq = 0, FDGainTol = 0.0001, rdim = 1, simple = false);  infoi
   info = [info; infoi]
   HDesign = [HDesign; [infoi.HDesign]]
   Qtot = [Qtot; Qi.sys];
   Rftilde = [Rftilde; Rfi.sys];
   orders[i] = order(Qi.sys)
end

# checks: Q[Gu Gd;I 0] = 0 and Q[Gf; 0] = Rftilde
syse = [sysf.sys; eye(mu,mu+md+mf)];
@test iszero(Qtot*syse[:,[sysf.controls; sysf.disturbances]], atol = 1.e-7)
@test iszero(Qtot*syse[:,sysf.faults]-Rftilde,atol=1.e-7)

@test orders == [1, 1, 1]
#step(Rftilde,10)

S, gains = fdisspec_(Rftilde, 0;  FDGainTol = 0.0001)
@test isequal(SFDI,S[:,:,1])
S1 = fditspec_(Rftilde; atol = 1.e-7,FDStol = 0.0001, FDfreq = 0)
@test isequal(SFDI,S1[:,:,1])

## Yuan et al. IJC (1997)  strong synthesis
p = 3; mu = 1; md = 0; mf = 8; n = 4;
A = [
-1 1 0 0
1 -2 1 0
0 1 -2 1
0 0 1 -2  ];
Bu = [1 0 0 0]';
Bf = [
1 0 0 0 1 0 0 0
0 1 0 0 -1 1 0 0
0 0 1 0 0 -1 1 0
0 0 0 1 0 0 -1 1];
C = [
 1 0 0 0
0 0 1 0
0 0 0 1];
Du = zeros(p,mu); Df = zeros(p,mf);

sys = dss(A,[Bu Bf],C,[Du Df]);
sysf = fdimodset(sys, c = 1:mu, d = mu .+ (1:md) , f = (mu+md) .+ (1:mf));

# opt = struct('tol',1.e-7,'FDGainTol',.01,...
#     'FDFreq',0,'sdeg',-0.05,'m1',mu+md);
# # apply fdigenspec to [Gu Gd Gf; I 0 0]
# S_strong = fdigenspec([sys; eye(mu,mu+md+mf)],opt)
#

S_strong = Bool[   0   0   0   1   0   0   1   1
0   1   1   0   1   1   1   0
0   1   1   1   1   1   0   1
0   1   1   1   1   1   1   1
1   0   1   0   1   1   1   0
1   0   1   1   1   1   0   1
1   0   1   1   1   1   1   1
1   1   0   0   1   1   0   0
1   1   0   1   1   1   1   1
1   1   1   0   1   1   1   0
1   1   1   1   1   1   0   1
1   1   1   1   1   1   1   1]

SFDI = S_strong

nb = size(SFDI,1)
orders = zeros(Int,nb)

Qtot = dss(zeros(0,p+mu)); Rftilde = dss(zeros(0,mf));
info = Any[]; HDesign = Any[]
for i = 1:nb
   # set input groups for i-th design
   indd = Vector(1:mf)[SFDI[i,:] .== false]
   indf = Vector(1:mf)[SFDI[i,:] .== true]
   sysc = fdimodset(sysf.sys, c = 1:mu, d = [Vector(mu .+ (1:md)); (mu+md) .+ indd],
                    f = (mu+md) .+ indf, aux = (mu+md) .+ (1:mf));
   #
   Qi, Rfi, infoi = afdsyn(sysc, atol = 1.e-7, sdeg =-5, smarg = -0.05, minimal = true,
                           FDfreq = 0, FDGainTol = 0.0001, rdim = 1, simple = false);  infoi
   info = [info; infoi]
   HDesign = [HDesign; [infoi.HDesign2]]
   Qtot = [Qtot; Qi.sys];
     Rftilde = [Rftilde; Rfi.sys[:,Rfi.aux]];
   orders[i] = order(Qi.sys)
end

# checks: Q[Gu Gd;I 0] = 0 and Q[Gf; 0] = Rftilde
syse = [sysf.sys; eye(mu,mu+md+mf)];
@test iszero(Qtot*syse[:,[sysf.controls; sysf.disturbances]], atol = 1.e-7)
@test iszero(Qtot*syse[:,sysf.faults]-Rftilde,atol=1.e-7)

@test orders == [1, 2, 2, 2, 1, 1, 1, 2, 2, 2, 2, 2]
#step(Rftilde,10)

S, gains = fdisspec_(Rftilde, 0;  FDGainTol = 0.0001)
@test isequal(SFDI,S[:,:,1])
S1 = fditspec_(Rftilde; atol = 1.e-7,FDStol = 0.0001, FDfreq = 0)
@test isequal(SFDI,S1[:,:,1])

## Yuan et al. IJC (1997)  weak synthesis with stabillization
p = 3; mu = 1; md = 0; mf = 8; n = 4;
A = [
-1 1 0 0
1 -2 1 0
0 1 -2 1
0 0 1 -2  ];
Bu = [1 0 0 0]';
Bf = [
1 0 0 0 1 0 0 0
0 1 0 0 -1 1 0 0
0 0 1 0 0 -1 1 0
0 0 0 1 0 0 -1 1];
C = [
 1 0 0 0
0 0 1 0
0 0 0 1];
Du = zeros(p,mu); Df = zeros(p,mf);

sys = dss(A,[Bu Bf],C,[Du Df]);
sysf = fdimodset(sys, c = 1:mu, d = mu .+ (1:md) , f = (mu+md) .+ (1:mf));

# opt = struct('tol',1.e-7,'FDGainTol',.01,...
#     'FDFreq',0,'sdeg',-0.05,'m1',mu+md);
# # apply fdigenspec to [Gu Gd Gf; I 0 0]
# S_strong = fdigenspec([sys; eye(mu,mu+md+mf)],opt)
#

S_weak = Bool[   0   0   0   1   0   0   1   1
0   1   1   0   1   1   1   0
0   1   1   1   1   1   0   1
0   1   1   1   1   1   1   1
1   0   1   0   1   1   1   0
1   0   1   1   1   1   0   1
1   0   1   1   1   1   1   1
1   1   0   0   1   1   0   0
1   1   0   1   1   1   1   1
1   1   1   0   0   1   1   0
1   1   1   0   1   0   1   0
1   1   1   0   1   1   1   0
1   1   1   1   0   1   0   1
1   1   1   1   0   1   1   1
1   1   1   1   1   0   0   1
1   1   1   1   1   0   1   1
1   1   1   1   1   1   0   1
1   1   1   1   1   1   1   1]

SFDI = S_weak;
nb = size(SFDI,1)
orders = zeros(Int,nb)

Qtot = dss(zeros(0,p+mu)); Rftilde = dss(zeros(0,mf));
info = Any[]; HDesign = Any[]
for i = 1:nb
   # set input groups for i-th design
   indd = Vector(1:mf)[SFDI[i,:] .== false]
   indf = Vector(1:mf)[SFDI[i,:] .== true]
   sysc = fdimodset(sysf.sys, c = 1:mu, d = [Vector(mu .+ (1:md)); (mu+md) .+ indd],
                    f = (mu+md) .+ indf, aux = (mu+md) .+ (1:mf));
   #
   Qi, Rfi, infoi = afdsyn(sysc, atol = 1.e-7, sdeg =-5, smarg = -0.05, minimal = true,
                           FDtol = 0.0001, rdim = 1, simple = false);  infoi
   info = [info; infoi]
   HDesign = [HDesign; [infoi.HDesign2]]
   Qtot = [Qtot; Qi.sys];
   Rftilde = [Rftilde; Rfi.sys[:,Rfi.aux]];
   orders[i] = order(Qi.sys)
end

# checks: Q[Gu Gd;I 0] = 0 and Q[Gf; 0] = Rftilde
syse = [sysf.sys; eye(mu,mu+md+mf)];
@test iszero(Qtot*syse[:,[sysf.controls; sysf.disturbances]], atol = 1.e-7)
@test iszero(Qtot*syse[:,sysf.faults]-Rftilde,atol=1.e-7)

@test orders == [1, 2, 2, 2, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2]
#step(Rftilde,10)

S = fditspec_(Rftilde;  atol = 1.e-6, FDtol = 0.0001)
@test isequal(SFDI,S)

## Yuan et al. IJC (1997)  weak synthesis with pole assignment and
#  enforcing least global order of 6!
p = 3; mu = 1; md = 0; mf = 8; n = 4;
A = [
-1 1 0 0
1 -2 1 0
0 1 -2 1
0 0 1 -2  ];
Bu = [1 0 0 0]';
Bf = [
1 0 0 0 1 0 0 0
0 1 0 0 -1 1 0 0
0 0 1 0 0 -1 1 0
0 0 0 1 0 0 -1 1];
C = [
 1 0 0 0
0 0 1 0
0 0 0 1];
Du = zeros(p,mu); Df = zeros(p,mf);

sys = dss(A,[Bu Bf],C,[Du Df]);
sysf = fdimodset(sys, c = 1:mu, d = mu .+ (1:md) , f = (mu+md) .+ (1:mf));

# opt = struct('tol',1.e-7,'FDGainTol',.01,...
#     'FDFreq',0,'sdeg',-0.05,'m1',mu+md);
# # apply fdigenspec to [Gu Gd Gf; I 0 0]
# S_strong = fdigenspec([sys; eye(mu,mu+md+mf)],opt)
#

S_weak = Bool[   0   0   0   1   0   0   1   1
0   1   1   0   1   1   1   0
0   1   1   1   1   1   0   1
0   1   1   1   1   1   1   1
1   0   1   0   1   1   1   0
1   0   1   1   1   1   0   1
1   0   1   1   1   1   1   1
1   1   0   0   1   1   0   0
1   1   0   1   1   1   1   1
1   1   1   0   0   1   1   0
1   1   1   0   1   0   1   0
1   1   1   0   1   1   1   0
1   1   1   1   0   1   0   1
1   1   1   1   0   1   1   1
1   1   1   1   1   0   0   1
1   1   1   1   1   0   1   1
1   1   1   1   1   1   0   1
1   1   1   1   1   1   1   1]

SFDI = S_weak;
nb = size(SFDI,1)
orders = zeros(Int,nb)

Qtot = dss(zeros(0,p+mu)); Rftilde = dss(zeros(0,mf));
info = Any[]; HDesign = Any[]
for i = 1:nb
   # set input groups for i-th design
   indd = Vector(1:mf)[SFDI[i,:] .== false]
   indf = Vector(1:mf)[SFDI[i,:] .== true]
   sysc = fdimodset(sysf.sys, c = 1:mu, d = [Vector(mu .+ (1:md)); (mu+md) .+ indd],
                    f = (mu+md) .+ indf, aux = (mu+md) .+ (1:mf));
   #
   Qi, Rfi, infoi = afdsyn(sysc, atol = 1.e-7, sdeg =-5, smarg = -5, poles = [-5, -6], minimal = true,
                            FDtol = 0.0001, rdim = 1, simple = false);  infoi
   info = [info; infoi]
   HDesign = [HDesign; [infoi.HDesign]]
   Qtot = [Qtot; Qi.sys];
   Rftilde = [Rftilde; Rfi.sys[:,Rfi.aux]];
   orders[i] = order(Qi.sys)
end

# checks: Q[Gu Gd;I 0] = 0 and Q[Gf; 0] = Rftilde
syse = [sysf.sys; eye(mu,mu+md+mf)];
@test iszero(Qtot*syse[:,[sysf.controls; sysf.disturbances]], atol = 1.e-7)
@test iszero(Qtot*syse[:,sysf.faults]-Rftilde,atol=1.e-7)

@test orders == [1, 2, 2, 2, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2]
#step(Rftilde,10)

S = fditspec_(Rftilde;  atol = 1.e-7, FDtol = 0.0001);
@test isequal(SFDI,S)

@test  count(ghanorm(Qtot)[2] .> 1.e-7) == 6

# ## DC Motor (Ding, 2013)
# s = rtf('s');
# Gu = 8.75/(1+1.225*s)/(1+0.03*s)/(1+0.005*s);
# Gd = -31.07/s/(1+0.005*s);
# Gc = 1.6*(1 + 1.225*s)/s;

# G = [Gu*Gc Gu 1 Gd ]./(1+Gu*Gc);
# # set input groups
# sysf = fdimodset(dss(G,minimal=true,atol=1.e-7),c = 1, f = 2:4);
# S_weak = fdigenspec(sysf,struct('tol',1.e-7))
# S_strong = fdigenspec(sysf,struct('tol',1.e-7,'FDFreq',0))

# # # check with expressions from Ding (2013)
# # gu = 14/(s*(1+0.03*s)*(1+0.005*s)+14);
# # gd = -31.07*(1+0.03*s)/(s*(1+0.03*s)*(1+0.005*s)+14);
# # gfa = 8.75*s/(1+1.225*s)/(s*(1+0.03*s)*(1+0.005*s)+14);
# # gfs = s*(1+0.03*s)*(1+0.005*s)/(s*(1+0.03*s)*(1+0.005*s)+14);
# # norm(gu-G(:,1),inf), norm(gd-G(:,4),inf), norm(gfa-G(:,2),inf), norm(gfs-G(:,3),inf),
# #

# ## case 0:
# f = [0 0 1]; T = [0:0.1:5]'; N = length(T); ucontrol = ones(N,1);
# Uf = [ucontrol, f(1)*ones(N,1) f(2)*ones(N,1) f(3)*ones(N,1)];
# lsim(G,Uf,T);

# ## case 1 (Echeveria, DC Motor Benchmark):
# f = [0.87 -0.12 0.53]; T = [0:0.1:5]'; N = length(T); ucontrol = rand(N,1);
# Uf = [ucontrol, f(1)*ones(N,1) f(2)*ones(N,1) f(3)*ones(N,1)];
# yf = lsim(G,Uf,T);
# yfp = yf.*(ones(N,1)+0.02*rand(N,1));
# yfp2 = yf.*(ones(N,1)+0.08*rand(N,1));

# x = rand(3,1);
# err = dcmotor( x, G, yf, T, ucontrol);
# lb = -ones(3,1); ub = ones(3,1);

# [x,err] = fmincon(@(x) dcmotor(x,G,yf,T,ucontrol),zeros(3,1),[],[],[],[],lb,ub)

# [xp,err1] = fmincon(@(x) dcmotor(x,G,yfp,T,ucontrol),zeros(3,1),[],[],[],[],lb,ub)
# [xp2,err2] = fmincon(@(x) dcmotor(x,G,yfp2,T,ucontrol),zeros(3,1),[],[],[],[],lb,ub)

end # test efdsyn

end # module

max_stars_repo_name: andreasvarga/FaultDetectionTools

max_issues_repo_name: andreasvarga/FaultDetectionTools

max_forks_repo_name: andreasvarga/FaultDetectionTools


text:
using Test

flip(bit) = bit == '0' ? '1' : '0'
most_common(bits) = 2count(==('0'), bits) > length(bits) ? '0' : '1'
least_common(bits) = flip.(most_common(bits))
lines_to_bits(lines) = hcat(Vector{Char}.(lines)...)
parse_bits(chars::Vector{Char}) = parse(Int, join(chars, ""); base = 2)

codes = readlines("day03.in")
test_codes = ["00100",
              "11110",
              "10110",
              "10111",
              "10101",
              "01111",
              "00111",
              "11100",
              "10000",
              "11001",
              "00010",
              "01010"]

function q1(codes)
    codebits = lines_to_bits(codes)
    γ = most_common.(eachrow(codebits))
    ε = flip.(γ)
    return parse_bits(γ) * parse_bits(ε)
end

function rating(criteria, codebits)
    matching = reduce(eachrow(codebits), init = 1:size(codebits, 2)) do m, row
        return length(m) == 1 ? m : m[row[m] .== criteria(row[m])]
    end
    return vec(codebits[:, matching])
end

function q2(codes)
    codebits = lines_to_bits(codes)
    oxygen_rating = rating(most_common, codebits)
    co2_rating = rating(least_common, codebits)
    return parse_bits(oxygen_rating) * parse_bits(co2_rating)
end

@test q1(test_codes) == 198
@test q2(test_codes) == 230

println("Q1: ", q1(codes))
println("Q2: ", q2(codes))

ext: jl

max_stars_repo_name: dhanak/competitive-coding

max_issues_repo_name: dhanak/competitive-coding

max_forks_repo_name: dhanak/competitive-coding


text:
using ParameterEstimocean
using Oceananigans
using GLMakie

cases = ["free_convection",
         "strong_wind_weak_cooling",
         "med_wind_med_cooling",
         "weak_wind_strong_cooling",
         "strong_wind",
         "strong_wind_no_rotation"]

suite = "two_day_suite"
case_path(case) = joinpath("data", suite, case * "_instantaneous_statistics.jld2")

fontsize_theme = Theme(fontsize = 28, linewidth=4)
set_theme!(fontsize_theme)

field_names = (:u, :v, :b, :e)
observations = [SyntheticObservations(case_path(case); field_names) for case in cases]

obs1 = first(observations)
z = znodes(Center, obs1.grid)
t = obs1.times
Nt = length(t)

@show obs1

fig = Figure(resolution=(1400, 600))
axb = Axis(fig[1, 1], xlabel="Buoyancy (m s⁻²)", ylabel="z (m)")
axe = Axis(fig[1, 2], xlabel="Turbulent kinetic energy (m² s⁻²)", ylabel="z (m)", yaxisposition=:right)
# axu = Axis(fig[1, 3], xlabel="Velocities (m s⁻¹)", ylabel="z (m)", yaxisposition=:right, xscale=log10)

hidespines!(axb, :r, :t)
hidespines!(axe, :l, :t, :l)
#hidespines!(axu, :r, :t)

blines = []

for (case, obs) in zip(cases, observations)
    bt = obs.field_time_serieses.b
    ut = obs.field_time_serieses.u
    vt = obs.field_time_serieses.v
    et = obs.field_time_serieses.e
    b = bt[Nt]
    e = et[Nt]

    #=
    u = ut[Nt]
    v = vt[Nt]
    s = compute!(Field(sqrt(∂z(u)^2 + ∂z(v)^2)))
    zf = znodes(s)
    =#

    label = replace(case, "_" => " ")
    bline = lines!(axb, interior(b, 1, 1, :) .- b[1, 1, 1], z; label)
            #lines!(axu, interior(u, 1, 1, :), z; label)
            #lines!(axu, interior(v, 1, 1, :), z; label, linestyle=:dash)
            #lines!(axu, interior(s, 1, 1, :), zf; label)
            #case != "free_convection" && lines!(axu, interior(s, 1, 1, :), zf; label)
            lines!(axe, interior(e, 1, 1, :), z; label)
    push!(blines, bline)
end

obs = first(observations)
b0 = obs.field_time_serieses.b[1]
bline = lines!(axb, interior(b0, 1, 1, :) .- b0[1, 1, 1], z;
               color = (:black, 0.2),
               #linestyle = :dot,
               linewidth = 6,
               label = "Initial condition")

labels = [replace(case, "_" => " ") for case in cases]
push!(labels, "Initial condition")

push!(blines, bline)
Legend(fig[1, 2], blines, labels,
       framevisible = false,
       tellwidth = false,
       tellheight = false,
       margin = (10, 10, 10, 10),
       halign = :right,
       valign = :bottom)

#axislegend(axb, position=:rb)
#axislegend(axe, position=:rb)

display(fig)

save("two_day_suite.png", fig)

max_stars_repo_name: glwagner/LocalOceanClosureCalibration

max_issues_repo_name: glwagner/LocalOceanClosureCalibration

max_forks_repo_name: glwagner/LocalOceanClosureCalibration


text:
################################################################################
# ptree.jl
#
# Code implementing a persistent hash array mapped trie tyoe on which the
# persistent collection library of Air is built. This type is not intended for
# use outside of the library.
#
# @author Noah C. Benson
#
# MIT License
# Copyright (c) 2020 Noah C. Benson

# ==============================================================================
# Private
#
# This section contains private constants and function definitions intendef for
# use only in this library or occasionally within the library, but not outside
# of the current module.

# # Constants ==================================================================
#   This is the type of the unsigned integer used for the leaves.
const HASH_T              = typejoin(typeof(hash(nothing)),
                                     typeof(objectid(nothing)))
const HASH_ZERO           = HASH_T(0x00)
const HASH_ONE            = HASH_T(0x01)
const HASH_MAX            = ~HASH_ZERO
# Number of bits in the total addressable hash-space of a PTree.
const HASH_BITCOUNT       = sizeof(HASH_T) * 8
const HASH_TOPBIT         = HASH_T(0x01) << (HASH_BITCOUNT - 1)
#   We use a constant shift of 5 throughout except at the root node (which is
#   Due to the implemeentation details, the twig shifts *MUST* be >= 3 and all
#   shifts must be <= 7.
const PTREE_NODE_SHIFT    = 6
const PTREE_TWIG_SHIFT    = 6
const PTREE_ROOT_SHIFT    = (let tmp = rem(HASH_BITCOUNT - PTREE_TWIG_SHIFT,
                                           PTREE_NODE_SHIFT)
                                 (tmp == 0 ? PTREE_NODE_SHIFT : tmp)
                             end)
const PTREE_ROOT_FIRSTBIT = HASH_BITCOUNT - PTREE_ROOT_SHIFT
const PTREE_ROOT_NCHILD   = (1 << PTREE_ROOT_SHIFT)
const PTREE_NODE_NCHILD   = (1 << PTREE_NODE_SHIFT)
const PTREE_TWIG_NCHILD   = (1 << PTREE_TWIG_SHIFT)
const PTREE_BITS_BITCOUNT = max(PTREE_ROOT_NCHILD,
                                PTREE_NODE_NCHILD,
                                PTREE_TWIG_NCHILD)
const PTREE_BITS_T        = (if     (PTREE_BITS_BITCOUNT <= 8)  UInt8
                             elseif (PTREE_BITS_BITCOUNT <= 16) UInt16
                             elseif (PTREE_BITS_BITCOUNT <= 32) UInt32
                             elseif (PTREE_BITS_BITCOUNT <= 64) UInt64
                             else                               UInt128
                             end)
const BITS_ZERO           = PTREE_BITS_T(0x0)
const BITS_ONE            = PTREE_BITS_T(0x1)
const PTREE_NODE_BITS     = HASH_BITCOUNT - PTREE_ROOT_SHIFT - PTREE_TWIG_SHIFT
const PTREE_NODE_LEVELS   = div(PTREE_NODE_BITS, PTREE_NODE_SHIFT)
const PTREE_LEVELS        = PTREE_NODE_LEVELS + 2
const PTREE_ROOT_DEPTH    = 0
const PTREE_TWIG_DEPTH    = PTREE_ROOT_DEPTH + PTREE_NODE_LEVELS + 1
const PTREE_LEAF_DEPTH    = PTREE_TWIG_DEPTH + 1
# Functions for making masks
"""
    lowmask(bitno, T)
    lowmask(bitno)

Yields a mask of (unsigned integer) type T with all bits above the given bit
number set to false and all bits below that number set to true. The bit itself
is set to false. Bits are indexed starting at 0.

The default value of T is the PTree hash type (HASH_T).

lowmask(bitno) is equal to ~highmask(bitno).
"""
lowmask(bitno::K, ::Type{T}) where {T<:Unsigned,K<:Integer} =
    ((T(0x01) << bitno) - 0x01)
lowmask(bitno::K) where {K <: Integer} = lowmask(bitno, HASH_T)
"""
    highmask(bitno, T)
    highmask(bitno)

Yields a mask of (unsigned integer) type T with all bits above the given bit
number set to true and all bits below that number set to false. The bit itself
is set to true. Bits are indexed starting at 0.

The default value of T is the PTree hash type (HASH_T).

highmask(bitno) is equal to ~lowmask(bitno).
"""
highmask(bitno::K, ::Type{T}) where {T<:Unsigned,K<:Integer} =
    ~lowmask(bitno, T)
highmask(bitno::K) where {K <: Integer} = highmask(bitno, HASH_T)
# The mask of the bits that represent the depth of the node-id in a PTree.
const PTREE_DEPTH_MASK = lowmask(PTREE_TWIG_SHIFT)
# The pair type for the dict interface.
const HASHPAIR_T{T} = Pair{HASH_T, T} where {T}

# # Functions
"""
    ptree_depth(nodeaddr)

Yields the depth of the node with the given node address. This depth is in the
theoretical complete tree, not in the reified tree represented with memory.
"""
ptree_depth(nodeid::HASH_T) = nodeid & PTREE_DEPTH_MASK
"""
    depth_to_bitshift(depth)

Yields the tuple (B0,S) of the first bit index and the shift for the given
depth.
"""
depth_to_bitshift(depth::Integer) = depth_to_bitshift(Int(depth))
depth_to_bitshift(depth::Int) = begin
    if depth == PTREE_TWIG_DEPTH
        return (0, PTREE_TWIG_SHIFT)
    elseif depth == 0
        return (PTREE_ROOT_FIRSTBIT, PTREE_ROOT_SHIFT)
    else
        return (PTREE_ROOT_FIRSTBIT - depth*PTREE_NODE_SHIFT,
                PTREE_NODE_SHIFT)
    end
end
"""
    ptree_bitshift(nodeid)

Yields the bitshift for the given ptree node id.
"""
ptree_bitshift(nodeid::HASH_T) = depth_to_bitshift(ptree_depth(nodeid))
"""
    ptree_shift(nodeid)

Yields the shift for the given ptree node id.
"""
ptree_shift(nodeid::HASH_T) = ptree_bitshift(nodeid)[2]
"""
    ptree_firstbit(nodeid)

Yields the first-bit for the given ptree node id.
"""
ptree_firstbit(nodeid::HASH_T) = ptree_bitshift(nodeid)[1]
"""
    ptree_minleaf(nodeid)

Yields the minimum child leaf index associated with the given nodeid.
"""
ptree_minleaf(nodeid::HASH_T) = nodeid & ~PTREE_DEPTH_MASK
"""
    ptree_maxleaf(nodeid)

Yields the maximum child leaf index assiciated with the given nodeid.
"""
ptree_maxleaf(nodeid::HASH_T) = begin
    (b0,s) = ptree_bitshift(nodeid)
    mask = (HASH_ONE << (b0 + s)) - HASH_ONE
    return nodeid | mask
end
"""
    ptree_minmaxleaf(nodeid)

Yields the (min, max) child leaf index assiciated with the given nodeid.
"""
ptree_minmaxleaf(nodeid::HASH_T) = begin
    mn = ptree_minleaf(nodeid)
    (bit0,shift) = ptree_bitshift(nodeid)
    mask = (HASH_ONE << (bit0+shift)) - HASH_ONE
    return (nodeid & ~mask, nodeid | mask)
end
"""
    ptree_id(minleaf, depth)

Yields the node-id for the node whose minimum leaf and depth are given.
"""
ptree_id(minleaf::Integer,depth::Integer) =
    ptree_id(HASH_T(minleaf), HASH_T(depth))
ptree_id(minleaf::HASH_T, depth::HASH_T) = minleaf | depth
"""
    ptree_parentid(nodeid0)

Yields the node-id of the parent of the given node. Note that node 0 (the tree's
theoretical root) has no parent. If given a node id of 0, this function will
return an arbitrary large number.
"""
ptree_parentid(nodeid0::HASH_T) = begin
    d = ptree_depth(nodeid0) - 0x1
    node = nodeid0 & ~((-HASH_T(1)) >> (d*PTREE_NODE_SHIFT))
    return node | d
end
"""
    ptree_isbeneath(nodeid, leafid)

Yields true if the given leafid can be found beneath the given node-id.
"""
ptree_isbeneath(nodeid::HASH_T, leafid::HASH_T) = begin
    (mn,mx) = ptree_minmaxleaf(nodeid)
    return leafid <= mx && leafid >= mn
end
"""
    ptree_highbitdiff(id1, id2)

Yields the highest bit that is different between id1 and id2.
"""
ptree_highbitdiff(id1::HASH_T, id2::HASH_T) =
    HASH_BITCOUNT - leading_zeros(xor(id1, id2)) - 1

# ==============================================================================
# Structures / Types
#
# Definitions of structures and types that should come before the various
# methods and constructors that operate on them.
# #PTree =======================================================================
"""
    PTree{T}

PTree is a persistent tree/array hybrid that maps arbitrary unsigned integers
(type HASH_T) to values. It can efficiently be used for arrays or for
hash-maps, and supports efficient lookup, association, and dissociation.

All fields of a PTree should be considered strictly private, as modification
of the fields may result in a kernel crash. The properties of a PTree are both
immutable and safe to inpect.
"""
struct PTree{T} <: AbstractDict{HASH_T, T}
    id::HASH_T
    bits::PTREE_BITS_T
    numel::Int
    cells::Union{Nothing, Vector{T}, Vector{PTree{T}}}
end
const PTREE_NODE_CELL_T{T} = Vector{PTree{T}} where {T}
const PTREE_TWIG_CELL_T{T} = Vector{T} where {T}
const PTREE_EMPTY_CELL_T = Nothing
const PTREE_CELLS_T{T} = Union{PTREE_EMPTY_CELL_T,
                               PTREE_NODE_CELL_T{T},
                               PTREE_TWIG_CELL_T{T}} where {T}
# We will occasionally want to do something across all the children of a node;
# we can exploit the bits data to speed this up in sparse cases. The code for
# doing that is in these two macros.
macro _ptree_forcells(tree, var::Symbol, expr::Expr)
    b = gensym("bits")
    return quote
        let $b = $(tree).bits, $var
            while true
                $var = trailing_zeros($b)
                if $var == PTREE_BITS_BITCOUNT
                    break
                else
                    $expr
                end
                $b &= ~($(HASH_T(0x01)) << $var)
            end
        end
    end |> esc
end
macro _ptree_forcells_from(tree, var::Symbol, from, expr::Expr)
    b = gensym("bits")
    f = gensym("from")
    return quote
        let $b = $(tree).bits & highmask($from), $var
            while true
                $var = trailing_zeros($b)
                if $var == PTREE_BITS_BITCOUNT
                    break
                else
                    $expr
                end
                b &= ~($(HASH_T(0x01)) << $var)
            end
        end
    end |> esc
end
"""
    ptree_cellindex(ptree, leafid)
    ptree_cellindex(nodeid, bits, leafid)
Yields a tuple (present, bitindex, cellindex) in which [1] present is a boolean
indicating whether a ptree containing the given leafid or the leaf itself is a
child of the given ptree; [2] bitindex is the index into ptree's bits integer
for the given leaf, and cellindex is the inndex innto the ptree's cell array
where that child is or would be found. If the leafid is outside of the given
ptree (i.e., it cannot exist beneath this ptree) then the cellindex returned
returned is 0, but the bitindex will still match the appropriate shift for the
ptree's depth.
"""
ptree_cellindex(id::HASH_T, bits::PTREE_BITS_T, leafid::HASH_T) = begin
    # Check that the leaf is below this leaf.
    (bit0, shift) = ptree_bitshift(id)
    # Grab the index out of the leaf id.
    idx = (leafid >> bit0) & lowmask(shift)
    ptree_isbeneath(id, leafid) || return (false, idx, 0)
    # See if the bit is set?
    flag = HASH_ONE << idx
    chno = 1 + count_ones(bits & lowmask(idx))
    return (bits & flag == flag, idx, chno)
end
ptree_cellindex(u::PTree{T}, leafid::HASH_T) where {T} =
    ptree_cellindex(getfield(u, :id), getfield(u, :bits), leafid)
"""
    ptree_cellindex!(ptree, leafid)
    ptree_cellindex!(nodeid, bits, leafid)
Yields the index into the ptree's cell vector of the child containing the leaf
with the given id. If the ptree does not contain the given leafid because the
appropriate bit is not set then 0 is returned. However, unlike the function
ptree_cellindex(), this function does not check whether or not the given leafid
is in the set of possible children of the tree.
"""
ptree_cellindex!(id::HASH_T, bits::PTREE_BITS_T, leafid::HASH_T) = begin
    # Check that the leaf is below this leaf.
    (bit0, shift) = ptree_bitshift(id)
    # Grab the index out of the leaf id.
    idx = (leafid >> bit0) & lowmask(shift)
    # See if the bit is set?
    flag = HASH_ONE << idx
    chno = 1 + count_ones(bits & lowmask(idx))
    return (bits & flag == flag, idx, chno)
end
ptree_cellindex!(ptree::PTree{T}, leafid::HASH_T) where {T} =
    ptree_cellindex!(getfield(ptree, :id), getfield(ptree, :bits), leafid)
"""
    ptree_cellkey(ptree, childidx)
Yields the leafid (a HASH_T value) of the key that goes with the particular
child index that is given. This only works correctly for twig nodes.
"""
ptree_cellkey(id::HASH_T, k::HASH_T) = begin
    mn = ptree_minleaf(id)
    return mn | k
end
ptree_cellkey(id::HASH_T, k::II) where {II<:Integer} =
    ptree_cellkey(id, (HASH_T(k)))
ptree_cellkey(u::PTree{T}, k::II) where {T,II<:Integer} =
    ptree_cellkey(getfield(u, :id), HASH_T(k))
ptree_cellkey(u::PTree{T}, k::HASH_T) where {T} =
    ptree_cellkey(getfield(u, :id), k)

# # Constructors ===============================================================
#   First, constructors for making empty Trees.
PTree{T}() where {T} = PTree{T}(0x0, 0x0, 0, nothing)
#   Now for copying trees of the same type.
PTree{T}(u::PTree{T}) where {T}   = u
PTree{T}(u::PTree{U}) where {T,U} = begin
    (id, bits, n, cells) = (getfield(u, :id),    getfield(u, :bits),
                            getfield(u, :numel), getfield(u, :cells))
    d = ptree_depth(id)
    if d == PTREE_TWIG_DEPTH
        cells::Vector{U}
        return PTree{T}(id, bits, n, convert(Vector{T}, cells))
    else
        cells::Vector{PTree{U}}
        return PTree{T}(id, bits, n, PTree{T}[PTree{T}(c) for c in cells])
    end
end
PTree{T}(d::AbstractDict{HASH_T,S}) where {T,S} = begin
    u = PTree{T}()
    for (k,v) in d
        u = setindex(u, v, k)
    end
    return u
end
#   We want to be able to construct the twig for a pair:
PTree{T}(kv::Pair{HASH_T,V}) where {T,V} = begin
    id = ptree_id(kv[1] & ~PTREE_DEPTH_MASK, PTREE_TWIG_DEPTH)
    bits = PTREE_BITS_T(0x1) << (kv[1] & PTREE_DEPTH_MASK)
    return PTree{T}(id, bits, 1, T[kv[2]])
end
PTree(kv::Pair{HASH_T,V}) where {V} = PTree{V}(kv)
#   We also construct from arrays; they are considered to be 0-indexed when used
#   with trees.
PTree{T}(u::AbstractArray{S,N}) where {T,S,N} = begin
    depth = PTREE_TWIG_DEPTH
    height = 1
    n = length(u)
    (n == 0) && return PTree{T}()
    (n == 1) && return PTree{T}(ptree_id(0x0, depth), 0x1, 1, T[u[1]])
    # Start by making all the twigs
    maxidx = fld(n + PTREE_TWIG_NCHILD - 1, PTREE_TWIG_NCHILD)
    bits = ~PTREE_BITS_T(0x0)
    nodes = Vector{PTree{T}}(undef, maxidx)
    for ii in 1:maxidx
        kk0 = (ii - 1)*PTREE_TWIG_NCHILD
        nn = min(PTREE_TWIG_NCHILD, n - kk0)
        nid = ptree_id(PTREE_TWIG_NCHILD * (ii-1), depth)
        ch = Vector{T}(undef, PTREE_TWIG_NCHILD)
        if nn == PTREE_TWIG_NCHILD
            for kk in 1:PTREE_TWIG_NCHILD
                @inbounds ch[kk] = u[kk + kk0]
            end
            nodes[ii] = PTree{T}(nid, bits, Int(PTREE_TWIG_NCHILD), ch)
        else
            for kk in 1:nn
                @inbounds ch[kk] = u[kk + kk0]
            end
            nodes[ii] = PTree{T}(nid, lowmask(nn, PTREE_BITS_T), Int(nn), ch)
        end
    end
    # Then iterate over the nodes doing the same thing.
    while true
        # Even if we are given an array with the max number of elements, we must
        # eventually have a list of only one node; that's the node we return.
        (maxidx == 1) && return nodes[1]
        # Increment
        depth -= 1
        height += 1
        n = length(nodes)
        oldnodes = nodes
        maxidx = div(n + PTREE_NODE_NCHILD - 1, PTREE_NODE_NCHILD)
        nodes = Vector{PTree{T}}(undef, maxidx)
        hnp = PTREE_NODE_NCHILD^(height-1) # number per node at the prev level
        hn  = Int(PTREE_NODE_NCHILD*hnp) # number per node at this level
        bits = ~PTREE_BITS_T(0x0)
        for ii in 1:maxidx
            kk0 = (ii - 1)*PTREE_NODE_NCHILD
            nn = min(PTREE_NODE_NCHILD, n - kk0)
            nid = ptree_id(hn * (ii-1), depth)
            ch = Vector{PTree{T}}(undef, nn)
            if nn == PTREE_NODE_NCHILD
                # split this out for faster looping
                for kk in 1:PTREE_NODE_NCHILD
                    @inbounds ch[kk] = (@inbounds oldnodes[kk + kk0])
                end
                nodes[ii] = PTree{T}(nid, bits, hn - hnp + getfield(ch[end], :numel), ch)
            else
                for kk in 1:nn
                    @inbounds ch[kk] = (@inbounds oldnodes[kk + kk0])
                end
                nodes[ii] = PTree{T}(nid, (PTREE_BITS_T(0x1) << nn) - 0x1,
                                     (nn - 1)*hnp + getfield(ch[end], :numel), ch)
            end
        end
    end
end
# Finally, we want to duplicate all of these constructors using the convert
Base.convert(::Type{PTree{T}}, u::PTree{T}) where {T} = u
Base.convert(::Type{PTree{T}}, u::PTree{U}) where {T,U} = PTree{T}(u)
Base.convert(::Type{PTree{T}}, u::AbstractDict{HASH_T,U}) where {T,U} =
    PTree{T}(u)
Base.convert(::Type{PTree{T}}, u::AbstractArray{1,U}) where {T,U} =
    PTree{T}(u)

# ==============================================================================
# Methods
#
# Definitions of methods of the above type.
Base.empty(u::PTree{T}) where {T} = PTree{T}(r)
Base.empty(u::PTree{T}, S::Type) where {T} = PTree{S}()
Base.isempty(u::PTree{T}) where {T} = (getfield(u, :numel) == 0)
Base.length(u::PTree{T}) where {T} = getfield(u, :numel)
Base.isequal(t::PTree{T}, s::PTree{S}) where {T,S} = begin
    bits = getfield(t, :bits)
    (bits == getfiield(s, :bits)) || return false
    (getfield(t, :id) == getfield(s, :id)) || return false
    tcells = getfield(t, :cells)
    scells = getfield(s, :cells)
    for k in 1:count_ones(bits)
        t = @inbounds tcells[k]
        s = @inbounds scells[k]
        isequal(t, s) || return false
    end
    return true
end
Base.copy(t::PTree{T}) where {T} = t
Base.propertynames(u::PTree{T}, private::Bool=false) where {T} =
    (:leafcount,
     :children,
     :minleaf,
     :maxleaf,
     :depth,
     :address)
Base.getproperty(u::PTree{T}, symbol::Symbol) where {T} = begin
    if symbol == :leafcount
        return getfield(u, :numel)
    elseif symbol == :children
        id = getfield(u, :id)
        (getfield(u, :numel) == 0) && return ()
        if ptree_depth(id) == PTREE_TWIG_DEPTH
            return tuple(u...)
        else
            cells = getfield(u, :cells)::Vector{PTree{T}}
            return tuple(cells...)
        end
    elseif symbol == :minleaf
        return ptree_minleaf(getfield(u, :id))
    elseif symbol == :maxleaf
        return ptree_maxleaf(getfield(u, :id))
    elseif symbol == :depth
        return ptree_depth(getfield(u, :id))
    elseif symbol == :address
        return getfield(u, :id)
    else
        throw(ArgumentError("Type PTree has no property $symbol"))
    end
end
getpair(u::PTree{T}, k::HASH_T) where {T} = begin
    # Start by making sure that the address spaces are compatible:
    # we want to make sure (once) that k is beneath u.
    id = getfield(u, :id)
    ptree_isbeneath(id, k) || return missing
    # Also, if we are empty, we need to return right away.
    (getfield(u, :numel) == 0) && return missing
    # Okay, let's try to find the child.
    while true
        # Is it in this Tree?
        (inq, bitidx, idx) = ptree_cellindex!(u, k)
        inq || return missing
        # Are we a twig?
        #if isa(getfield(u, :cells), Vector{T})
        if ptree_depth(id) == PTREE_TWIG_DEPTH
            qq = getfield(u, :cells)::Vector{T}
            return k => (@inbounds qq[idx])
        else
            q = getfield(u, :cells)::Vector{PTree{T}}
            u = (@inbounds q[idx])
        end
        id = getfield(u, :id)
    end
end
Base.get(u::PTree{T}, k::HASH_T, df) where {T} = begin
    # Start by making sure that the address spaces are compatible:
    # we want to make sure (once) that k is beneath u.
    id = getfield(u, :id)
    ptree_isbeneath(id, k) || return df
    # Also, if we are empty, we need to return right away.
    (getfield(u, :numel) == 0) && return df
    # Okay, let's try to find the child.
    while true
        # Is it in this Tree?
        (inq, bitidx, idx) = ptree_cellindex(u, k)
        inq || return df
        # Are we a twig?
        #if isa(getfield(u, :cells), Vector{T})
        if ptree_depth(id) == PTREE_TWIG_DEPTH
            qq = getfield(u, :cells)::Vector{T}
            return (@inbounds qq[idx])
        else
            q = getfield(u, :cells)::Vector{PTree{T}}
            u = (@inbounds q[idx])
        end
        id = getfield(u, :id)
    end
end
Base.in(kv::Pair{HASH_T,T}, u::PTree{T}, f::F) where {T,F<:Function} = begin
    id = getfield(u, :id)
    ptree_isbeneath(id, k) || return df
    # Also, if we are empty, we need to return right away.
    (getfield(u, :numel) == 0) && return df
    # Okay, let's try to find the child.
    while true
        # Is it in this Tree?
        (inq, bitidx, idx) = ptree_cellindex!(u, k)
        inq || return df
        # Are we a twig?
        #if isa(getfield(u, :cells), Vector{T})
        if ptree_depth(id) == PTREE_TWIG_DEPTH
            qq = getfield(u, :cells)::Vector{T}
            return f(kv[2], (@inbounds qq[idx])) == true
        else
            q = getfield(u, :cells)::Vector{PTree{T}}
            u = (@inbounds q[idx])
        end
        id = getfield(u, :id)
    end
end
Base.in(kv::Pair{HASH_T,T}, u::PTree{T}) where {T} = in(kv, u, (===))

Base.iterate(u::PTree{T}) where {T} = begin
    (getfield(u, :numel) == 0) && return nothing
    id = getfield(u, :id)
    d = ptree_depth(id)
    while d < PTREE_TWIG_DEPTH
        cells = getfield(u, :cells)::Vector{PTree{T}}
        u = cells[1]
        id = getfield(u, :id)
        d = ptree_depth(id)
    end
    leaves = getfield(u, :cells)::Vector{T}
    bits = getfield(u, :bits)
    bitno = trailing_zeros(bits)
    k = ptree_cellkey(id, bitno)
    v = leaves[1] # @inbounds leaves[1]
    return (Pair{HASH_T,T}(k,v), k)
end
iterkeys(u::PTree{T}) where {T} = begin
    (getfield(u, :numel) == 0) && return nothing
    id = getfield(u, :id)
    d = ptree_depth(id)
    while d < PTREE_TWIG_DEPTH
        cells = getfield(u, :cells)::Vector{PTree{T}}
        u = cells[1]
        id = getfield(u, :id)
        d = ptree_depth(id)
    end
    leaves = getfield(u, :cells)::Vector{T}
    bits = getfield(u, :bits)
    bitno = trailing_zeros(bits)
    k = ptree_cellkey(id, bitno)
    v = leaves[1] # @inounds leaves[1]
    return (k, k)
end
itervals(u::PTree{T}) where {T} = begin
    (getfield(u, :numel) == 0) && return nothing
    id = getfield(u, :id)
    d = ptree_depth(id)
    while d < PTREE_TWIG_DEPTH
        cells = getfield(u, :cells)::Vector{PTree{T}}
        u = cells[1]
        id = getfield(u, :id)
        d = ptree_depth(id)
    end
    leaves = getfield(u, :cells)::Vector{T}
    bits = getfield(u, :bits)
    bitno = trailing_zeros(bits)
    k = ptree_cellkey(id, bitno)
    v = leaves[1] # @inounds leaves[1]
    return (v, k)
end

macro _ptree_iterate_gencode(rtype::Symbol)
    cids = [gensym("cid") for _ in 1:PTREE_LEVELS-1]
    cels = [gensym("cel") for _ in 1:PTREE_LEVELS-1]
    bits = [gensym("bit") for _ in 1:PTREE_LEVELS-1]
    if rtype == :keys
        rexpr = :(k)
        fn = :iterkeys
    elseif rtype == :vals
        rexpr = :(v)
        fn = :itervals
    else
        rexpr = :(Pair{HASH_T,T}(k,v))
        fn = :(Base.iterate)
    end
    twigexpr = quote
        # We must be a twig at this point.
        leaves = getfield(u, :cells)::Vector{T}
        id = getfield(u, :id)
        bits = getfield(u, :bits)
        (inq, bitno, idx) = ptree_cellindex(id, bits, k0)
        mask = (BITS_ONE << (bitno + 1)) - BITS_ONE
        nextbitno = trailing_zeros(bits & ~mask)
        if nextbitno < PTREE_BITS_BITCOUNT
            k = ptree_cellkey(id, nextbitno)
            idx = count_ones(bits & mask) + 1
            v = (@inbounds leaves[idx])
            return ($rexpr, k)
        end
        # If we reach here, we did not find a next leaf.
    end
    # At the lowest level, the try and twig expressions are the same.
    tryexpr = twigexpr
    for ii in PTREE_LEVELS-1:-1:1
        cel = cels[ii]
        cid = cids[ii]
        bit = bits[ii]
        tryexpr = quote
            if d == PTREE_TWIG_DEPTH
                $twigexpr
            else
                $cel = getfield(u, :cells)::Vector{PTree{T}}
                $bit = getfield(u, :bits)
                (inq, bitno, $cid) = ptree_cellindex(id, $bit, k0)
                u = $cel[$cid]
                id = getfield(u, :id)
                d = ptree_depth(id)
                $tryexpr
                # If we reach this point, we haven't found a next pair yet.
                n = count_ones($bit)
                if $cid < n
                    u = $cel[$cid + 1]
                    break
                end
            end
        end
    end
    # Put these together into the functions.
    return quote
        $fn(u::PTree{T}, k0::HASH_T) where {T} = begin
            id = getfield(u, :id)
            d = ptree_depth(id)
            while true
                $tryexpr
                # If we reach the end of the loop, there's nothing past k0.
                return nothing
            end
            # Upon finding a cell that hasn't been iterated, the code breaks
            # from the abbove loop and brings us here.
            id = getfield(u, :id)
            d = ptree_depth(id)
            while d < PTREE_TWIG_DEPTH
                cells = getfield(u, :cells)::Vector{PTree{T}}
                u = (@inbounds cells[1])
                id = getfield(u, :id)
                d = ptree_depth(id)
            end
            leaves = getfield(u, :cells)::Vector{T}
            bits = getfield(u, :bits)
            nextbitno = trailing_zeros(bits)
            k = ptree_cellkey(id, nextbitno)
            v = (@inbounds leaves[1])
            return ($rexpr, k)
        end
    end |> esc
end
# Run the macro to generate the iteration functions:
(@_ptree_iterate_gencode keys)
(@_ptree_iterate_gencode vals)
(@_ptree_iterate_gencode pairs)

setindex(u::PTree{T}, v::V, k::HASH_T) where {T,V} = begin
    # First of all, if this node is empty, we just return a new node
    (getfield(u, :numel) == 0) && return PTree{T}(k => v)
    # Next, if k is a child of this node, we return a new ancestor node that
    # contains us both and is the highest possible ancestor that contains us
    # both.
    id = getfield(u, :id)
    if !ptree_isbeneath(id, k)
        # Make the new twig that will hold k => v.
        kv = PTree{T}(k => v)
        # What's the lowest bit at which the mismatch in address starts?
        topmiss = ptree_highbitdiff(id, k)
        if topmiss <= HASH_BITCOUNT - PTREE_ROOT_SHIFT
            nodelev = div(topmiss - PTREE_TWIG_SHIFT, PTREE_NODE_SHIFT)
            bit0 = PTREE_TWIG_SHIFT + nodelev * PTREE_NODE_SHIFT
            shift = PTREE_NODE_SHIFT
            newdepth = PTREE_LEVELS - 2 - nodelev
        else
            newdepth = 0
            bit0 = HASH_BITCOUNT - PTREE_ROOT_SHIFT
            shift = PTREE_ROOT_SHIFT
        end
        newminleaf = id & ~((HASH_ONE << bit0) - HASH_ONE)
        newid = ptree_id(newminleaf, newdepth)
        newmask = (HASH_ONE << shift) - HASH_ONE
        ii_k = newmask & (id >> bit0)
        ii_u = newmask & (k >> bit0)
        newbits = (BITS_ONE << ii_k) | (BITS_ONE << ii_u)
        newchs = ii_u < ii_k ? PTree{T}[kv, u] : PTree{T}[u, kv]
        return PTree{T}(newid, newbits, 1 + getfield(u, :numel), newchs)
    end
    # Otherwise, this key goes beneath this node, so find out where.
    bits = getfield(u, :bits)
    numel = getfield(u, :numel)
    (bit0, shift) = ptree_bitshift(id)
    # Grab the index out of the leaf id.
    idx = (k >> bit0) & lowmask(shift)
    # We'll want to see if this bit is set and then insert/edit at this index:
    flag = BITS_ONE << idx
    idx = 1 + count_ones(bits & lowmask(idx))
    # However, what we do next depends on whether we are in a twig node or not.
    if ptree_depth(id) == PTREE_TWIG_DEPTH
        cells = getfield(u, :cells)::Vector{T}
        if bits & flag == flag
            # We are replacing an item
            return PTree{T}(id, bits, numel, setindex(cells, v, idx))
        else
            # If we get here, we are adding a new node, which means we are
            # inserting a leaf.
            return PTree{T}(id, bits | flag, numel + 1, insert(cells, idx, v))
        end
    else
        cells = getfield(u, :cells)::Vector{PTree{T}}
        if bits & flag == flag
            # We pass control along then return a copy of ourselves on the
            # way back up the control stack.
            oldc = cells[idx]
            oldn = getfield(oldc, :numel)
            newc = setindex(cells[idx], v, k)
            (newc === cells[idx]) && return u
            newn = getfield(newc, :numel)
            newcells = setindex(cells, newc, idx)
            return PTree{T}(id, bits, numel - oldn + newn, newcells)
        else
            # We add a twig beneath this node.
            newcells = insert(cells, idx, PTree{T}(k => v))
            return PTree{T}(id, bits | flag, numel + 1, newcells)
        end
    end
end
delete(u::PTree{T}, k::HASH_T) where {T} = begin
    id = getfield(u, :id)
    bits = getfield(u, :bits)
    (inq, bitidx, idx) = ptree_cellindex(id, bits, k)
    # If the node lies outside of this node or the bit isn't set, there's
    # nothing to do.
    inq || return u
    # There's something to delete; start by determining if we are or are not a
    # twig node:
    numel = getfield(u, :numel)
    # Check if we're a twig or something else.
    if ptree_depth(id) == PTREE_TWIG_DEPTH
        # If we are deleting the only element, we return a new empty node.
        (numel == 1) && return PTree{T}()
        cells = getfield(u, :cells)::Vector{T}
        newcells = delete(cells, idx)
        return PTree{T}(id, bits & ~(BITS_ONE << bitidx), numel-1, newcells)
    else
        cells = getfield(u, :cells)::Vector{PTree{T}}
        oldc = cells[idx]
        oldn = getfield(oldc, :numel)
        newc = delete(oldc, k)
        newn = getfield(newc, :numel)
        numel += newn - oldn
        (numel == 0) && return newc
        if newn == 0
            newcells = delete(cells, idx)
            return PTree{T}(id, bits & ~(BITS_ONE << bitidx), numel, newcells)
        else
            newcells = setindex(cells, newc, idx)
            return PTree{T}(id, bits, numel, newcells)
        end
    end
end


text:
using Oceananigans
using Oceananigans.BoundaryConditions: fill_halo_regions!, ImpenetrableBoundaryCondition
using Printf
using GLMakie

grid = RectilinearGrid(size=128, z=(-10, 10), topology=(Flat, Flat, Bounded))
sinking = AdvectiveForcing(WENO5(), w=-1)
rising = AdvectiveForcing(WENO5(), w=+1)

b_to_a(x, y, z, t, a, b) = + a * b
a_to_b(x, y, z, t, a, b) = - a * b

a_reaction = Forcing(a_to_b, field_dependencies=(:a, :b))
b_reaction = Forcing(b_to_a, field_dependencies=(:a, :b))

model = HydrostaticFreeSurfaceModel(; grid,
                                    velocities = nothing,
                                    tracers = (:a, :b),
                                    buoyancy = nothing,
                                    closure = ScalarDiffusivity(κ=1e-2),
                                    forcing = (a=(a_reaction, sinking), b=(b_reaction, rising)))

aᵢ(x, y, z) = exp(-(z - 4)^2)
bᵢ(x, y, z) = exp(-(z + 4)^2)
set!(model, a=aᵢ, b=bᵢ)

simulation = Simulation(model; Δt=1e-2, stop_iteration=0)
progress(sim) = @info @sprintf("Iter: %d, time: %.2e", iteration(sim), time(sim))
simulation.callbacks[:progress] = Callback(progress, IterationInterval(10))

z = znodes(Center, grid)
a = interior(model.tracers.a, 1, 1, :)
b = interior(model.tracers.b, 1, 1, :)

fig = Figure()
ax = Axis(fig[1, 1],
          xlabel = "Reactant concentration",
          ylabel = "z",
          title = "Tracer reactions at t=0")
xlims!(ax, -1, 4)

ℓa = lines!(ax, a, z, label="a")
ℓb = lines!(ax, b, z, label="b")

axislegend(ax, position=:rb)

display(fig)

function update_plot!(sim)
    ℓa.input_args[1][] = interior(sim.model.tracers.a, 1, 1, :)
    ℓb.input_args[1][] = interior(sim.model.tracers.b, 1, 1, :)
    ax.title[] = @sprintf("Tracer reactions at t=%.2e", time(sim))
end

record(fig, "tracer_reactions.mp4", 1:100, framerate=24) do nn
    simulation.stop_iteration += 10
    run!(simulation)
    update_plot!(simulation)
end


text:
"""
Simulate `n` points in 2D space distributed over `c` nested circles.

# Keyword Arguments

- `rng=StableRNG(1234)`: A random number generator for reproducibility.
- `p=8//10`: A prior probability on how well a sample's position predicts its class (Bayes error).
"""
function simulate_nested_circles(n::Int, c::Int=2; p::Real=8//10, rng::AbstractRNG=StableRNG(1234))
    # allocate outputs
    X = Matrix{Float64}(undef, n, 2)
    class = Vector{Int}(undef, n)

    # for each sample...
    for i in axes(X, 1)
        # generate a point (x, y) via polar coordinates
        r = sqrt(c * rand(rng))
        θ = 2*π * rand(rng)
        x = r * cos(θ)
        y = r * sin(θ)

        X[i, 1] = x
        X[i, 2] = y

        # assign a class via an intermediate point and the Bayes error, p
        class_bayes = ceil(Int, x^2 + y^2)

        if rand(rng) ≤ p # accept proposed class
            class[i] = class_bayes
        else # assign a random class uniformly over all remaining classes
            class[i] = rand(rng, setdiff(1:c, class_bayes))
        end
    end

    return class, X
end

"""
Simulate `n` waveform data points with `p` features, over 3 classes.

# Keyword Arguments

- `rng=StableRNG(1234)`: A random number generator for reproducibility.
- `m=6`: Waveform's peak.
- `r=4`: Shift parameter.
- `s=11`: Shift parameter.
"""
function simulate_waveform(n::Int, p::Int; m::Int=6, r::Int=4, s::Int=11, rng::AbstractRNG=StableRNG(1234))
    # allocate outputs
    X = Matrix{Float64}(undef, n, p)
    class = Vector{Int}(undef, n)

    # for each sample...
    for i in axes(X, 1)
        u₁ = rand(rng)
        u₂ = rand(rng)
        h₁(t) = max(m - abs(t-s), 0)
        h₂(t) = h₁(t-r)
        h₃(t) = h₁(t+r)
        if u₁ ≤ 1//3
            class[i] = 1
            for j in axes(X, 2)
                ϵ = randn(rng)
                X[i,j] = u₂*h₁(j) + (1-u₂)*h₂(j) + ϵ
            end
        elseif 1//3 < u₁ ≤ 2//3
            class[i] = 2
            for j in axes(X, 2)
                ϵ = randn(rng)
                X[i,j] = u₂*h₁(j) + (1-u₂)*h₃(j) + ϵ
            end
        else
            class[i] = 3
            for j in axes(X, 2)
                ϵ = randn(rng)
                X[i,j] = u₂*h₂(j) + (1-u₂)*h₃(j) + ϵ
            end
        end
    end

    return class, X
end

"""
Simulate `p`-dimensional feature vectors equally distributed over `c` classes with known effect sizes.

Only a subset of features are informative in predicting a sample's class. Both degree of class separation `d` and noise level `sigma` affect class masking.

# Keyword Arguments

- `rng=StableRNG(1234)`: A random number generator for reproducibility.
- `d=1.0`: Degree of class separation, with larger values correspoding to well-separated classes.
- `sigma=1.0`: Standard deviation of Normal distribution used to seed features.
- `ncausal=2`: The number of informative features.
"""
function simulate_ground_truth(p::Int, c::Int, samples_per_class::Int;
    rng::AbstractRNG=StableRNG(1234),
    d::Real=1.0,
    sigma::Real=1.0)
    # Sanity checks
    any(≤(0), (p, c, samples_per_class)) && error("Values for `n`, `p`, and `samples_per_class` must be positive.")
    d < 0 && error("Class separation `d` must be a nonnegative value.")
    sigma ≤ 0 && error("Noise level `sigma` must be a positive value.")

    A1 = ( 1 + sqrt(c) ) / ( (c-1)^(3/2) )
    A2 = sqrt( c / (c-1) )
    vertex = Vector{Vector{Float64}}(undef, c)
    vertex[1] = 1 / sqrt(c-1) * ones(c-1)
    for j in 2:c
        v = -A1 * ones(c-1)
        v[j-1] += A2
        vertex[j] = v
    end

    # Initialize with iid Normal deviate samples, N(0, σ²)
    n = c * samples_per_class
    X = 1/sigma * randn(rng, n, p)
    B = zeros(p, c-1)
    for i in 1:c
        idx = samples_per_class*(i-1)+1:samples_per_class*i
        θ = π/3 + 2*(i-1)*π/c
        a = iseven(i) ? cos(θ) : sin(θ)
        X[idx,i] .= d * sign(a) .+ a * sigma * randn(rng, samples_per_class)
        B[i,:] .= sign(a) * vertex[i] / c
    end

    # Assign classes and shuffle samples around.
    Y = X*B
    targets = map(y -> argmin([max(0, norm(y-v)) for v in vertex]), eachrow(Y))

    idx = randperm(rng, n)
    # targets, X = targets[idx], X[idx,:]
    return targets, X, B
end

"""
Simulate `p`-dimensional feature vectors equally distributed over `c` based on a recipe from Wang & Shen 2007.

Only two predictors are informative. Both degree of class separation `d` and noise level `sigma` affect class masking.

# Keyword Arguments

- `rng=StableRNG(1234)`: A random number generator for reproducibility.
- `d=1.0`: Degree of class separation, with larger values correspoding to well-separated classes.
- `sigma=1.0`: Standard deviation of Normal distribution used to seed features.
"""
function simulate_WS2007(p::Int, c::Int, samples_per_class::Int;
    rng::AbstractRNG=StableRNG(1234),
    d::Real=1.0,
    sigma::Real=1.0,)
    # Sanity checks
    any(≤(0), (p, c, samples_per_class)) && error("Values for `n`, `p`, and `samples_per_class` must be positive.")
    d < 0 && error("Class separation `d` must be a nonnegative value.")
    sigma ≤ 0 && error("Noise level `sigma` must be a positive value.")

    # Initialize with iid Normal deviate samples, N(0, σ²)
    n = c * samples_per_class
    targets = zeros(Int, n)
    X = sigma * randn(rng, n, p)
    for i in 1:c
        idx = samples_per_class*(i-1)+1:samples_per_class*i

        # Causal predictors
        θ = π/3 + 2*(i-1)*π/c
        X[idx,1] .= d*cos(θ)
        X[idx,2] .= d*sin(θ)

        # Class assignment
        targets[idx] .= i
    end

    # Shuffle samples around.
    idx = randperm(rng, n)
    targets, X = targets[idx], X[idx,:]
    return targets, X
end

"""
Simulate Gaussian point data with `n` samples and `c` classes.

# Keyword Arguments

- `rng=StableRNG(1234)`: A random number generator for reproducibility.
- `sigma=1.0`: Standard deviation for Normal distribution.
"""
function simulate_gaussian_clouds(n::Int, c::Int=3; sigma::Real=1.0, rng::AbstractRNG=StableRNG(1234))
    # Define one half of cluster centers.
    centers = [(k, (k-1)*π / c) for k in 1:c]

    # Initialize data matrix with iid N(0,1) variates
    target, X = Vector{Int}(undef, n), randn(rng, n, 2)
    σ = sigma

    for i in axes(X, 1)
        # Sample a center uniformly from the three classes and randomly reflect it.
        (class, θ) = rand(rng, centers)
        rand(rng, (true, false)) && (θ += π)

        X[i,1] = cos(θ) + σ * X[i,1]
        X[i,2] = sin(θ) + σ * X[i,2]
        target[i] = class
    end

    return target, X
end


text:
### A Pluto.jl notebook ###
# v0.14.5

using Markdown
using InteractiveUtils

# ╔═╡ 51ab86a0-af67-11eb-2542-018f2a541d80
begin
	using LinearAlgebra
	using Plots
	using Printf
	using IJulia
end

# ╔═╡ 13dac5b4-d8bb-4668-9aa8-1d2a5ce6b59d
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
function toeplitz(el,e,eu,n)
	dl = el*ones(n-1);
	du = eu*ones(n-1);
	d = e * ones(n);
	A = Tridiagonal(dl,d,du);
	return A;
end
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

# ╔═╡ de0276af-306d-4758-96b0-d76e024a5291
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
function generate_b(n)
	b = ones(n);
	return b;
end
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

# ╔═╡ ae0165b3-6a37-4da5-bb07-22c3cce19a7b
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
function jacobi(A,b,u0)
	iter = 0;
	res = 1;
	iterations = 1e3;
	eps = 1e-12;
	n = length(b);
	y_old = u0;
	y_new = zeros(n);
	logs_res = zeros(Int(iterations));
	while(iter<iterations && res>eps)
		for i=1:n
			e = zeros(n);
			MM = zeros(n);
			for j=1:n
				MM[i] += A[i,j]*y_old[j];
			end
			e[i] = (1/A[i,i])*(b[i] - MM[i]);
			y_new[i] = y_old[i] + e[i];

		end
		y_old = y_new;
		res = norm(b - A*y_old,2);
		iter = iter + 1;
		logs_res[iter] = res;
	end
	#printf("Exiting at iterations number %d\n",iter);
	return y_new,logs_res;
end
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

# ╔═╡ b17e0683-c34c-43e7-9170-cd8a5a293a17
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

function jacobi_ω(A,b,x0,ω)
	iterations = 1e3;
	eps = 1e-12;
	iter = 0;
	res = 1;
	n = length(b);
	y_old = x0;
	y_new = y_old;
	logs_res = zeros(Int(iterations));
	while(res>eps && iter<iterations)
		for i=1:n
			e = zeros(n);
			MM = zeros(n);
			for j=1:n
				MM[i] += A[i,j]*y_old[j];
			end
			e[i] = (ω/A[i,i])*(b[i] - MM[i]);
			y_new[i] = y_old[i] + e[i];
		end
		y_old = y_new;
		res = norm(b-A*y_old,1);
		iter = iter + 1;
		logs_res[iter] = res;
	end
	return y_new , logs_res;
end
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

# ╔═╡ b38689ac-e045-4ae6-845f-e11a8a902a39
begin
	# Main function
	n = 25;
	A = toeplitz(-1,2,-1,n);
	b = generate_b(n);
	iterations = 1e3;
	domain = 1:iterations;
	IC = ones(n);
	ω = 0.5;
	ω1 = 1.7;
	ω_range = 0.1:0.2:1;
	ω1_range = 0.1:0.3:2;
end

# ╔═╡ acb70cc7-8a6c-4eb2-8451-c3637a36b129
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

function gauss(A,b,x0)
	iter = 0 ;
	eps = 1e-12;
	res = 1;
	iterations = 1e3;
	y = x0;
	logs_res = zeros(Int(iterations));
	while(iter<iterations && res > eps)
		for i=1:n
			e = zeros(n);
			MM = zeros(n);
			for j=1:n
				MM[i] += A[i,j]*y[j];
			end
			e[i] = (1/A[i,i])*(b[i] - MM[i]);
			y[i] = y[i] + e[i];
		end
		res = norm(b - A*y,2);
		iter = iter + 1;
		logs_res[iter] = res;
	end
	return y,logs_res;
end
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

# ╔═╡ 944d3e05-1ddc-422c-9207-a819f5341254
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
function gauss_ω(A,b,x0,ω)
	iter = 0 ;
	eps = 1e-12;
	res = 1;
	iterations = 1e3;
	y = x0;
	logs_res = zeros(Int(iterations));
	while(iter<iterations && res > eps)
		for i=1:n
			e = zeros(n);
			MM = zeros(n);
			for j=1:n
				MM[i] += A[i,j]*y[j];
			end
			e[i] = (ω/A[i,i])*(b[i] - MM[i]);
			y[i] = y[i] + e[i];
		end
		res = norm(b - A*y,1);
		iter = iter + 1;
		logs_res[iter] = res;
	end
	return y,logs_res;
end
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

# ╔═╡ 827ecd18-8b80-4fe6-b6e1-ed19fcb071cc
begin
	# Analytical solutions
	u = inv(A)*b;
	u = vcat(0,u,0)  # include BC to the solution
	x = LinRange(0,1,n+2);
	plot(x,u,label="Actual solution")
end

# ╔═╡ 4240bfe8-eaef-454a-8113-68572ef822f2
# Jacobi's and Gauss' solution and their weighted solutions

begin
	y,RES_j = jacobi(A,b,IC)
	plot(domain,RES_j,xaxis=:log,label="Jacobi");
	y_g,RES = gauss(A,b,IC)
	plot!(domain,RES,xaxis=:log,label="Gauss");
end

# ╔═╡ 3d9f8d64-c18f-40e4-8510-be3619caa190
# Weighted Jacobi and Gauss Methods
begin
	y_j,RES_ja = jacobi_ω(A,b,IC,ω);
	plot(domain,RES_ja,xaxis=:log,label="Weighted Jacobi");
	y_gr,RES_ga = gauss_ω(A,b,IC,ω1);
	plot!(domain,RES_ga,xaxis=:log,label="Weighted Gauss");
end

# ╔═╡ 2e83adfa-c254-4a4f-9681-c140ed64b0dd
begin
	y_,R = jacobi_ω(A,b,IC,ω_range[1]);
	p1 = plot!(domain,R,xaxis=:log);
end

# ╔═╡ 7c9839b6-171d-4039-b5cb-7cf2a8278fac
begin
	y_1,R1 = gauss_ω(A,b,IC,ω_range[5]);
	p2 = plot!(domain,R1,xaxis=:log);
end

# ╔═╡ Cell order:
# ╠═51ab86a0-af67-11eb-2542-018f2a541d80
# ╠═13dac5b4-d8bb-4668-9aa8-1d2a5ce6b59d
# ╠═de0276af-306d-4758-96b0-d76e024a5291
# ╠═ae0165b3-6a37-4da5-bb07-22c3cce19a7b
# ╠═acb70cc7-8a6c-4eb2-8451-c3637a36b129
# ╠═b17e0683-c34c-43e7-9170-cd8a5a293a17
# ╠═944d3e05-1ddc-422c-9207-a819f5341254
# ╠═b38689ac-e045-4ae6-845f-e11a8a902a39
# ╠═827ecd18-8b80-4fe6-b6e1-ed19fcb071cc
# ╠═4240bfe8-eaef-454a-8113-68572ef822f2
# ╠═3d9f8d64-c18f-40e4-8510-be3619caa190
# ╠═2e83adfa-c254-4a4f-9681-c140ed64b0dd
# ╠═7c9839b6-171d-4039-b5cb-7cf2a8278fac


text:
#=
Created on 31/01/2021 16:19:17
Last update: -

@author: Michiel Stock
michielfmstock@gmail.com

Illustrating performance improvements on 2D convolution.
=#

using BenchmarkTools

function convolve_2d(M::Matrix, K::Matrix)
	out = similar(M)
	n_rows, n_cols = size(M)
	fill!(out, 0.0)
	m = div(size(K, 1), 2)
	for i in 1:n_rows
		for j in 1:n_cols
			for k in -m:m
				for l in -m:m
					out[i,j] += M[clamp(i+k, 1, n_rows), clamp(j+l, 1, n_cols)] * K[k+m+1,l+m+1]
				end
			end
		end
	end
	return out
end

function convolve_2d2(M::Matrix, K::Matrix)
	out = similar(M)
	n_rows, n_cols = size(M)
	fill!(out, 0.0)
	m = div(size(K, 1), 2)
	for j in 1:n_rows
		for i in 1:n_cols
			for l in -m:m
				for k in -m:m
					out[i,j] += M[clamp(i+k, 1, n_rows), clamp(j+l, 1, n_cols)] * K[k+m+1,l+m+1]
				end
			end
		end
	end
	return out
end

function convolve_2d3(M::Matrix, K::Matrix)
	out = similar(M)
	n_rows, n_cols = size(M)
	fill!(out, 0.0)
	m = div(size(K, 1), 2)
	for j in 1:n_rows
		for i in 1:n_cols
			for l in -m:m
				for k in -m:m
					@inbounds out[i,j] += M[clamp(i+k, 1, n_rows), clamp(j+l, 1, n_cols)] * K[k+m+1,l+m+1]
				end
			end
		end
	end
	return out
end

function convolve_2d4(M::Matrix, K::Matrix)
	out = similar(M)
	n_rows, n_cols = size(M)
	fill!(out, 0.0)
	m = div(size(K, 1), 2)
	@simd for j in 1:n_rows
		@simd for i in 1:n_cols
			@simd for l in -m:m
				@simd for k in -m:m
					out[i,j] += M[clamp(i+k, 1, n_rows), clamp(j+l, 1, n_cols)] * K[k+m+1,l+m+1]
				end
			end
		end
	end
	return out
end

function convolve_2d5(M::Matrix, K::Matrix)
	out = similar(M)
	n_rows, n_cols = size(M)
	fill!(out, 0.0)
	m = div(size(K, 1), 2)
	@simd for j in 1:n_rows
		@simd for i in 1:n_cols
			@simd for l in -m:m
				@simd for k in -m:m
					@inbounds out[i,j] += M[clamp(i+k, 1, n_rows), clamp(j+l, 1, n_cols)] * K[k+m+1,l+m+1]
				end
			end
		end
	end
	return out
end

M = randn(1000, 1000)
K = rand(5, 5)

@btime convolve_2d($M, $K);
@btime convolve_2d2($M, $K);
@btime convolve_2d3($M, $K);
@btime convolve_2d4($M, $K);
@btime convolve_2d5($M, $K);