module Test_analysis

using DescriptorSystems
using LinearAlgebra
using Polynomials
using Test

println("Test_analysis")
@testset "zeros, rank, poles" begin

# test example for SLICOT subroutine AB08ND
A = [
   1.0   0.0   0.0   0.0   0.0   0.0
   0.0   1.0   0.0   0.0   0.0   0.0
   0.0   0.0   3.0   0.0   0.0   0.0
   0.0   0.0   0.0  -4.0   0.0   0.0
   0.0   0.0   0.0   0.0  -1.0   0.0
   0.0   0.0   0.0   0.0   0.0   3.0];
E = I;
B = [
   0.0  -1.0
  -1.0   0.0
   1.0  -1.0
   0.0   0.0
   0.0   1.0
  -1.0  -1.0];
C = [
   1.0   0.0   0.0   1.0   0.0   0.0
   0.0   1.0   0.0   1.0   0.0   1.0
   0.0   0.0   1.0   0.0   0.0   1.0];
D = [
   0.0   0.0
   0.0   0.0
   0.0   0.0];
sys = dss(A,E,B,C,D);

# zeros
@time val  = gzero(sys)
@test sort(real(val)) ≈ [-1, 2, Inf,Inf]

@time val  = gzero(sys[:,1])
@test sort(real(val)) ≈ [-1, 2, Inf]

@time val, info  = gzeroinfo(sys)
@test sort(real(val)) ≈ [-1, 2, Inf,Inf]  &&
      info.nfz == 2 && info.niev == 4 && info.nisev == 2 && info.niz == 2 &&
      info.nfsz == 1 && info.nfsbz == 0 && info.nfuz == 1 &&
      info.nrank == 8 && info.miev == [2, 2] && info.miz == [1, 1] &&
      info.rki == Int64[] && info.lki == [2] && !info.regular && !info.stable

@time val, info  = gzeroinfo(dss(A,E,B,C,D,Ts=-1))
@test sort(real(val)) ≈ [-1, 2, Inf,Inf]  &&
      info.nfz == 2 && info.niev == 4 && info.nisev == 2 && info.niz == 2 &&
      info.nfsz == 0 && info.nfsbz == 1 && info.nfuz == 1 &&
      info.nrank == 8 && info.miev == [2, 2] && info.miz == [1, 1] &&
      info.rki == Int64[] && info.lki == [2] && !info.regular && !info.stable

# poles
@time val  = gpole(sys)
@test sort(real(val)) ≈ sort([1.0, 1.0, 3.0, -4.0, -1.0, 3.0])

@time val  = gzero(dss(A = sys.A, E = sys.E))
@test sort(real(val)) ≈ sort([1.0, 1.0, 3.0, -4.0, -1.0, 3.0])

@test !isstable(sys)
@test isstable(dss(A = sys.A-5I))

@time val, info  = gpoleinfo(sys)
@test sort(real(val)) ≈ sort([1.0, 1.0, 3.0, -4.0, -1.0, 3.0]) &&
      info.nfev == 6 && info.nfsev == 2 && info.nfuev == 4

# rank
@test gnrank(sys, fastrank = true) == 2 && gnrank(sys, fastrank = false) == 2

@test gnrank(sys[:,1], fastrank = true) == 1 && gnrank(sys[:,1], fastrank = false) == 1

# output decoupling zeros
@time val  = gzero(dss(A = sys.A,E = sys.E,C = sys.C))
@test val ≈ [-1]

# input decoupling zeros
@time val  = gzero(dss(A = sys.A,E = sys.E,B = sys.B))
@test val ≈ [-4]

# test example for SLICOT subroutine AB08ND
A = [
    1     0     0     0     0     0     0     0     0
    0     1     0     0     0     0     0     0     0
    0     0     1     0     0     0     0     0     0
    0     0     0     1     0     0     0     0     0
    0     0     0     0     1     0     0     0     0
    0     0     0     0     0     1     0     0     0
    0     0     0     0     0     0     1     0     0
    0     0     0     0     0     0     0     1     0
    0     0     0     0     0     0     0     0     1];
E = [
    0     0     0     0     0     0     0     0     0
    1     0     0     0     0     0     0     0     0
    0     1     0     0     0     0     0     0     0
    0     0     0     0     0     0     0     0     0
    0     0     0     1     0     0     0     0     0
    0     0     0     0     1     0     0     0     0
    0     0     0     0     0     0     0     0     0
    0     0     0     0     0     0     1     0     0
    0     0     0     0     0     0     0     1     0];
B = [
    -1     0     0
     0     0     0
     0     0     0
     0    -1     0
     0     0     0
     0     0     0
     0     0    -1
     0     0     0
     0     0     0];
C = [
    0     1     1     0     3     4     0     0     2
    0     1     0     0     4     0     0     2     0
    0     0     1     0    -1     4     0    -2     2];
D = [
    1     2    -2
    0    -1    -2
    0     0     0];

sys = dss(A,E,B,C,D);

# zeros
@time val  = gzero(sys)
@test val ≈ [1, Inf,Inf]

@time val, info  = gzeroinfo(sys)
@test val ≈ [1, Inf,Inf]  &&
      info.nfz == 1 && info.niev == 7 && info.nisev == 1 && info.niz == 2 &&
      info.nfsz == 0 && info.nfsbz == 0 && info.nfuz == 1 &&
      info.nrank == 11 && info.miev == [1, 1, 1, 1, 3] && info.miz == [2] &&
      info.rki == [2] && info.lki == [1] && !info.regular && !info.stable

@time val, info  = gzeroinfo(dss(A,E,B,C,D,Ts=-1))
@test val ≈ [1, Inf,Inf]  &&
      info.nfz == 1 && info.niev == 7 && info.nisev == 1 && info.niz == 2 &&
      info.nfsz == 0 && info.nfsbz == 1 && info.nfuz == 0 &&
      info.nrank == 11 && info.miev == [1, 1, 1, 1, 3] && info.miz == [2] &&
      info.rki == [2] && info.lki == [1] && !info.regular && !info.stable

# poles
@time val  = gpole(sys,check_reg = true)
@test val ≈ [Inf, Inf, Inf, Inf, Inf, Inf]

@time val  = gzero(dss(A = sys.A, E = sys.E))
@test val ≈ [Inf, Inf, Inf, Inf, Inf, Inf]

@test !isstable(sys)
@test isstable(dss(A = sys.E, E = sys.A, Ts=-1))

@time val, info  = gpoleinfo(sys)
@test val ≈ [Inf, Inf, Inf, Inf, Inf, Inf] &&
      info.nfev == 0 && info.nfsev == 0 && info.nfuev == 0 &&
      info.niev == 9 && info.nip == 6 && info.miev == [3, 3, 3] && info.mip == [2, 2, 2] &&
      info.regular && !info.proper && !info.stable

@test gnrank(sys, atol = 1.e-7, fastrank = true) == 2 && gnrank(sys, atol = 1.e-7, fastrank = false) ==2

@test gnrank(sys[:,1], fastrank = true) == 1 && gnrank(sys[:,1], fastrank = false) == 1

# output decoupling zeros
@time val  = gzero(dss(A = sys.A,E = sys.E,C = sys.C))
@test val ≈ [Inf, Inf, Inf, Inf]

# input decoupling zeros
@time val  = gzero(dss(A = sys.A,E = sys.E,B = sys.B))
@test val ≈ Float64[]

# stability tests
@test isstable(rdss(5,3,3, stable = true))
@test isstable(rdss(5,3,3, id = ones(Int,3), stable = true), atol = 1.e-7)

# infinite structure test
n = 5; m = 2; p = 3;
sys = rdss(n,p,m,id=[ones(Int,3);2*ones(Int,2)]);
@time val, info  = gpoleinfo(sys,atol=1.e-7)
@test count(isinf.(val)) == 2 &&
      info.nfev == n && info.niev == 7 && info.nisev == 3 && info.nfsev + info.nfuev == n &&
      info.nip == 2 && info.miev == [1, 1, 1, 2, 2] && info.mip == [1, 1] &&
      info.regular && !info.proper && isregular(sys)

A = rand(n,n); E = rand(n,n);
@time val, info  = gpoleinfo(dss(A=A,E=E))
@test count(isinf.(val)) == 0 &&
      info.nfev == n && info.niev == 0 && info.nisev == 0 && info.nfsev + info.nfuev == n &&
      info.nip == 0 && info.miev == [] && info.mip == [] &&
      info.regular && info.proper

As, Es, = schur(A,E)
@time val, info  = gpoleinfo(dss(A=As,E=Es))
@test count(isinf.(val)) == 0 &&
      info.nfev == n && info.niev == 0 && info.nisev == 0 && info.nfsev + info.nfuev == n &&
      info.nip == 0 && info.miev == [] && info.mip == [] &&
      info.regular && info.proper

@time val, info  = gpoleinfo(dss(A=A,E=triu(E)))
@test count(isinf.(val)) == 0 &&
      info.nfev == n && info.niev == 0 && info.nisev == 0 && info.nfsev + info.nfuev == n &&
      info.nip == 0 && info.miev == [] && info.mip == [] &&
      info.regular && info.proper

end

@testset "gl2norm & gh2norm" begin

sys = rdss(0,0,0);
@time l2norm = gl2norm(sys)
@test l2norm == 0

sys = rdss(0,2,2);
@time l2norm = gl2norm(sys)
@test l2norm == Inf

sys = rdss(0,2,2,disc=true);
@time l2norm = gl2norm(sys)
@test l2norm == norm(sys.D)

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d);
@time l2norm = gh2norm(sys)
@test l2norm ≈ 1.419727086450068e+01

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
e = rand(2,2);
sys = dss(e*a,e,e*b,c,d);
@time l2norm = gh2norm(sys)
@test l2norm ≈ 1.419727086450068e+01

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d);
@time l2norm = gl2norm(sys)
@test l2norm ≈ 1.419727086450068e+01

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d);
@time l2norm = opnorm(sys,2)
@test l2norm ≈ 1.419727086450068e+01

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
e = rand(2,2)
sys = dss(e*a,e,e*b,c,d);
@time l2norm = gl2norm(sys)
@test l2norm ≈ 1.419727086450068e+01

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d);
@time l2norm = gl2norm(sys')
@test l2norm ≈ 1.419727086450068e+01

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
e = rand(2,2)
sys = dss(e*a,e,e*b,c,d);
@time l2norm = gl2norm(sys')
@test l2norm ≈ 1.419727086450068e+01

# discrete standard & descriptor
a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
sys = dss(a,b,c,d,Ts = 1);
@time l2norm = gh2norm(sys)
@test l2norm ≈ 3.438689619923066e+01

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
e = rand(2,2)
sys = dss(e*a,e,e*b,c,d,Ts = 1);
@time l2norm = gh2norm(sys)
@test l2norm ≈ 3.438689619923066e+01

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
sys = dss(a,b,c,d,Ts = 1);
@time l2norm = gl2norm(sys)
@test l2norm ≈ 3.438689619923066e+01

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
e = rand(2,2)
sys = dss(e*a,e,e*b,c,d,Ts = 1);
@time l2norm = gl2norm(sys)
@test l2norm ≈ 3.438689619923066e+01

a = [-1 2;0 0]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
sys = dss(a,b,c,d);
@time l2norm = gl2norm(sys)
@test l2norm ≈ Inf

a = [-1 2;0 0]; e = [1 0; 0 2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
sys = dss(a,e,b,c,d);
@time l2norm = gl2norm(sys)
@test l2norm ≈ Inf

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
sys = dss(a,b,c,d,Ts = 1);
@time l2norm = gl2norm(sys')
@test l2norm ≈ 3.438689619923066e+01

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
e = rand(2,2)
sys = dss(e*a,e,e*b,c,d,Ts = 1);
@time l2norm = gl2norm(sys')
@test l2norm ≈ 3.438689619923066e+01

z = Polynomial([0, 1],'z')
g = [z^2+z+1 4*z^2+3*z+2 2*z^2-2;
    z 4*z-1 2*z-2;
    z^2 4*z^2-z 2*z^2-2*z];

sys = dss(g,minimal = true, atol = 1.e-7,Ts = 1)
@time l2norm = gl2norm(sys, atol = 1.e-7)
@test l2norm ≈ gl2norm(sys', atol = 1.e-7)

n = 5; m = 3; p = 2;

Ty = Float64; fast = true;
for Ty in (Float64, Complex{Float64})

for fast in (true, false)

sys = rss(n,p,m,T = Ty, stable = true);
sys.D[:,:] = zeros(Ty,p,m);
@test gh2norm(sys) ≈ gl2norm(sys')

sys = rss(n,p,m,T = Ty, stable = true);
@test gh2norm(sys) ≈ gl2norm(sys')

sys = rss(n,p,m,T = Ty, stable = true, disc = true);
@test gh2norm(sys) ≈ gl2norm(sys')

sys = rdss(n,p,m,T = Ty, stable = true);
sys.D[:,:] = zeros(Ty,p,m);
@test gh2norm(sys) ≈ gl2norm(sys')

sys = rdss(n,p,m,T = Ty, stable = true);
@test gh2norm(sys) ≈ gl2norm(sys')

sys = rdss(n,p,m,T = Ty, stable = true, disc = true);
@test gh2norm(sys) ≈ gl2norm(sys')

sys = rdss(n,p,m, T = Ty, stable = true, id=ones(Int,3));
@test gh2norm(sys) ≈ gl2norm(sys')

end # fast
end # Ty
end # gl2norm

@testset "ghanorm" begin

sys = rdss(0,0,0);
@time hanorm, hsv = ghanorm(sys)
@test hanorm == 0 && hsv == Float64[]

n = 5; m = 3; p = 2;

Ty = Float64; fast = true;
for Ty in (Float64, Complex{Float64})

for fast in (true, false)

sys = rss(n,p,m,stable = true);

try
  @time hanorm, hsv = ghanorm(sys,fast = fast)
  @test true
catch
  @test false
end

try
    @time hanorm, hsv = ghanorm(sys',fast = fast)
    @test false
catch
    @test true
end

sys = rss(n,p,m,T = Ty, stable = true);

@time hanorm, hsv = ghanorm(sys-sys,fast = fast)
@test hanorm < 1.e-7

@time hanorm, hsv = ghanorm(sys+sys,fast = fast)
@test norm(hsv[n+1:end]) < 1.e-7

@time hanorm, hsv = ghanorm([sys sys],fast = fast)
@test norm(hsv[n+1:end]) < 1.e-7

sys = rss(n,p,m,T = Ty, disc = true, stable = true);
try
    @time hanorm, hsv = ghanorm(sys,fast = fast)
    @test true
catch
    @test false
end

try
    @time hanorm, hsv = ghanorm(sys',fast = fast)
    @test false
catch
    @test true
end
@time hanorm, hsv = ghanorm(sys-sys,fast = fast)
@test hanorm < 1.e-7

@time hanorm, hsv = ghanorm(sys+sys,fast = fast)
@test norm(hsv[n+1:end]) < 1.e-7

@time hanorm, hsv = ghanorm([sys sys],fast = fast)
@test norm(hsv[n+1:end]) < 1.e-7

sys = rdss(n,p,m,T = Ty, stable = true,id=ones(Int,3));
try
  @time hanorm, hsv = ghanorm(sys,atol=1.e-7)
  @test true
catch
  @test false
end

try
    @time hanorm, hsv = ghanorm(sys',fast = fast)
    @test false
catch
    @test true
end

sys = rdss(n,p,m,T = Ty, stable = true);
@time hanorm, hsv = ghanorm(sys-sys,fast = fast)
@test hanorm < 1.e-7

@time hanorm, hsv = ghanorm(sys+sys,fast = fast)
@test norm(hsv[n+1:end]) < 1.e-7

@time hanorm, hsv = ghanorm([sys sys],fast = fast)
@test norm(hsv[n+1:end]) < 1.e-7

sys = rdss(n,p,m,T = Ty, disc = true,stable = true,id=ones(Int,3));
try
    @time hanorm, hsv = ghanorm(sys,fast = fast)
    @test true
catch
    @test false
end

try
    @time hanorm, hsv = ghanorm(sys',fast = fast)
    @test false
catch
    @test true
end

sys = rdss(n,p,m,T = Ty, disc = true,stable = true);
@time hanorm, hsv = ghanorm(sys-sys,fast = fast)
@test hanorm < 1.e-7

@time hanorm, hsv = ghanorm(sys+sys,fast = fast)
@test norm(hsv[n+1:end]) < 1.e-7

@time hanorm, hsv = ghanorm([sys sys],fast = fast)
@test norm(hsv[n+1:end]) < 1.e-7

end # fast
end # Ty
end # ghanorm

@testset "glinfnorm & ghinfnorm" begin

sys = rdss(0,0,0);
@time linfnorm, fpeak = glinfnorm(sys)
@test linfnorm == 0 && fpeak == 0

sys = rdss(0,2,2);
@time linfnorm, fpeak = glinfnorm(sys)
@test linfnorm == opnorm(sys.D) && fpeak == 0

sys = rdss(0,2,2,disc=true);
@time linfnorm, fpeak = glinfnorm(sys)
@test linfnorm == opnorm(sys.D) && fpeak == 0

a = [0 1;-1 0]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d);
@time hinfnorm, fpeak = ghinfnorm(sys)
@test isinf(hinfnorm) && isnan(fpeak)

a = [0 1;-1 0]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d);
@time linfnorm, fpeak = glinfnorm(sys)
@test isinf(linfnorm) && fpeak ≈ 1

a = [0 1;-1 0]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d, Ts = 2);
@time linfnorm, fpeak = glinfnorm(sys)
@test isinf(linfnorm) && fpeak ≈ pi/4

a = [-1 2;0 -2]; e = [0 1;0 0]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,e,b,c,d);
@time linfnorm, fpeak = glinfnorm(sys)
@test isinf(linfnorm) && isinf(fpeak)

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d);
@time hinfnorm, fpeak = ghinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,im*fpeak)) ≈ hinfnorm &&
      round(hinfnorm, digits=6) ≈ round(1.163398400218353e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.695926597795647,digits=3)

sys = dss(a+im*a,b+im*b,c+im*c,d+im*d);
@time hinfnorm, fpeak = glinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,im*fpeak)) ≈ hinfnorm &&
      abs(hinfnorm - 56.237554184492573) < 1.e-6 &&  # compare with Matlab results
      abs(fpeak+3.952242830041314) < 0.001

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
e = rand(2,2);
sys = dss(e*a,e,e*b,c,d);
@time hinfnorm, fpeak = ghinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,im*fpeak)) ≈ hinfnorm &&
      round(hinfnorm, digits=6) ≈ round(1.163398400218353e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.695926597795647,digits=3)

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
e = rand(Complex{Float64},2,2);
sys = dss(e*(a+im*a),e,e*(b+im*b),c+im*c,d+im*d);
@time hinfnorm, fpeak = glinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,im*fpeak)) ≈ hinfnorm &&
      abs(hinfnorm - 56.237554184492573) < 1.e-6 &&  # compare with Matlab results
      abs(fpeak+3.952242830041314) < 0.001

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d);
@time linfnorm, fpeak = glinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,im*fpeak)) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(1.163398400218353e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.695926597795647,digits=3)

@time linfnorm, fpeak = opnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,im*fpeak)) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(1.163398400218353e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.695926597795647,digits=3)

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
e = rand(2,2);
sys = dss(e*a,e,e*b,c,d);
@time linfnorm, fpeak = glinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,im*fpeak)) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(1.163398400218353e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.695926597795647,digits=3)

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
sys = dss(a,b,c,d);
@time linfnorm, fpeak = glinfnorm(sys',rtolinf = 0.0000001)
@test opnorm(evalfr(sys,im*fpeak)) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(1.163398400218353e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.695926597795647,digits=3)

a = [-1 2;-3 -2]; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = zeros(3,3);
e = rand(2,2)
sys = dss(e*a,e,e*b,c,d);
@time linfnorm, fpeak = glinfnorm(sys',rtolinf = 0.0000001)
@test opnorm(evalfr(sys,im*fpeak)) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(1.163398400218353e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.695926597795647,digits=3)

# discrete standard & descriptor
a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
sys = dss(a,b,c,d,Ts = 1);
@time hinfnorm, fpeak = ghinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,exp(im*fpeak))) ≈ hinfnorm &&
      round(hinfnorm, digits=6) ≈ round(4.133509781281479e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.753904640692288,digits=3)

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
e = rand(2,2)
sys = dss(e*a,e,e*b,c,d,Ts = 1);
@time hinfnorm, fpeak = ghinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,exp(im*fpeak))) ≈ hinfnorm &&
      round(hinfnorm, digits=6) ≈ round(4.133509781281479e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.753904640692288,digits=3)

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
sys = dss(a,b,c,d,Ts = 1);
@time linfnorm, fpeak = glinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,exp(im*fpeak))) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(4.133509781281479e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.753904640692288,digits=3)

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
e = rand(2,2)
sys = dss(e*a,e,e*b,c,d,Ts = 1);
@time linfnorm, fpeak = glinfnorm(sys,rtolinf = 0.0000001)
@test opnorm(evalfr(sys,exp(im*fpeak))) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(4.133509781281479e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.753904640692288,digits=3)

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
sys = dss(a,b,c,d,Ts = 1);
@time linfnorm, fpeak = glinfnorm(sys',rtolinf = 0.0000001)
@test opnorm(evalfr(sys,exp(im*fpeak))) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(4.133509781281479e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.753904640692288,digits=3)

a = [-1 2;-3 -2]/10; b = [2 3 4; 1 2 3]; c = [1 4; 2 2; 1 3]; d = ones(3,3);
e = rand(2,2)
sys = dss(e*a,e,e*b,c,d,Ts = 1);
@time linfnorm, fpeak = glinfnorm(sys',rtolinf = 0.0000001)
@test opnorm(evalfr(sys,exp(im*fpeak))) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(4.133509781281479e+01,digits=6) &&
      round(fpeak,digits = 3) ≈ round(2.753904640692288,digits=3)

z = Polynomial([0, 1],'z')
g = [z^2+z+1 4*z^2+3*z+2 2*z^2-2;
    z 4*z-1 2*z-2;
    z^2 4*z^2-z 2*z^2-2*z];

sys = dss(g,minimal = true, atol = 1.e-7,Ts = 1)
@time linfnorm, fpeak = glinfnorm(sys, atol = 1.e-7)
@test glinfnorm(sys, atol = 1.e-7)[1] ≈ glinfnorm(sys', atol = 1.e-7)[1] &&
      opnorm(evalfr(sys,exp(im*fpeak))) ≈ linfnorm &&
      round(linfnorm, digits=6) ≈ round(1.048808848170152e+01,digits=6) &&
      fpeak == 0

n = 50; m = 30; p = 20;

# continuous-time standard, real
Ty = Float64; fast = true;
sys = rss(n,p,m,T = Ty,disc=false);
@time linfnorm, fpeak = glinfnorm(sys, fast = fast, rtolinf = 0.0000000001)
syst = (linfnorm)^2*I-sys'*sys; ev=gpole(inv(syst),atol=1.e-7);
@test sort(abs.(real(ev)))[1] < 1.e-5

# discrete-time standard, real
Ty = Float64; fast = true;
sys = rss(n,p,m,T = Ty,disc=true);
@time linfnorm, fpeak = glinfnorm(sys,fast = fast, rtolinf = 0.0000000001);
syst = (linfnorm)^2*I-sys'*sys; ev=gpole(inv(syst),atol=1.e-7);
@test sort(abs.(abs.(ev).-1))[1] < 1.e-5

# continuous-time descriptor, real
Ty = Float64; fast = true;
sys = rdss(n,p,m,T = Ty,disc=false);
@time linfnorm, fpeak = glinfnorm(sys,fast = fast, rtolinf = 0.0000000001);
syst = (linfnorm)^2*I-sys'*sys; ev=gpole(inv(syst),atol=1.e-7);
@test sort(abs.(real(ev)))[1] < 1.e-5

# discrete-time descriptor, real
Ty = Float64; fast = true;
sys = rdss(n,p,m,T = Ty,disc=true);
@time linfnorm, fpeak = glinfnorm(sys,fast = fast, rtolinf = 0.0000000001);
syst = (linfnorm)^2*I-sys'*sys; ev=gpole(inv(syst),atol=1.e-7);
@test sort(abs.(abs.(ev).-1))[1] < 1.e-5

# continuous-time standard, complex
Ty = Complex{Float64}; fast = true;
sys = rss(n,p,m,T = Ty,disc=false);
@time linfnorm, fpeak = glinfnorm(sys,fast = fast, rtolinf = 0.0000000001);
syst = (linfnorm)^2*I-sys'*sys; ev=gpole(inv(syst),atol=1.e-7);
@test sort(abs.(real(ev)))[1] < 1.e-5 && opnorm(evalfr(sys,im*fpeak)) ≈ linfnorm

# discrete-time standard, complex
Ty = Complex{Float64}; fast = true;
sys = rss(n,p,m,T = Ty,disc=true);
@time linfnorm, fpeak = glinfnorm(sys,fast = fast, rtolinf = 0.0000000001);
syst = (linfnorm)^2*I-sys'*sys; ev=gpole(inv(syst),atol=1.e-7);
@test sort(abs.(abs.(ev).-1))[1] < 1.e-5 && opnorm(evalfr(sys,exp(im*fpeak))) ≈ linfnorm

# continuous-time descriptor, complex
Ty = Complex{Float64}; fast = true;
sys = rdss(n,p,m,T = Ty,disc=false);
@time linfnorm, fpeak = glinfnorm(sys,fast = fast, rtolinf = 0.0000000001);
syst = (linfnorm)^2*I-sys'*sys; ev=gpole(inv(syst),atol=1.e-7);
@test sort(abs.(real(ev)))[1] < 1.e-5 && opnorm(evalfr(sys,im*fpeak)) ≈ linfnorm

# discrete-time descriptor, complex
Ty = Complex{Float64}; fast = true;
sys = rdss(n,p,m,T = Ty,disc=true);
@time linfnorm, fpeak = glinfnorm(sys,fast = fast, rtolinf = 0.0000000001);
syst = (linfnorm)^2*I-sys'*sys; ev=gpole(inv(syst),atol=1.e-7);
@test sort(abs.(abs.(ev).-1))[1] < 1.e-5 && opnorm(evalfr(sys,exp(im*fpeak))) ≈ linfnorm

n = 5; m = 3; p = 2;

Ty = Float64; fast = true;
Ty = Complex{Float64}; fast = true;
rtolinf = 0.000001
for Ty in (Float64, Complex{Float64})

for fast in (true, false)

sys = rss(n,p,m,T = Ty, stable = true);
sys.D[:,:] = zeros(Ty,p,m);
@time hinf, hfpeak = ghinfnorm(sys,fast = fast,rtolinf = rtolinf)
@time linf, lfpeak = glinfnorm(sys',fast = fast,rtolinf = rtolinf)
@test abs(hinf-linf)/hinf < 2*rtolinf && (hfpeak ≈ lfpeak || abs(hfpeak-lfpeak) <= 0.01*abs(lfpeak))

sys = rss(n,p,m,T = Ty, stable = true);
@time hinf, hfpeak = ghinfnorm(sys,fast = fast,rtolinf = rtolinf)
@time linf, lfpeak = glinfnorm(sys',fast = fast,rtolinf = rtolinf)
@test abs(hinf-linf)/hinf < 2*rtolinf && (hfpeak ≈ lfpeak || abs(hfpeak-lfpeak) <= 0.01*abs(lfpeak))

sys = rss(n,p,m,stable = true, disc = true);
@time hinf, hfpeak = ghinfnorm(sys,fast = fast,rtolinf = rtolinf)
@time linf, lfpeak = glinfnorm(sys',fast = fast,rtolinf = rtolinf)
@test abs(hinf-linf)/hinf < 2*rtolinf && (hfpeak ≈ lfpeak || abs(hfpeak-lfpeak) <= 0.01*abs(lfpeak))

sys = rdss(n,p,m,T = Ty, stable = true);
sys.D[:,:] = zeros(Ty,p,m);
@time hinf, hfpeak = ghinfnorm(sys,fast = fast,rtolinf = rtolinf)
@time linf, lfpeak = glinfnorm(sys',fast = fast,rtolinf = rtolinf)
@test abs(hinf-linf)/hinf < 2*rtolinf && (hfpeak ≈ lfpeak || abs(hfpeak-lfpeak) <= 0.01*abs(lfpeak))

sys = rdss(n,p,m,T = Ty, stable = true);
@time hinf, hfpeak = ghinfnorm(sys,fast = fast,rtolinf = rtolinf)
@time linf, lfpeak = glinfnorm(sys',fast = fast,rtolinf = rtolinf)
@test abs(hinf-linf)/hinf < 2*rtolinf && (hfpeak ≈ lfpeak || abs(hfpeak-lfpeak) <= 0.01*abs(lfpeak))

sys = rdss(n,p,m,T = Ty, stable = true, disc = true);
@time hinf, hfpeak = ghinfnorm(sys,fast = fast,rtolinf = rtolinf)
@time linf, lfpeak = glinfnorm(sys',fast = fast,rtolinf = rtolinf)
@test abs(hinf-linf)/hinf < 2*rtolinf && (hfpeak ≈ lfpeak || abs(hfpeak-lfpeak) <= 0.01*abs(lfpeak))

sys = rdss(n,p,m,T = Ty, stable = true, id=ones(Int,3));
@time hinf, hfpeak = ghinfnorm(sys,fast = fast,rtolinf = rtolinf)
@time linf, lfpeak = glinfnorm(sys',fast = fast,rtolinf = rtolinf)
@test abs(hinf-linf)/hinf < 2*rtolinf && (hfpeak ≈ lfpeak || abs(hfpeak-lfpeak) <= 0.01*abs(lfpeak))

end # fast
end # Ty
end # glinfnorm

@testset "gnugap" begin

## Examples from the Vinnicombe's book, p. 115
s = rtf('s'); N = 3;
sys = (dss(100/(2*s+1)), dss(100/(2*s-1)), dss(100/(s+1)^2))
distgap = zeros(N);
for j = 1:N
    distgap[j], = gnugap(sys[1],sys[j],atol=1.e-7);
end
@test norm(distgap-[0,0.02,0.8988]) <0.001

## Examples from the Vinnicombe's book, p. 125
s = rtf('s');
sys1 = dss(1/(s+0.2));                               # P1
sys2 = dss((s+0.1)^2/(s-0.2)/(s^2+0.1*s+0.01));      # P2
nugap, fpeak = gnugap(sys2,sys1,atol=1.e-7)
@test abs(nugap-0.403) < 0.0001 && abs(fpeak-0.32) < 0.01

## Examples from the Vinnicombe's book, p. 160 with wno ~= 0
s = rtf('s'); N = 3;
sys = (dss(0.3*(s-1)/(s^2+1)), dss(0.3*(s-1)/(s^2+2)), dss(0.3*(s-1)*(s-2)/(s^2+2)/(s-sqrt(2))))
distgap = zeros(N);
for j = 1:N
    distgap[j], = gnugap(sys[1],sys[j],atol=1.e-7);
end

@test norm(distgap-[0,1,1]) < 1.e-6

## Examples from the Vinnicombe's book, p. 201
s = rtf('s');
sys1 = dss(1/s^2);    # P1
sys2 = dss(1/s);      # P2
nugap, fpeak = gnugap(sys2,sys1,atol=1.e-7)
@test nugap ≈ 1/sqrt(2) && fpeak ≈ 1

## Examples from the Vinnicombe's book, p. 202  nugap = 1/sqt(2) for epsi>0
s = rtf('s'); epsi = 0.01;
sys1 = dss([1/s; 1/(s+epsi)]);    # P1
sys2 = dss([1/s; 1/s]);           # P2
nugap, = gnugap(sys2,sys1,atol=1.e-7)
@test nugap ≈ 1/sqrt(2)

## Examples from the Vinnicombe's book, p. 202  modified
#  realization of sys2 is not stabilizable
s = rtf('s'); epsi = 0.01;
sys1 = dss([1/s^2; 1/(s+epsi)]);    # P1
sys2 = dss([1/s^2; 1/s]);           # P2
nugap, = gnugap(sys2,sys1,atol=1.e-7)
@test nugap < epsi

## Examples from the Vinnicombe's book, p. 202  modified
#  realization of sys2 is not stabilizable
s = rtf('s'); epsi = 0.01;
sys1 = dss([s^2+epsi; (s+epsi)]);    # P1
sys2 = dss([s^2; s]);           # P2
nugap, = gnugap(sys2,sys1,atol=1.e-7)
@test nugap < sqrt(2)*epsi

##
z = rtf('z');
sys1 = dss(1/z); sys2 = dss(0.5/z);
nugap, = gnugap(sys1,sys2,atol=1.e-6)
@test nugap ≈ 0.316227766016838

##
z = rtf('z');
sys1 = dss(1/z); sys2 = dss(0.5/z^2);
nugap, = gnugap(sys1,sys2,atol=1.e-6)
@test nugap ≈ 0.948683298050514

## ν-gap = 1
s = rtf('s');
sys1 = dss((s+1)/(s+2)); sys2 = dss(-1);
nugap, = gnugap(sys1,sys2,atol=1.e-7)
@test nugap == 1

## gapmetric produces the wrong result nugap = 0
sys1 = dss(1); sys2 = dss(-1);
nugap, = gnugap(sys1,sys2,atol=1.e-5)
@test nugap == 1

## ν-gap = 1
sys1 = rss(3,2,2); sys1.D[:,:] = eye(2); sys2 = -inv(sys1)';
nugap, fpeak = gnugap(sys1,sys2,atol=1.e-6)
@test nugap == 1

n = 5; m = 3; p = 2;

Ty = Float64; fast = true;
Ty = Complex{Float64}; fast = true;
rtolinf = 0.000001
for Ty in (Float64, Complex{Float64})

for fast in (true, false)

## nugap = 0
sys = rss(n,p,m, T = Ty);
@time nugap, fpeak = gnugap(sys,sys,fast = fast, atol = 1.e-7)
@test nugap < 1.e-10

sys = rss(n,p,m,T = Ty, disc=true);
@time nugap, fpeak = gnugap(sys,sys, fast = fast, atol = 1.e-7)
@test nugap < 1.e-10

sys = rdss(n,p,m,T = Ty);
@time nugap, fpeak = gnugap(sys,sys,fast = fast, atol = 1.e-7)
@test nugap < 1.e-10

sys = rdss(n,p,m,T = Ty, disc=true);
@time nugap, fpeak = gnugap(sys,sys, fast = fast, atol = 1.e-7)
@test nugap < 1.e-10

sys = rss(n,p,m, T = Ty);
@time nugap, fpeak = gnugap(sys,sys,fast = fast, atol = 1.e-7, freq = collect(range(0.1, 0.3, length=5)))
@test maximum(nugap) < 1.e-10

sys = rss(n,p,m,T = Ty, disc=true);
@time nugap, fpeak = gnugap(sys, sys, fast = fast, atol = 1.e-7, freq = collect(range(0.1, 0.3, length=5)))
@test maximum(nugap) < 1.e-10

sys = rdss(n,p,m,T = Ty);
@time nugap, fpeak = gnugap(sys, sys,fast = fast, atol = 1.e-7, freq = collect(range(0.1, 0.3, length=5)))
@test maximum(nugap) < 1.e-10

sys = rdss(n,p,m,T = Ty, disc=true);
@time nugap, fpeak = gnugap(sys, sys, fast = fast, atol = 1.e-7, freq = collect(range(0.1, 0.3, length=5)))
@test maximum(nugap) < 1.e-10

sys1 = rss(n,p,m,T = Ty); sys1d = sys1*0.001/glinfnorm(sys1)[1]; sys2 = sys1+sys1d;
@time nugap, fpeak = gnugap(sys2,sys1,fast = fast, atol = 1.e-7)
@time nugap1, fpeak1 = gnugap(sys2,sys1,fast = fast, atol = 1.e-7,freq=fpeak)
@test nugap ≈ nugap1[1] &&  nugap < 0.01

sys1 = rss(n,p,m,T = Ty); sys1d = rss(3,2,3,stable=true,T = Ty); sys2 = sys1+sys1d*0.001/glinfnorm(sys1d)[1];
@time nugap, fpeak = gnugap(sys2,sys1,fast = fast, atol = 1.e-7)
@time nugap1, fpeak1 = gnugap(sys2,sys1,fast = fast, atol = 1.e-7,freq=fpeak)
@test nugap ≈ nugap1[1] &&  nugap < 0.01

@time nugap, fpeek = gnugap(sys1,sys2,fast = fast, atol=1.e-7)
sysd1 = gbilin(sys1,rtfbilin("c2d")[1])[1]; sysd2 = gbilin(sys2,rtfbilin("c2d")[1])[1];
@time nugap1, fpeak1 = gnugap(sysd2,sysd1,fast = fast, atol=1.e-7)
@test abs(nugap-nugap1) < 0.001

sysc1 = gbilin(sysd1,rtfbilin("d2c")[1])[1]; sysc2 = gbilin(sysd2,rtfbilin("d2c")[1])[1];
nugap2, fpeak2 = gnugap(sysc2,sysc1,fast = fast, atol=1.e-7)
@test abs(nugap-nugap2) < 0.0001

end # fast
end # Ty
end # gnugap

end # module


@testset "Misc" begin
  include("Misc/psibound.jl")
  include("Misc/FiniteField.jl")
  include("Misc/Primes.jl")
  include("Misc/Places.jl")
  include("Misc/meataxe.jl")
  include("Misc/stable_subgroups.jl")
  include("Misc/NumberField.jl")
  include("Misc/Poly.jl")
  include("Misc/Localization.jl")
  include("Misc/OrdLocalization.jl")
  include("Misc/jordan_test.jl")
  include("Misc/RelFinField.jl")
end


include("../util.jl")

n = 16
Lx = 10; Ly = Lx;
Nx = n; Ny = n;
x = LinRange(-Lx/2,Lx/2, Nx+1)[1:end-1]; y = LinRange(-Ly/2,Ly/2, Ny+1)[1:end-1];
# x = LinRange(-Lx/2,Lx/2, Nx); y = LinRange(-Ly/2,Ly/2, Ny);
psi0 = one.(x*y') |> complex
psi = Torus(psi0,x,y)

v = PointVortex(uniform(x[2], x[end-1]), uniform(y[2],y[end-1]), 1)
vortex!(psi, v)

vf = findvortices(psi)
naivePlaquette(psi.ψ, [x, y], 0)
remove_vortices_edge(findvortices_jumps(psi), psi)

dx = x[2]-x[1]
euclidVorts(v, vf[1])

maxError = sqrt(2)*(dx/((30/4)))

function runErrorSim(Ntrials)
    diffs = []
    count = 0
    while (count < Ntrials)
        n = 16
        Lx = 10; Ly = Lx;
        Nx = n; Ny = n;
        x = LinRange(-Lx/2,Lx/2, Nx+1)[1:end-1]; y = LinRange(-Ly/2,Ly/2, Ny+1)[1:end-1];
        # x = LinRange(-Lx/2,Lx/2, Nx); y = LinRange(-Ly/2,Ly/2, Ny);
        psi0 = one.(x*y') |> complex
        psi = Torus(psi0,x,y)

        v = PointVortex(uniform(x[2], x[end-1]), uniform(y[2],y[end-1]), 1)
        vortex!(psi, v)
        vf = naivePlaquette(psi.ψ, [x, y], 0)
        push!(diffs, euclidVorts(v, vf[1]))
        count+=1
    end
    return diffs
end

errorBound = sqrt(1/2)*dx
avgError = dx*(1/sqrt(2*pi))

N = 1000000

diffs = runErrorSim(N)
maxError = findmax(diffs)[1]

avgResult = mean(diffs)

maxError/errorBound
avgResult/avgError


#!/usr/bin/env julia

p(n) = (reduce(+,map((x->(4/(1+(x*x)))),(0.5/n):(1/n):1))/n)

function picalc(numsteps)

  # Get number of processes.
  # np = nworkers()
  np = 1

  println("Calculating PI using:")
  println("  ", numsteps, " slices")
  println("  ", np, " worker(s)")
  start = time()

  mypi = p(numsteps)

  stop = time()

  elapsed = (stop - start)

  println("Obtained value of PI: ", mypi)
  println("Time taken: ", elapsed, " seconds")

end

numsteps=50000000

if length(ARGS) > 0
  numsteps = parse(Int, ARGS[1])
end

picalc(numsteps)


####################
## CIR model type ##
####################

type CIR
    alpha::Float64
    mu::Float64
    sigma::Float64
    scale::DatePeriod # which period is Δt = 1
end

## convenient constructors
##------------------------

function CIR(x::Array)
    return CIR(x[1], x[2], x[3], Dates.Year(1))
end

function CIR(x1, x2, x3)
    return CIR(x1, x2, x3, Dates.Year(1))
end

## customized display
##-------------------

import Base.display
function display(cirMod::CIR)
    println("α:        $(cirMod.alpha)")
    println("μ:        $(cirMod.mu)")
    println("σ:        $(cirMod.sigma)")
    println("scaling:  Δt = $(cirMod.scale)")
end

## information retrieval
##----------------------

function getParams(cirMod::CIR)
    return (cirMod.alpha, cirMod.mu, cirMod.sigma)
end

#################
## likelihoods ##
#################

function cirNllh(da::DataArray, params::Array{Float64, 1},
                 Δt::Float64 = 1)
    # Δt means time step of data relativ to time step represented by
    # parameters

    # calculate negative log-likelihood for given parameter values
    α, μ, σ = params

    # get constant values
    c = 2*α/(σ^2*(1-exp(-α*Δt)))
    q = 2*α*μ/σ^2 - 1

    # get changing values u and v
    uWithNA = c*da[1:(end-1)]*exp(-α*Δt)
    vWithNA = c*da[2:end]

    # eliminate observations with missing value for either u or v
    notElim = (!isna(uWithNA)) & (!isna(vWithNA))
    u = uWithNA[notElim]
    v = vWithNA[notElim]

    nObs = size(u, 1)

    return -(nObs)*log(c) +
    sum(u + v - 0.5*q*log(v./u) - log(besseli(q, 2*sqrt(u.*v))))
end

function cirNllh(da::DataArray, cirMod::CIR, Δt::Float64 = 1)
    return cirNllh(da, [getParams(cirMod)...], Δt)
end

function cirNllhx(da::DataArray, params::Array{Float64, 1},
                  Δt::Float64 = 1)
    # calculate negative log-likelihood for given parameter values
    # using scaled modified bessel function of first order
    α, μ, σ = params

    # get constant values
    c = 2*α/(σ^2*(1-exp(-α*Δt)))
    q = 2*α*μ/σ^2 - 1

    # get changing values u and v
    uWithNA = c*da[1:(end-1)]*exp(-α*Δt)
    vWithNA = c*da[2:end]

    # eliminate observations with missing value for either u or v
    notElim = (!isna(uWithNA)) & (!isna(vWithNA))
    u = uWithNA[notElim]
    v = vWithNA[notElim]

    nObs = size(u, 1)

    return -(nObs)*log(c) +
    sum(u + v - 0.5*q*log(v./u) -
        log(besselix(q, 2*sqrt(u.*v))) - 2*sqrt(u.*v))
end

function cirNllhx(da::DataArray, cirMod::CIR, Δt::Float64 = 1)
    return cirNllhx(da, [getParams(cirMod)...], Δt)
end

####################
## OLS estimation ##
####################

function cirOls(x::DataArray, Δt::Float64=1)
    y = (x[2:end] .- x[1:(end-1)])./sqrt(x[1:(end-1)])
    x1 = 1 ./ sqrt(x[1:(end-1)])
    x2 = sqrt(x[1:(end-1)])

    # conduct regression
    df = DataFrame(y = y, x1 = x1, x2 = x2)
    olsFit = lm(y ~ 0 + x1 + x2, df)

    a1, a2 = coef(olsFit)

    # get associated parameters
    α = -a2 / Δt
    μ = a1 / (Δt * α)
    σ = sqrt(var(residuals(olsFit)) /Δt)
    return (α, μ, σ)
end

##############################
## conditional distribution ##
##############################

function getCondDistr(cirMod::CIR, r0::Float64,  Δt::Float64=1.)
    # conditional cdf given r
    α, μ, σ = cirMod.alpha, cirMod.mu, cirMod.sigma

    df = 4*α*μ/σ^2
    c = 2*α/(σ^2*(1-exp(-α*Δt)))
    λ = 2*c*r0*exp(-α*Δt)
    return (df, λ, c)
end

import Distributions.cdf
function cdf(cirMod::CIR, x::Float64, r0::Float64, Δt::Float64=1.)
    df, λ, c = getCondDistr(cirMod, r0, Δt)

    return pnchisq(x*2*c, df, λ)
end

function cdf(cirMod::CIR, x::Array{Float64, 1}, Δt::Float64=1.)
    ## take first value as given input
    nObs = length(x)-1
    condDistrParams = [getCondDistr(cirMod, x[ii], Δt) for ii=1:nObs]

    u = zeros(nObs)
    for ii=1:nObs
        df, λ, c = condDistrParams[ii]
        u[ii] = pnchisq(x[ii+1]*2*c, df, λ)
    end

    return u
end

function quantile(cirMod::CIR, q::Float64, r0::Float64, Δt::Float64=1.)
    df, λ, c = getCondDistr(cirMod, r0, Δt)

    return qnchisq(q, df, λ)/(2*c)
end

####################
## simulating CIR ##
####################

function simulate(cirMod::CIR, r0::Float64, nObs::Int, Δt::Float64=1.)
    α, μ, σ = cirMod.alpha, cirMod.mu, cirMod.sigma

    df = 4*α*μ/σ^2 # constant
    c = 2*α/(σ^2*(1-exp(-α*Δt))) # constant for given Δt

    vals = [r0; Array(Float64, nObs)]
    for ii=1:nObs
        # get conditional non-centrality
        λ = 2*c*vals[ii]*exp(-α*Δt)
        vals[ii+1] = rnchisq(df, λ)/(2*c)
    end
    return vals
end


module MCMC

using LinearAlgebra
using Distributions

export update_mean, update_var, metropolis, adaptive_metropolis

eye(n) = diagm(0 => ones(Float64, n))

## posterior interval
pint(theta::Array{Float64,2}) = [quantile(theta[:,j], [0.025, 0.975]) for j in 1:size(theta,2)]

update_mean(t::Int, Xt::Float64, m0::Float64) = (Xt + m0*(t-1)) / t
update_mean(t::Int, Xt::Array{Float64,1}, m0::Array{Float64,1}) = @. (Xt + m0*(t-1)) / t

function update_var(t::Int, Xt::Float64, mt::Float64, m0::Float64, V0::Float64)
    return V0*(t-2)/(t-1) + t*(mt-m0)^2
end

function update_var(t::Int, Xt::Array{Float64,1}, mt::Array{Float64,1},
                   m0::Array{Float64,1}, V0::Array{Float64,2}, eps::Matrix{Float64})
    return V0.*(t-2)/(t-1) + 1/(t-1).*((t-1).*(m0 * transpose(m0)) .- t.* (mt * transpose(mt)) .+ (Xt * transpose(Xt)) + eps)
end

## single Metropolis-Hastings step
function metropolis(target_log::Function, init, proposal::Function, proposal_lpdf::Function)
    prop = proposal(init)
    q1 = proposal_lpdf(prop, init)
    q2 = proposal_lpdf(init, prop)

    r = q1 + target_log(prop) - (q2 + target_log(init))

    if log(rand(1)[1]) < r
        return prop, 1
    else
        return init, 0
    end
end

function metropolis(target_log::Function, init::T, prop::T) where T <: Real
    r = target_log(prop) - target_log(init)

    if log(rand()) < r
        return prop, 1
    else
        return init, 0
    end
end

metropolis(target_log::Function, init, proposal::Function) = metropolis(target_log, init, proposal(init))

function metropolis(target_log::Function, init::T, proposal::Function, init_loglik::T) where T <: Real
    prop = proposal(init)
    tlp = target_log(prop)
    tli = target_log(init, init_loglik)

    if log(rand()) < (tlp[1] - tli[1])
        return prop, 1, tlp[2]
    else
        return init, 0, tlp[2]
    end
end

## single adaptive Metropolis step
function adaptive_metropolis(target_log::Function, init::Vector{Float64}, V::Matrix{Float64},
    t::Int, t0::Int=2*length(init), s::Float64=(2.38)^2/length(init), beta::Float64=0.05)

    d = length(init)
    proposal = zeros(Float64, d)

    if (t <= t0) | (rand() < beta)
        proposal = rand(MvNormal(init, (0.1)^2*eye(d)/d))
    else
        proposal = rand(MvNormal(init, s.*V))
    end

    r = target_log(proposal) - target_log(init)

    if log(rand()) < r
        return proposal, 1
    else
        return init, 0
    end
end

function adaptive_metropolis(target_log::Function, init::Float64, V::Float64, t::Int, t0::Int=2, s::Float64=2.38, beta::Float64=0.05)

    proposal::Float64 = 0

    if (t <= t0) | (rand() < beta)
        proposal = rand(Normal(init, 0.1))
    else
        if(s*V == 0)
            proposal = init
        else
            proposal = rand(Normal(init, s*sqrt(V)))
        end
    end

    r = target_log(proposal) - target_log(init)

    if log(rand()) < r
        return proposal, 1
    else
        return init, 0
    end
end

end


begin
    using Revise
    using BNPRegressionGGA2021
    using DataFrames
    using Distributions
    using Random
    using RCall
    using Statistics
    using StatsBase
    using LinearAlgebra
    const BNP = BNPRegressionGGA2021
    import Distributions: pdf
    import Base: rand
end;

function preprocess(dy, x0, x1)
    # Simulate the responses
    y0 = @. rand(dy(x0))
    y1 = LinRange(-3, 3, 50) |> collect

    # Expand the grid
    grid = rcopy(R"""expand.grid(y1 = $y1, x1 = $x1)""")
    y1 = grid[!, :y1]
    x1 = grid[!, :x1]

    # Expand x0-x1 using splines
    x0min, x0max = extrema(x0)
    R"""
    X0 <- splines::bs($x0, df = 6, Boundary.knots = c($x0min, $x0max))
    X1 <- predict(X0, $x1)
    """
    @rget X0 X1

    # Standardise responses and predictors
    mX0, sX0 = mean_and_std(X0, 1)
    for col in 1:size(X0, 2)
        if sX0[col] > 0
            X0[:, col] = (X0[:, col] .- mX0[col]) ./ sX0[col]
            X1[:, col] = (X1[:, col] .- mX0[col]) ./ sX0[col]
        end
    end
    my0, sy0 = mean_and_std(y0, 1)
    y0 = (y0 .- my0) ./ sy0

    # Add a constant term to the design matrices
    X0 = [ones(size(X0, 1)) X0]
    X1 = [ones(size(X1, 1)) X1]

    # Generate the true (standardized) density over the grid
    f1 = pdf.(dy.(x1), my0 .+ y1 .* sy0) .* sy0

    # Return the preprocessed results
    return y0, y1, x1, X0, X1, f1
end;

function fitmodel(y0, y1, x1, X0, X1; mcmcsize = 20000, censoring_rate = 0)
    # Generate some incidental censoring
    event0 = rand(length(y0)) .> censoring_rate

    # Fit the model
    smpl = BNP.DGSBPNormalDependent(; y0, X0, y1, X1, event0);
    fhchain, _ = BNP.sample!(smpl; mcmcsize);
    fh = mean(fhchain)

    # Organize the results as a DataFrame
    df = DataFrame(; y1, x1, f1, fh)
    return df, smpl
end;

function plot(df; figname)
    R"""
    for (var in c("fh", "f1")) {
        xv <- sort(unique($df[["x1"]]))
        yv <- sort(unique($df[["y1"]]))
        zv <- $df |>
            dplyr::select(x1, y1, !!var) |>
            tidyr::pivot_wider(values_from = !!var, names_from = y1) |>
            dplyr::select(-x1) |>
            as.matrix()
        png(paste0($figname, "-", var, ".png"))
        fig <- persp(
            x = xv, y = yv, z = zv,
            phi = 90, theta = 45, xlab = "x", ylab = "y", zlab = var
        )
        dev.off()
    }
    """
end;

# Example 1: Continuous predictor (linear regression, normal distribution)
begin
    Random.seed!(3)
    N0, N1, Nrep = 300, 50, 20
    x1raw = LinRange(-1, 1, N1)
    x0raw = repeat(LinRange(-2, 2, N0 ÷ Nrep), Nrep)
    dy(x) = Normal(x, 1)
    y0, y1, x1, X0, X1, f1 = preprocess(dy, x0raw, x1raw)
    df, smpl = fitmodel(y0, y1, x1, X0, X1; mcmcsize = 20000, censoring_rate = .2)
    plot(df; figname = "figures/density-31-03-2022-ex-1")
end;

# Example 2: Continuous predictor (linear regression, mixture distribution)
begin
    Random.seed!(3)
    N0, N1, Nrep = 300, 50, 20
    x1raw = LinRange(-1, 1, N1)
    x0raw = repeat(LinRange(-2, 2, N0 ÷ Nrep), Nrep)
    dy(x) = SkewNormal(2x, 4, 4)
    y0, y1, x1, X0, X1, f1 = preprocess(dy, x0raw, x1raw)
    df, smpl = fitmodel(y0, y1, x1, X0, X1; mcmcsize = 10, censoring_rate = .2)
    plot(df; figname = "figures/density-31-03-2022-ex-2")
end;

# Example 3: Continuous predictor (cubic regression, normal distribution)
begin
    Random.seed!(3)
    N0, N1, Nrep = 300, 50, 20
    x1raw = LinRange(-1.5, 1.5, N1)
    x0raw = repeat(LinRange(-2, 2, N0 ÷ Nrep), Nrep)
    dy(x) = Normal(0.2x^3, 0.5)
    y0, y1, x1, X0, X1, f1 = preprocess(dy, x0raw, x1raw)
    df, smpl = fitmodel(y0, y1, x1, X0, X1; mcmcsize = 20000, censoring_rate = .2)
    plot(df; figname = "figures/density-31-03-2022-ex-3")
end;

# Example 4: Continuous predictor (cubic regression, mixture distribution)
begin
    Random.seed!(3)
    N0, N1, Nrep = 300, 50, 20
    x1raw = LinRange(-2, 2, N1)
    x0raw = repeat(LinRange(-2.5, 2.5, N0 ÷ Nrep), Nrep)
    dy(x) = SkewNormal(0.4x^3, 4, 4)
    y0, y1, x1, X0, X1, f1 = preprocess(dy, x0raw, x1raw)
    df, smpl = fitmodel(y0, y1, x1, X0, X1; mcmcsize = 20000, censoring_rate = .2)
    plot(df; figname = "figures/density-31-03-2022-ex-4")
end;


fermi(x) = x <= 0 ? (1 / (1 + exp(x))) : 1 - fermi(-x)
dfermi(x) = - fermi(x) * fermi(-x)

fermi(ϵ, β) = fermi(ϵ*β)
dfermi(ϵ, β) = β * dfermi(ϵ*β)


#=
Created on 20/01/2021 10:58:10
Last update: -

@author: Michiel Stock
michielfmstock@gmail.com

Here, we illustrate type-based dispatch, using the type system
=#

abstract type ODESolver end

struct Euler <: ODESolver end
struct SecondOrder <: ODESolver end

function solve_ode(f′, (t₀, tₑ), y₀, Δt, method::ODESolver)
    tsteps = t₀:Δt:tₑ
    # initialize solution vector
    y = zeros(length(tsteps))
    # set initial value
    y[1] = y₀
    for (i, t) in enumerate(tsteps)
        i == 1 && continue
        yᵢ₋₁ = y[i-1]
        yᵢ = step(f′, t, yᵢ₋₁, Δt, method)
        y[i] = yᵢ
    end
    return y
end

step(f′, t, yᵢ₋₁, Δt, ::Euler) = yᵢ₋₁ + Δt * f′(t, yᵢ₋₁)

function step(f′, t, yᵢ₋₁, Δt, ::SecondOrder)
    y = yᵢ₋₁ + 0.5Δt * f′(t, yᵢ₋₁)  # half step
    return yᵢ₋₁ + Δt * f′(t, y)
end

f′(t, y) = -y + sin(t * y)

y_euler = solve_ode(f′, (0, 10), 1.0, 0.1, Euler())
y_second_order = solve_ode(f′, (0, 10), 1.0, 0.1, SecondOrder())

plot(y_euler, label="Euler", xlabel="t", ylabel="y")
plot!(y_second_order, label="second order")


using Plots
using LaTeXStrings

# backend(:pgfplotsx)
backend(:pyplot)
# backend(:gr)

#reset past font scaling
Plots.scalefontsizes()
#scale fonts
Plots.scalefontsizes(5.00)
#Extracting stored varibles

#Add colors
lightblue = colorant"rgb(63%,74%,78%)"
orange = colorant"rgb(85%,55%,13%)"
silver = colorant"rgb(69%,67%,66%)"
rust = colorant"rgb(72%,26%,6%)"

x0 = 1.0
y0 = 1.0

KL(x,y) = x * log(x / x0) + y * log(y / y0) - x - y + x0 + y0
squared_distance(x,y) = 0.5 * ((x - x0)^2 + (y - y0)^2)

ep = 0.0001

x1 = (0.0 + ep) : 0.05 : (2.0 + ep)
y1 = (0.0 + ep) : 0.05 : (2.0 + ep)

x2 = (0.0 + ep) : 0.10 : (2.0 + ep)
y2 = (0.0 + ep) : 0.10 : (2.0 + ep)

srf = wireframe(x1, y1, squared_distance, camera=(60,30), linecolor=silver, linealpha=0.2, xlabel=L"x", ylabel=L"y", zticks=0.0:0.5:2.0, label=L"\mathrm{true} \ f)", thickness_scaling=1.5, linewidth=0.75)

wireframe!(srf, x2, y2,  KL, linecolor=rust, label=L"\mathbb{D}\left(x\middle|y\right)", legend=:topright)

savefig(srf, "./out/figures/divergences.pdf")
run(`pdfcrop --margins '5 5 5 5' ./out/figures/divergences.pdf ./out/figures/divergences.pdf`)


# Copyright 2020 Google LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

""" Returns a small test LP.
The LP:
  min 5 x_1 + 2 x_2 + x_3 +   x_4 - 14 s.t.
  2 x_1 +   x_2 + x_3 + 2 x_4  = 12
    x_1 +         x_3         >=  7
                  x_3 -   x_4 >=  1
  0 <= x_1 <= 2
  0 <= x_2 <= 4
  0 <= x_3 <= 6
  0 <= x_4 <= 3

Optimum solutions:
  Primal: x_1 = 1, x_2 = 0, x_3 = 6, x_4 = 2. Value: 5 + 0 + 6 + 2 - 14 = -1.
  Dual: [0.5, 4.0, 0.0]  Value: 6 + 28 - 3.5*6 - 14 = -1
"""
function example_lp()
  return FirstOrderLp.linear_programming_problem(
    [0.0, 0.0, 0.0, 0.0],  # variable_lower_bound
    [2.0, 4.0, 6.0, 3.0],  # variable_upper_bound
    [5.0, 2.0, 1.0, 1.0],  # objective_vector
    -14.0,                 # objective_constant
    [
      2.0 1.0 1.0 2.0
      1.0 0.0 1.0 0.0
      0.0 0.0 1.0 -1.0
    ],                     # constraint_matrix
    [12.0, 7.0, 1.0],      # right_hand_side
    1,                     # num_equalities
  )
end

""" Returns a one-variable LP with a no variable bounds.
The LP:
  min -x_1 s.t.
  -x_1 >= -2 (as a linear constraint)

Optimum solution:
  Primal: x_1 = 2, Value: -2.
  Dual: [1.0]  Value: -2.
"""
function example_lp_without_bounds()
  return FirstOrderLp.linear_programming_problem(
    [-Inf],                # variable_lower_bound
    [Inf],                 # variable_upper_bound
    [-1.0],                # objective_vector
    0.0,                   # objective_constant
    reshape([-1.0], 1, 1), # constraint_matrix
    [-2.0],                # right_hand_side
    0,                     # num_equalities
  )
end

""" Returns a small test QP.
The QP:
  min 2 x_1^2 + 0.5 x_2^2 - x_1 - x_2 s.t.
  x_1 + x_2  <= 1

  0 <= x_1 <= 1
  0 <= x_2 <= 1

Optimum solutions:
  Primal: x_1 = 0.2, x_2 = 0.8.
  Dual: [0.2]  Value: 0.04 + 0.32 - 0.2 - 0.8 = -0.6
"""
function example_qp()
  return FirstOrderLp.QuadraticProgrammingProblem(
    [0.0, 0.0],     # variable_lower_bound
    [1.0, 1.0],     # variable_upper_bound
    [
      4.0 0.0
      0.0 1.0
    ],              # objective_matrix
    [-1.0, -1.0],   # objective_vector
    -0.0,           # objective_constant
    [-1.0 -1.0],    # constraint_matrix
    [-1.0],         # right_hand_side
    0,              # num_equalities
  )
end

""" Returns a small test QP.
The QP:
  min 2 x_1^2 + 0.5 x_2^2 - x_1 - x_2 s.t.
  x_1 + x_2  <= 1

  0 <= x_1 <= 1
  0 <= x_2 <= 1

Optimum solutions:
  Primal: x_1 = 0.25, x_2 = 0.0.
  Dual: [0.0]  Value: 0.125 + 0 - 0.25 - 0 = -0.125
"""
function example_qp2()
  return FirstOrderLp.QuadraticProgrammingProblem(
    [0.0, 0.0],     # variable_lower_bound
    [1.0, 1.0],     # variable_upper_bound
    [
      4.0 0.0
      0.0 1.0
    ],              # objective_matrix
    [-1.0, 1.0],    # objective_vector
    -0.0,           # objective_constant
    [-1.0 -1.0],    # constraint_matrix
    [-1.0],         # right_hand_side
    0,              # num_equalities
  )
end

"""Returns a correlation clustering LP.
This is the LP for minimizing disagreements for correlation clustering for the
4-vertex graph
   1 - 3 - 4
   | /
   2
In integer solutions x_ij is 1 if i and j are in the same cluster and 0
otherwise. The 6 variables are in the order
 x_12, x_13, x_14, x_23, x_24, x_34.
For any distinct i,j,k there's a triangle inequality
  (1-x_ik) <= (1-x_ij) + (1-x_jk)
i.e.
  -x_ij - x_jk + x_ik >= -1.
For brevity we only include 3 out of the 12 possible triangle inequalities: two
needed in the optimal solution and 1 other."""
function example_cc_lp()
  return FirstOrderLp.linear_programming_problem(
    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],     # variable_lower_bound
    [1.0, 1.0, 1.0, 1.0, 1.0, 1.0],     # variable_upper_bound
    [-1.0, -1.0, 1.0, -1.0, 1.0, -1.0], # objective_vector
    4.0,                                # objective_constant
    [
      0.0 -1.0 1.0 0.0 0.0 -1.0
      0.0 0.0 0.0 -1.0 1.0 -1.0
      -1.0 -1.0 0.0 1.0 0.0 0.0
    ],                                  # constraint_matrix
    [-1.0, -1.0, -1.0],                 # right_hand_side
    0,                                  # num_equalities
  )
end

"""Returns another 4-vertex correlation clustering LP.

The variables are x_12, x_13, x_14, x_23, x_24, and x_34.
This time the graph is a star centered at vertex 1.
Only the three triangle inequalities that are needed are included."""
function example_cc_star_lp()
  return FirstOrderLp.linear_programming_problem(
    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],    # variable_lower_bound
    [1.0, 1.0, 1.0, 1.0, 1.0, 1.0],    # variable_upper_bound
    [-1.0, -1.0, -1.0, 1.0, 1.0, 1.0], # objective_vector
    3.0,                               # objective_constant
    [
      -1.0 -1.0 0.0 1.0 0.0 0.0
      -1.0 0.0 -1.0 0.0 1.0 0.0
      0.0 -1.0 -1.0 0.0 0.0 1.0
    ],      # constraint_matrix
    [-1.0, -1.0, -1.0],                # right_hand_side
    0,                                 # num_equalities
  )
end

""" Returns a small test LP with rows that are linearly dependent.
The LP:
  min x_1 + 2 x_2 + 3 x_3 +  4 x_4 s.t.
    x_1 +     x_2 +   x_3 +   x_4  == 2
    x_1 +     x_2 +   x_3 +   x_4  == 2
    x_1 +                     x_4  == 1
  0 <= x_1
  0 <= x_2
  0 <= x_3
  0 <= x_4

Optimum solution:
  Primal: x_1 = 1, x_2 = 1, x_3 = 0, x_4 = 0.
  Dual solution is not unique.
  Value: 1 + 2 = 3
"""
function example_lp_dependent_rows()
  return FirstOrderLp.linear_programming_problem(
    [0.0, 0.0, 0.0, 0.0],  # variable_lower_bound
    [Inf, Inf, Inf, Inf],  # variable_upper_bound
    [1.0, 2.0, 3.0, 4.0],  # objective_vector
    0.0,                   # objective_constant
    [
      1.0 1.0 1.0 1.0
      1.0 1.0 1.0 1.0
      1.0 0.0 0.0 1.0
    ],                     # constraint_matrix
    [2.0, 2.0, 1.0],       # right_hand_side
    3,                     # num_equalities
  )
end

""" Returns a small test LP that is primal infeasible.
The LP:
  min -x_1 + 0.5 x_2 s.t.
    -x_1 -    x_2   == 1
  0 <= x_1
  0 <= x_2
"""
function example_lp_easy_primal_infeasible()
  return FirstOrderLp.linear_programming_problem(
    [0.0, 0.0],           # variable_lower_bound
    [Inf, Inf],           # variable_upper_bound
    [1.0, 0.5],           # objective_vector
    0.0,                  # objective_constant
    [-1.0 -1.0],          # constraint_matrix
    [1.0],                # right_hand_side
    1,                    # num_equalities
  )
end

""" Returns a small test LP that is primal infeasible. Compared with
example_lp_easy_primal_infeasible() this is a hard problem. Note that
the nonnegativity constraints have no impact on feasibility.
The LP:
  min x_1 + 2 x_2 + 3 x_3 +  4 x_4 s.t.
    x_1 +     x_2                   == 1
              x_2 +   x_3           == 1
                      x_3 +    x_4  == 1
   x_1  +     x_2 +   x_3 +    x_4  == 2 + tol
  0 <= x_1
  0 <= x_2
  0 <= x_3
  0 <= x_4
"""
function example_lp_hard_primal_infeasible(tol::Float64)
  @assert tol > 0.0
  return FirstOrderLp.linear_programming_problem(
    [0.0, 0.0, 0.0, 0.0],      # variable_lower_bound
    [Inf, Inf, Inf, Inf],      # variable_upper_bound
    [1.0, 2.0, 3.0, 4.0],      # objective_vector
    0.0,                       # objective_constant
    [
      1.0 1.0 0.0 0.0
      0.0 1.0 1.0 0.0
      0.0 0.0 1.0 1.0
      1.0 1.0 1.0 1.0
    ],                         # constraint_matrix
    [1.0, 1.0, 1.0, 2 + tol],  # right_hand_side
    4,                         # num_equalities
  )
end

""" Returns a small test LP that is dual infeasible.
The LP:
  min -x_1 + 0.5 x_2 s.t.
    x_1 -     x_2   == 1
  0 <= x_1
  0 <= x_2
"""
function example_lp_dual_infeasible()
  return FirstOrderLp.linear_programming_problem(
    [0.0, 0.0],            # variable_lower_bound
    [Inf, Inf],            # variable_upper_bound
    [-1.0, 0.4],           # objective_vector
    0.0,                   # objective_constant
    [1.0 -2.0],            # constraint_matrix
    [1.0],                 # right_hand_side
    1,                     # num_equalities
  )
end


function imagesc(A::AbstractMatrix)
    Ny, Nx = size(A)

    if Nx > 30
        k = ceil(Nx / 30)
        A = A[:, 1:k:Nx]
    end

    if Ny > 30
        k = ceil(Ny / 30)
        A = A[1:k:Ny, :]
    end

    Ny, Nx = size(A)

    io = IOBuffer()

    t1 = quantile(A[:], 0.2)
    t2 = quantile(A[:], 0.4)
    t3 = quantile(A[:], 0.6)
    t4 = quantile(A[:], 0.8)

    print(io, "\n\n")

    for i = 1:Ny
        print(io, "\t")
        for j = 1:Nx
            if A[i, j] <= t1
                print(io, "  ")
            elseif A[i, j] <= t2
                print(io, ". ")
            elseif A[i, j] <= t3
                print(io, "+ ")
            elseif A[i, j] <= t4
                print(io, "# ")
            else
                print(io, "@#")
            end
        end
        print(io, "\n")
    end

    print(io, "\n")

    return ASCIIPlot(bytestring(io))
end
function imagesc(A::AbstractMatrix)
    Ny, Nx = size(A)

    if Nx > 30
        k = ceil(Nx / 30)
        A = A[:, 1:k:Nx]
    end

    if Ny > 30
        k = ceil(Ny / 30)
        A = A[1:k:Ny, :]
    end

    Ny, Nx = size(A)

    io = IOBuffer()

    t1 = quantile(A[:], 0.2)
    t2 = quantile(A[:], 0.4)
    t3 = quantile(A[:], 0.6)
    t4 = quantile(A[:], 0.8)

    print(io, "\n\n")

    for i = 1:Ny
        print(io, "\t")
        for j = 1:Nx
            if A[i, j] <= t1
                print(io, "  ")
            elseif A[i, j] <= t2
                print(io, ". ")
            elseif A[i, j] <= t3
                print(io, "+ ")
            elseif A[i, j] <= t4
                print(io, "# ")
            else
                print(io, "@#")
            end
        end
        print(io, "\n")
    end

    print(io, "\n")

    return ASCIIPlot(bytestring(io))
end


#    Copyright (c) 2021 J.A. Duffek
#    Copyright (c) 2000 D.M. Spink
#
#    This program is free software; you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation; either version 2 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program; if not, see <http://www.gnu.org/licenses/>.

"""
    bspderiv(d::I,c::Matrix{F},k::Vector{F}
            ) where {I<:Integer,F<:AbstractFloat}

Evaluate the control points and knot sequence of the derivative of a univariate
B-Spline.

# Arguments:
- `d`: Degree of the B-Spline.
- `c`: Control points, matrix of size `(dim,nc)`.
- `k`: Knot sequence, vector of size `nk`.

# Output:
- `dc`: Control points of the derivative.
- `dk`: Knot sequence of the derivative.

# Examples:
```julia
julia> dc,dk = bspderiv(d,c,k)
```

# Reference:
- derived from Algorithm A3.3 from 'The NURBS BOOK' pg98 ([1997Piegl](@cite))
"""
function bspderiv(d::I,c::Matrix{F},k::Vector{F}
                   ) where {I<:Integer,F<:AbstractFloat}
# TODO: the original C function passed c, k, dc, and dk as reference
# TODO: this function has optimization potential

mc,nc = size(c);
nk = length(k);

# control points of the derivative
dc = zeros(F,(mc,nc-1));
for i in 0:nc-2
  tmp = d / (k[i+d+2] - k[i+2]);
  for j in 0:mc-1
    # TODO yeah 1-1 is zero have a look if that is indented to have weights set
    #      to zero
    dc[j+1,i+1] = tmp * (c[j+1,i+2] - c[j+1,i+1]);
  end # for j
end # for i

# knots of the derivative
dk = zeros(F,nk-2);
for i in 1:nk-2
  dk[i] = k[i+1];
end # for i

return dc,dk;
end # bspderiv


using Pkg
Pkg.activate(".")
using Test, LinearAlgebra, Plots, Revise, RecursiveClusterAlgorithm
gr(grid=false,legend=false)

## get_dipoles
# some vortices
xi = [-0.549 1.160 0.335 -1.773]
yi = [0.123 -0.603 -0.079 0.220]

scatter(xi[1:2],yi[1:2],m=:circle,c=:red)
scatter!(xi[3:4],yi[3:4],m=:circle,c=:blue)

n = 4
nplus = 2
Lx = 1.2
Ly = 0.3

κi = [ones(nplus); -ones(n-nplus)]

xd,yd,is_dipole,num_found_in_dipoles = get_dipoles(xi,yi,n,nplus,Lx,Ly)

@test num_found_in_dipoles == 2

## seed_clusters
xi = [-0.549 -0.548 0.160 0.335 -1.772 -1.78]
yi = [0.123 0.16 -0.103 -0.079 0.20 0.220]
xi,yi=xi[:],yi[:]
scatter(xi[1:3],yi[1:3],m=:circle,c=:red)
scatter!(xi[4:6],yi[4:6],m=:circle,c=:blue)
n = 6
nplus = 3
Lx = 1.2
Ly = 0.3
xs,ys,is_seed,seeds_index,num_found_in_seeds = seed_clusters(xi,yi,n,nplus,Lx,Ly)

@test num_found_in_seeds == 4

## grow_plus_clusters

## RCA
# some vortices
xp,yp = xi[1:3],yi[1:3]
xn,yn = xi[4:6],yi[4:6]

# RCA body code
nplus = length(xp)
nminus = length(xn)
n = nplus + nminus
kappai = [ones(nplus); -ones(nminus)]
vortex_index = 1:n

#
#   @info "All vortices are in dipoles: nothing left to do..."
#   PCLUSTERS = struct()
#   NCLUSTERS = struct()
#   DIPOLES.x = x_dipoles
#   DIPOLES.y = y_dipoles
#   DIPOLES.n = size(x_dipoles,1)
#
#   if PLOT
#  PlotDipoles(x_dipoles,y_dipoles,Lx,Ly)
#   end
#   return

# elseif num_found_in_dipoles == n-2 && sum(kappanew == 1) == 1 && sum(kappanew == -1) ==1
#   # NOTE!! This part is a HACK, to fix a BUG currently lurking in MATLAB
#   # 2012b (16/07/2014). RCA fails when there are only two vortices left, each
#   # of different signs (which necessarily must form a dipole). For some reason
#   # MATLAB thinks that the arrays are not the same size when using ==, even
#   # when they are. The problem only occurs when the routine is a function,
#   # so keyboard doesn"t help to debug either. It works fine in the command
#   # line...
#    x_dipoles = [x_dipoles xnew(1) xnew(2)]
#    y_dipoles = [y_dipoles ynew(1) ynew(2)]
#    PCLUSTERS = struct()
#    NCLUSTERS = struct()
#    DIPOLES.x = x_dipoles
#    DIPOLES.y = y_dipoles
#    DIPOLES.n = size(x_dipoles,1)
#
#   # if PLOT
#   #   PlotDipoles(x_dipoles,y_dipoles,Lx,Ly)
#   # end
#
#   @info "All vortices are in dipoles: nothing left to do..."
#   return
# end

## TODO Recursively look for dipoles, until no more can be found
# initial dipole search
x_dipoles,y_dipoles,is_dipole,num_found_in_dipoles = get_dipoles(xi,yi,n,nplus,Lx,Ly)

xnew = @. xi[~Bool(is_dipole)]
ynew = @. yi[~Bool(is_dipole)]
kappanew = @. kappai[~Bool(is_dipole)]
remaining_vortices_index =  @. vortex_index[~Bool(is_dipole)]

np = sum(@. kappanew==1.0)
ntot = length(kappanew)

if num_found_in_dipoles == n
  @info "All vortices are in dipoles. Finished."
end

# while (num_found_in_dipoles >0) && ~isempty(xnew)
  xd,yd,is_dipole2,num_found_in_dipoles = get_dipoles(xnew,ynew,ntot,np,Lx,Ly)

  add_to_dipoles = @. remaining_vortices_index[Bool(is_dipole2)]

  is_dipole = Bool.(is_dipole)
  @. is_dipole[add_to_dipoles] = true

  xnew = @. xi[~is_dipole]
  ynew = @. yi[~is_dipole]
  kappanew = @. kappai[~is_dipole]
  remaining_vortices_index = @. vortex_index[~is_dipole]
  np = sum(@. kappanew==1.0)
  ntot = length(kappanew)
  x_dipoles = [x_dipoles; xd] # ok
  y_dipoles = [y_dipoles; yd] # ok. A little sloppy here ...
# end


#==============================================================================#
#
#                 some general & simple utility functions
#
#==============================================================================#

"""
    outer(x)

Compute an outer product
"""
outer(x) = x*x'

"""
    get_or_default(container, elem::Symbol, default)

Return `container`.`elem` if it exists, otherwise return `default`
"""
function get_or_default(container, elem::Symbol, default)
    haskey(container, elem) ? getindex(container, elem) : default
end

"""
    static_accessor_HFc(u::SVector, ::Val{T}) where T

Access data stored in the container `u` so that it matches the shapes of H,F,c
and points to the correct points in `u`. `T` is the dimension of the stochastic
process.
"""
function static_accessor_HFc(u::K, ::Val{T}) where {K<:Union{SVector,MVector},T}
    Hidx = SVector{T*T,Int64}(1:T*T)
    Fidx = SVector{T,Int64}((T*T+1):(T*T+T))
    #cidx = SVector{1,Int64}((T*T+T+1):(T*T+T+1))
    reshape(u[Hidx], Size(T,T)), u[Fidx], u[T*T+T+1]
end

@doc raw"""
    standard_guid_prop_time_transf(tt)

Standard time transformation used for guided proposals:
```math
τ: t → t_0 + (t-t_0)\left[2 - \frac{(t-t_0)}{T-t_0}\right],
```
applied to a vector `tt` where $t_0:=$`tt[1]` and $T:=$`tt[end]`
"""
standard_guid_prop_time_transf(tt) = τ(tt[1], tt[end]).(tt)
τ(t₀,T) = (t) ->  t₀ + (t-t₀) * (2-(t-t₀)/(T-t₀))


## This makes implementing operators simpler
# but overrided BandedMatrix directly is likely to be faster

abstract type TridiagonalOperator{T} <: Operator{T} end
abstract type SymTridiagonalOperator{T} <: TridiagonalOperator{T} end
abstract type BidiagonalOperator{T} <: TridiagonalOperator{T} end
abstract type DiagonalOperator{T} <: BidiagonalOperator{T} end
# override getindex

bandwidths(::TridiagonalOperator)=1,1
bandwidths(::BidiagonalOperator)=0,1
bandwidths(::DiagonalOperator)=0,0

Tridiagonal(J::TridiagonalOperator{T},n::Integer) where {T}=Tridiagonal(T[J.γ(k) for k=2:n],
T[J.α(k) for k=1:n],
T[J.β(k) for k=1:n-1])

function symmetrize(J::TridiagonalOperator{T},n::Integer) where T
    d=Array{T}(undef, n)
    d[1]=1
    for k=2:n
        d[k]=sqrt(J[k,k-1]/J[k-1,k])*d[k-1]
    end

   SymTridiagonal(
    T[J[k,k] for k=1:n],
    T[J[k,k+1]*d[k+1]/d[k] for k=1:n-1])
end

struct DiagIteratorOperator{IT,T} <: DiagonalOperator{T}
    iterator::IT
end

DiagIteratorOperator(it) = DiagIteratorOperator{typeof(it),eltype(it)}(it)

getindex(D::DiagIteratorOperator,k::Integer,j::Integer) =
    k==j ? D.iterator[k] : zero(eltype(D))

domainspace(D::DiagIteratorOperator) = ℓ⁰
rangespace(D::DiagIteratorOperator) = ℓ⁰


#!/usr/bin/env julia

### images.jl
#
# Copyright (C) 2016 Mosè Giordano.
#
# Maintainer: Mosè Giordano <mose AT gnu DOT org>
# Keywords: periodogram, lomb scargle
#
# This file is a part of LombScargle.jl.
#
# License is BSD 3-clause "New" or "Revised".
#
### Commentary:
#
# This file produces images used in documentation.
#
### Code:

using StableRNGs

rng = StableRNG(42)

using Measurements, LombScargle, Plots

sz = (1200, 800)

# First periodograms
ntimes = 1001
t = range(0.01, stop=10pi, length=ntimes)
t += step(t)*rand(rng, ntimes)
s = @. sinpi(t) + 1.5cospi(2t) + rand(rng)
p = lombscargle(t, s)
plot(freqpower(p)..., size = sz, xlim = (0.0, 2.0), xlabel = "Frequency",
     ylabel = "Lomb–Scargle power")
savefig(joinpath(@__DIR__, "freq-periodogram.png"))
plot(periodpower(p)..., size = sz, xlim = (0.5, 2.5), xlabel = "Period",
     ylabel = "Lomb–Scargle power")
savefig(joinpath(@__DIR__, "period-periodogram.png"))

# signal with uncertainties
ntimes = 1001
t = range(0.01, stop=10pi, length=ntimes)
s = @. sinpi(2t)
errors = rand(rng, 0.1:1e-3:4.0, ntimes)
p = lombscargle(t, s, errors, maximum_frequency=1.5)
plot(freqpower(p)..., size = sz, xlim = (0.25, 1.5), xlabel = "Frequency",
     ylabel = "Lomb–Scargle power")
savefig(joinpath(@__DIR__, "freq-uncertainties.png"))
plot(periodpower(p)..., size = sz, xlim = (0.75, 2.0), xlabel = "Period",
     ylabel = "Lomb–Scargle power")
savefig(joinpath(@__DIR__, "period-uncertainties.png"))


function DFS!(G)
	v = 1:size(G.AdjMatrix, 1);
	for u = v
		G.V[u].color = 'W';
		G.V[u].pred = -1;
	end
	for u = v
		if G.V[u].color == 'W';
			DFS_Visit!(G, u);
		end
	end
end
function DFS_Visit!(G, u)
	G.V[u].color = 'G';
        for v in findall(G.AdjMatrix[u,:] .==1)
		if G.V[v].color == 'W'
			G.V[v].pred = u;
			DFS_Visit!(G, v);
		end
	end
	G.V[u].color = 'B';
end


using DelimitedFiles

function ReadData(fname::AbstractString)
    data = readdlm(fname, String)
    return data
end

# useful constants
const openers = ['(', '[', '{', '<']
const closers = Dict([(')','('), (']','['), ('}','{'), ('>','<')])
const errorScores  = Dict([(')',3), (']',57), ('}',1197), ('>',25137)])
const incomScores  = Dict([('(',1), ('[',2),  ('{',3),    ('<',4)])

function Part1(data)
    score = 0
    for line in data
        stack = []
        for c in line
            if c in openers
                pushfirst!(stack, c)
            else
                o = popfirst!(stack)
                if closers[c] != o
                    score += errorScores[c]
                    break
                end
            end
        end
    end

    return score
end

function Part2(data)
    scores = []
    for line in data
        stack = []
        skipline = false
        for c in line
            if c in openers
                pushfirst!(stack, c)
            else
                o = popfirst!(stack)
                if closers[c] != o
                    skipline = true
                    break
                end
            end
        end

        if !skipline && length(stack) > 0
            score = 0
            while length(stack) > 0
                o = popfirst!(stack)
                score = 5*score + incomScores[o]
            end
            push!(scores, score)
        end
    end

    sort!(scores)
    return scores[div(length(scores),2) + 1]
end

#data = ReadData("../data/test10.txt")
data = ReadData("../data/day10.txt")

println(Part1(data))
println(Part2(data))


using OptimalTransport

using Distances
using ForwardDiff
using LogExpFunctions
using PythonOT: PythonOT

using LinearAlgebra
using Random
using Test

const POT = PythonOT

Random.seed!(100)

@testset "sinkhorn_stabilized.jl" begin
    # size of source and target
    M = 250
    N = 200

    # create two random histograms
    μ = normalize!(rand(M), 1)
    ν = normalize!(rand(N), 1)

    # create random cost matrix
    C = pairwise(SqEuclidean(), rand(1, M), rand(1, N); dims=2)

    # regularization parameter
    ε = 0.01

    @testset "example" begin
        # compute optimal transport plan and optimal transport cost
        γ = sinkhorn(μ, ν, C, ε, SinkhornStabilized(); maxiter=5_000, rtol=1e-9)
        c = sinkhorn2(μ, ν, C, ε, SinkhornStabilized(); maxiter=5_000, rtol=1e-9)

        # check that plan and cost are consistent
        @test c ≈ dot(γ, C)

        # compare with POT
        γ_pot = POT.sinkhorn(
            μ, ν, C, ε; method="sinkhorn_stabilized", numItermax=5_000, stopThr=1e-9
        )
        c_pot = POT.sinkhorn2(
            μ, ν, C, ε; method="sinkhorn_stabilized", numItermax=5_000, stopThr=1e-9
        )[1]
        @test γ_pot ≈ γ rtol = 1e-6
        @test c_pot ≈ c rtol = 1e-7

        # compute optimal transport cost with regularization term
        c_w_regularization = sinkhorn2(
            μ, ν, C, ε, SinkhornStabilized(); maxiter=5_000, regularization=true
        )
        @test c_w_regularization ≈ c + ε * sum(x -> iszero(x) ? x : x * log(x), γ)

        # ensure that provided plan is used and correct
        c2 = sinkhorn2(similar(μ), similar(ν), C, rand(), SinkhornStabilized(); plan=γ)
        @test c2 ≈ c
        c2_w_regularization = sinkhorn2(
            similar(μ), similar(ν), C, ε, SinkhornStabilized(); plan=γ, regularization=true
        )
        @test c2_w_regularization ≈ c_w_regularization

        # batches of histograms
        d = 10
        for (size2_μ, size2_ν) in
            (((), (d,)), ((1,), (d,)), ((d,), ()), ((d,), (1,)), ((d,), (d,)))
            # generate batches of histograms
            μ_batch = repeat(μ, 1, size2_μ...)
            ν_batch = repeat(ν, 1, size2_ν...)

            # compute optimal transport plan and check that it is consistent with the
            # plan for individual histograms
            γ_all = sinkhorn(
                μ_batch, ν_batch, C, ε, SinkhornStabilized(); maxiter=5_000, rtol=1e-9
            )
            @test size(γ_all) == (M, N, d)
            @test all(view(γ_all, :, :, i) ≈ γ for i in axes(γ_all, 3))

            # compute optimal transport cost and check that it is consistent with the
            # cost for individual histograms
            c_all = sinkhorn2(
                μ_batch, ν_batch, C, ε, SinkhornStabilized(); maxiter=5_000, rtol=1e-9
            )
            @test size(c_all) == (d,)
            @test all(x ≈ c for x in c_all)
        end
    end

    @testset "infinite absorption tolerance" begin
        # compute optimal transport plan with infinite absorption tolerance
        alg = SinkhornStabilized(; absorb_tol=Inf)
        γ = sinkhorn(μ, ν, C, ε, alg; maxiter=100)
        c = sinkhorn(μ, ν, C, ε, alg; maxiter=100)

        # compare with regular Sinkhorn algorithm
        γ_sinkhorn = sinkhorn(μ, ν, C, ε; maxiter=100)
        c_sinkhorn = sinkhorn(μ, ν, C, ε; maxiter=100)
        @test γ_sinkhorn ≈ γ
        @test c_sinkhorn ≈ c
    end

    @testset "AD" begin
        # compute gradients with respect to source and target marginals separately and
        # together
        ForwardDiff.gradient(zeros(N)) do xs
            return sinkhorn2(μ, softmax(xs), C, ε, SinkhornStabilized())
        end
        ForwardDiff.gradient(zeros(M)) do xs
            return sinkhorn2(softmax(xs), ν, C, ε, SinkhornStabilized())
        end
        ForwardDiff.gradient(zeros(M + N)) do xs
            return sinkhorn2(
                softmax(xs[1:M]), softmax(xs[(M + 1):end]), C, ε, SinkhornStabilized()
            )
        end
    end

    @testset "deprecations" begin
        γ = sinkhorn(μ, ν, C, ε, SinkhornStabilized(; absorb_tol=10))
        @test (@test_deprecated sinkhorn_stabilized(μ, ν, C, ε; absorb_tol=10)) == γ

        γ = sinkhorn(μ, ν, C, ε, SinkhornStabilized(); atol=1e-6)
        @test (@test_deprecated sinkhorn_stabilized(μ, ν, C, ε; tol=1e-6)) == γ

        γ = sinkhorn(μ, ν, C, ε, SinkhornStabilized())
        u, v = @test_deprecated sinkhorn_stabilized(μ, ν, C, ε; return_duals=true)
        @test exp.(-(C .- u .- v') / ε) ≈ γ
    end
end


legalMoves = [2 3 4
    1 5 7
    1 6 8
    1 9 10
    2 6 11
    3 5 12
    2 9 13
    3 10 14
    4 7 15
    4 8 16
    5 13 17
    6 14 17
    7 11 18
    8 12 19
    9 16 18
    10 15 19
    11 12 20
    13 15 20
    14 16 20
    17 18 19]
# Each line of this matrix legalMoves represents a vertex and specifies three neighboring vertices.

#= Function move expects one number as a current vertex and a matrix of legal moves.
It returns one number as a new, randomly chosen, legal, current vertex. =#
function move(n, A)
    return A[n, rand(1:3)]
end

#= Function simulate expects one number as the number of simulations and a matrix of legal moves.
It returns the average number of moves the spider needed in to get to an ant. =#
function simulate(noSimulations, B)
    sum = 0
    for i in 1:noSimulations
        #= The state of the simulation is reset.
        Ant is positioned in a vertex numbered 1.
        Spider is positioned in a vertex numbered 20.
        Number of moves is reset to 0. =#
        ant = 1
        spider = 20
        noMoves = 0

        # Until the spider moves to the desired position it moves in random, legal moves that are counted.
        while (ant != spider)
            spider = move(spider, B)
            noMoves += 1
        end

        # The number of moves needed in this simulation is added to the sum of all moves needed in previous simulations.
        sum += noMoves
    end

    # The average number of moves is calculated by dividing the sum of all moves needed in all simulations by the number of sumulations.
    return(sum / noSimulations)
end

# Specify the number of simulations for every function simulate call.
noSimulations = 1_000_000

# Run the function 10 times and print their averages.
for j in 1:10
    println(simulate(noSimulations, legalMoves))
end


struct ConvOp{T} <: AbstractArray{T,3}
    data::Array{T,3}
    k0::Array{Int,2}
    k1::Array{Int,2}
    tail::Array{T,2}
    length::Int
end

function Base.size(obj)
    return (size(obj.data)[2:3]...,obj.length)
end

function Base.getindex(obj::ConvOp, m::Int, n::Int, k::Int)
    if k < obj.k0[m,n]
        return zero(eltype(obj.data))
    end

    if k > obj.k1[m,n]
        return obj.tail[m,n]
    end

    return obj.data[k-obj.k0[m,n]+1,m,n]
end

function convolve!(y, Z::ConvOp, x, X, j, k_start, k_stop=size(Z,3))
    for n in axes(x,1)
        for m in axes(y,1)
            k0 = Z.k0[m,n]
            k1 = Z.k1[m,n]
            for k in max(k0,k_start):min(k1,k_stop)
                p = k - k0 + 1
                j-k < 0 && continue
                y[m] += Z.data[p,m,n] * x[n,j-k+1]
            end

            j-k1 > 0 || continue
            y[m] += Z.tail[m,n] * X[n,j-k1]
        end
    end
end

function polyeig(Z::ConvOp)
    kmax = maximum(Z.k1)
    M = size(Z,1)
    Q = zeros(eltype(Z), 2M, 2M, kmax+1)
    for k in 1:kmax
        Q[1:M,1:M,k] .= Z[:,:,k]
    end
    Id = Matrix{eltype(Z)}(LinearAlgebra.I,M,M)
    Q[M+1:2M,1:M,1] .= -Id
    Q[M+1:2M,M+1:2M,1] .= Id
    Q[M+1:2M,M+1:2M,2] .= -Id
    Q[1:M,M+1:2M,kmax+1] .= Z[:,:,kmax+1]
    return eigvals(companion(Q)), Q
    # return Q
end

function polyeig(Z)
    return eigvals(companion(Z))
end

function Base.:+(a::ConvOp, b::ConvOp)

    @assert size(a.data) == size(b.data)
    M,N = size(a.data)
    T = promote_type(eltype(a.data), eltype(b.data))

    k0 = min.(a.k0, b.k0)
    k1 = max.(a.k1, b.k1)

    bandwidth = maximum(k1 - k0) + 1
    data = zeros(T, bandwidth, M, N)

    k1max = maximum(k1)
    tail = zeros(T,M,N)
    for m in 1:M, n in 1:N
        tail[m,n] = a[m,n,k1max+1] + b[m,n,k1max+1]
    end

    for m in 1:M
        for n in 1:N
            for k in k0[m,n]:k1[m,n]
                data[k,m,n] = a[m,n,k] + b[m,n,k]
            end
        end
    end

    lgt = max(a.length, b.length)
    return ConvOp(data, tail, k0, k1, lgt)
end


"""
    markov_transition(μ::Function, Σ::Function, minp::AbstractFloat, statevectors::AbstractVector...)

Returns the sparse, Markov Transition Matrix (that is, `P[i,j] = Pr(i|j)`) where all
elements are `> minp`. The state-space is defined as the Cartesian Product of the `statevectors`.
The functions `μ` and `Σ` should take a tuple from `Base.product(statevectors...)` and return EITHER
  - the mean and **STANDARD DEVIATION** of `Distributions.Normal`, or
  - the mean and **VARIANCE MATRIX** of `Distributions.MvNormal`
"""
function markov_transition(μ::Function, Σ::Function, minp::AbstractFloat, statevectors::AbstractVector{T}...) where {T<:AbstractFloat}

  0. <= minp < 1. || throw(DomainError())

  state_prod = Base.product(statevectors...)

  P = zeros(T, length(state_prod), length(state_prod))

  for (j, s2) in enumerate(state_prod)
    for (i, s1) in enumerate(state_prod)
      dist = myDist(μ(s1), Σ(s1))
      P[i,j] = mypdf(dist, s2)
    end
  end

  return sparsify!(P, minp)
end

"""
    markovswitching_transition(μ::Function, Σ::Function, π::Matrix{Float64}, minp::AbstractFloat, statevectors::AbstractVector...)

Returns a sparse, Markov Transition Matrix (that is, `P[i,j] = Pr(i|j)`) where all
elements are `> minp`. The state space is defined as the Cartesian Product of the
`statevectors` with `1:size(π,1)`. The functions `μ(::Int, ::Tuple)` and `Σ(::Int, ::Tuple)`
should take the index of a regime `1:size(π,1)` and a tuple from `Base.product(statevectors...)`
and return EITHER
  - the mean and **STANDARD DEVIATION** of `Distributions.Normal`, or
  - the mean and **VARIANCE MATRIX** of `Distributions.MvNormal`
The markov-switching matrix is NOT transposed and equals `π[i,j] = Pr(j|i)`

"""
function markovswitching_transition(μ::Function, Σ::Function, π::Matrix{Float64}, minp::AbstractFloat, statevectors::AbstractVector{T}...) where {T<:AbstractFloat}

  k = size(π,1)
  k == size(π,2)   || throw(DimensionMismatch())
  all(sum(π, dims=2) .≈ 1.0) || throw(error("each row of π must sum to 1"))

  n = prod(map(length, statevectors))
  regimes = Base.OneTo(k)
  P = zeros(T, k*n, k*n)

  for r1 in regimes
    for r2 in regimes
      P[(r1-1)*n+1:r1*n,  (r2-1)*n+1:r2*n] .= π[r1, r2] .* markov_transition( (s) -> μ(r1, s), (s) -> Σ(r1, s), minp, statevectors... )
    end
  end

  return whichP(P, Val{minp > 0.0})
end


"""
    DescriptorSystems.dss(sys::AbstractStateSpace)

Convert `sys` to a descriptor statespace system from [DescriptorSystems.jl](https://andreasvarga.github.io/DescriptorSystems.jl/dev/index.html)
"""
function DescriptorSystems.dss(sys::AbstractStateSpace)
    A,B,C,D = ssdata(sys)
    DescriptorSystems.dss(A, B, C, D; Ts = isdiscrete(sys) ? sys.Ts : 0)
end

function ControlSystems.ss(G::DescriptorSystems.DescriptorStateSpace)
    try
        G,r = DescriptorSystems.gss2ss(G)
        if r < size(G.A, 1)
            G = DescriptorSystems.dss2ss(G, fast=false)
        end
    catch
        G,_ = DescriptorSystems.dss2ss(G, simple_infeigs = false, fast=false)
    end
    G.Ts >= 0 ? ss(G.A, G.B, G.C, G.D) : ss(G.A, G.B, G.C, G.D, G.Ts)
end

"""
    n, ω = hinfnorm2(sys::LTISystem; kwargs...)

A numerically robust version of `hinfnorm` using DescriptorSystems.jl

For keyword arguments, see the docstring of `DescriptorSystems.ghinfnorm`, reproduced below
$(@doc(DescriptorSystems.ghinfnorm))
"""
function hinfnorm2(sys::LTISystem; kwargs...)
    DescriptorSystems.ghinfnorm(dss(ss(sys)); kwargs...)
end

"""
    n = h2norm(sys::LTISystem; kwargs...)

A numerically robust version of `norm` using DescriptorSystems.jl

For keyword arguments, see the docstring of `DescriptorSystems.gh2norm`, reproduced below
$(@doc(DescriptorSystems.gh2norm))
"""
function h2norm(sys::LTISystem; kwargs...)
    DescriptorSystems.gh2norm(dss(ss(sys)); kwargs...)
end

"""
    n, hsv = hankelnorm(sys::LTISystem; kwargs...)

Compute the hankelnorm and the hankel singular values

For keyword arguments, see the docstring of `DescriptorSystems.ghanorm`, reproduced below
$(@doc(DescriptorSystems.ghanorm))
"""
function hankelnorm(sys::LTISystem; kwargs...)
    DescriptorSystems.ghanorm(dss(ss(sys)); kwargs...)
end

"""
    nugap(sys0::LTISystem, sys1::LTISystem; kwargs...)

Compute the ν-gap metric between two systems. See also [`ncfmargin`](@ref).

For keyword arguments, see the docstring of `DescriptorSystems.gnugap`, reproduced below
$(@doc(DescriptorSystems.gnugap))
"""
function nugap(sys0::LTISystem, sys1::LTISystem; kwargs...)
    DescriptorSystems.gnugap(dss(ss(sys0)), dss(ss(sys1)); kwargs...)
end

const νgap = nugap

"""
    baltrunc2(sys::LTISystem; residual=false, n=missing, kwargs...)

Compute the a balanced truncation of order `n` and the hankel singular values

For keyword arguments, see the docstring of `DescriptorSystems.gbalmr`, reproduced below
$(@doc(DescriptorSystems.gbalmr))
"""
function baltrunc2(sys::LTISystem; residual=false, n=missing, kwargs...)
    sysr, hs = DescriptorSystems.gbalmr(dss(sys); matchdc=residual, ord=n, kwargs...)
    ss(sysr), hs
end

##

function Base.:\(G1::AbstractStateSpace, G2::AbstractStateSpace)
    g1,g2 = dss(G1), dss(G2)
    try
        ss(g1\g2)
    catch
        error("Failed to solve G1\\G2, the product might not be proper. To keep the product on descriptor form, use dss(G1)\\dss(G2)")
    end
end


using Test: @test, @testset, @inferred
using LinearAlgebra: I, norm, Symmetric, Diagonal, LowerTriangular
using WhitenedRBMs: Affine, whitening_transform, whitening_transform!

@testset "affine" begin
    x = randn(5, 7)
    t = Affine(randn(5,5), randn(5))
    s = Affine(randn(5,5), randn(5))

    @test Affine(5).A ≈ I
    @test iszero(Affine(5).u)
    @test Affine(5) * x ≈ x

    @test t * x ≈ t.A * (x .- t.u)
    @test t \ (t * x) ≈ t * (t \ x) ≈ x

    @test (s * t) * x ≈ s * (t * x)
    @test (s \ t) * x ≈ s \ (t * x)
    @test (t \ t) * x ≈ x

    @test (s + t) * x ≈ s * x + t * x
    @test (s - t) * x ≈ s * x - t * x

    @test (2 * t) * x ≈ 2 * (t * x)
    @test (t * 2) * x ≈ t * (2 * x)
    @test (-t) * x ≈ -(t * x)

    @test inv(t) * x ≈ t \ x
    @test inv(t) * (t * x) ≈ (inv(t) * t) * x ≈ x

    @test (inv(t) * t).A ≈ I
    @test norm((inv(t) * t).u) < 1e-10
end

@testset "whitening_transform" begin
    μ = randn(5)
    C = randn(5,5)
    C = C * C'
    affine = whitening_transform(μ, C)
    @test norm(affine * μ) < 1e-10
    @test affine.A * C * affine.A' ≈ I
    @test affine.A' * affine.A ≈ inv(C)

    affine = whitening_transform(μ)
    @test affine.A ≈ I
    @test affine.u ≈ μ
    @test norm(affine * μ) < 1e-10

    affine = whitening_transform(C)
    @test affine.A * C * affine.A' ≈ I
    @test iszero(affine.u)

    affine = whitening_transform(C) * whitening_transform(μ)
    @test norm(affine * μ) < 1e-10
    @test affine.A * C * affine.A' ≈ I
end

@testset "whitening_transform!" begin
    μ = randn(5)
    C = randn(5,5)
    C = C * C'
    affine = whitening_transform!(Affine(LowerTriangular(randn(5,5)), randn(5)), μ, C)
    @test norm(affine * μ) < 1e-10
    @test affine.A * C * affine.A' ≈ I
    @test affine.A' * affine.A ≈ inv(C)

    affine = whitening_transform!(Affine(Diagonal(randn(5)), randn(5)), μ)
    @test affine.A ≈ I
    @test affine.u ≈ μ
    @test norm(affine * μ) < 1e-10

    affine = whitening_transform!(Affine(LowerTriangular(randn(5,5)), randn(5)), C)
    @test affine.A * C * affine.A' ≈ I
    @test iszero(affine.u)

    C = Diagonal(rand(5))
    affine = whitening_transform!(Affine(Diagonal(randn(5)), randn(5)), μ, C)
    @test norm(affine * μ) < 1e-10
    @test affine.A * C * affine.A' ≈ I
    @test affine.A' * affine.A ≈ inv(C)
end


# Original Java version Copyright 2005 Google Inc.
# Julia translation Copyright 2022 Ted Dunning
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

using S2
using LinearAlgebra
using Test

# Basics

# Check that the conversion between Pi radians and 180 degrees is exact.
@test Float64(π) == radians(S1Angle(π))
@test 180.0 == degrees(S1Angle(π))
@test Float64(π) == radians(degrees(180))
@test 180.0 == degrees(degrees(180))

@test 90.0 == degrees(radians(π / 2))
@test 90.0 == degrees(S1Angle(π / 2))

@test -90.0 == degrees(S1Angle(-π / 2))
@test Float64(-π / 4) == radians(degrees(-45))

@test 30.0 ≈ degrees(degrees(10) + degrees(20))
@test -10 ≈ degrees(degrees(10) - degrees(20))
@test 20 ≈ degrees(2 * degrees(10))
@test 5 ≈ degrees(degrees(10) / 2.0)
@test 1 ≈ cos(S1Angle(0))
@test 1 ≈ sin(degrees(90))
@test 1 ≈ tan(degrees(45))

@test 100.0 * π ≈ distance(S1Angle(π), 100.0)
@test 50.0 * π ≈ distance(S1Angle(π / 2), 100.0)
@test 25.0 * π ≈ distance(radians(π / 4), 100.0)

@test 0.0 ≈ radians(normalizeRange(degrees(360.0)))
@test -90.0 ≈ degrees(normalizeRange(degrees(-90.0)))
@test 180.0 ≈ degrees(normalizeRange(degrees(-180.0)))
@test 90.0 ≈ degrees(normalizeRange(degrees(90.0)))
@test 180.0 ≈ degrees(normalizeRange(degrees(180.0)))
@test -90.0 ≈ degrees(normalizeRange(degrees(270.0)))
@test 180.0 ≈ degrees(normalizeRange(degrees(540.0)))
@test 90.0 ≈ degrees(normalizeRange(degrees(-270.0)))

@test π ≈ radians(normalizeRange(radians(π)))

# -π maps to π.
@test π ≈ radians(normalizeRange(radians(-π)))

let a = degrees(90.0)
    @test a === normalizeRange(a)
end

@test 1.5 == radians(S1Angle(0.5) + S1Angle(0.75) + S1Angle(0.25))

@test 180 == degrees(normalizeRange(degrees(90) + degrees(45) + degrees(30) + degrees(15)))

@test 0.75 * π ≈ radians(S1Angle(π / 2) + degrees(45))


"""
```
hog_params = HOG([orientations = 9], [cell_size = 8], [block_size = 2], [block_stride = 1], [norm_method = "L2-norm"])
```

Histogram of Oriented Gradient (HOG) is a dense feature desciptor usually used for object detection. See "Histograms of Oriented Gradients for Human Detection" by Dalal and Triggs.

Parameters:
-    orientations   = number of orientation bins
-    cell_size      = size of a cell is cell_size x cell_size (in pixels)
-    block_size     = size of a block is block_size x block_size (in terms of cells)
-    block_stride   = stride of blocks. Controls how much adjacent blocks overlap.
-    norm_method    = block normalization method. Options: L2-norm, L2-hys, L1-norm, L2-sqrt.
"""
mutable struct HOG <: Params
    orientations::Int
    cell_size::Int
    block_size::Int
    block_stride::Int
    norm_method::String
end

function HOG(; orientations::Int = 9, cell_size::Int = 8, block_size::Int = 2, block_stride::Int = 1, norm_method::String = "L2-norm")
    HOG(orientations, cell_size, block_size, block_stride, norm_method)
end

function create_descriptor(img::AbstractArray{CT, 2}, params::HOG) where CT<:Images.NumberLike
    #compute gradient
    gx = imfilter(img, centered([-1 0 1]))
    gy = imfilter(img, centered([-1 0 1]'))
    mag = hypot.(gx, gy)
    phase = orientation.(gx, gy)

    create_hog_descriptor(mag, phase, params)
end

function create_descriptor(img::AbstractArray{CT, 2}, params::HOG) where CT<:Images.Color{T, N} where T where N
    #for color images, compute seperate gradient for each color channel and take one with largest norm as pixel's gradient vector
    rows, cols = size(img)
    gx = channelview(imfilter(img, centered([-1 0 1])))
    gy = channelview(imfilter(img, centered([-1 0 1]')))
    mag = hypot.(gx, gy)
    phase = orientation.(gx, gy)

    max_mag = zeros(rows, cols)
    max_phase = zeros(rows, cols)

    for j in axes(mag, 3)
        for i in axes(mag, 2)
            ind = argmax(view(mag, :, i, j))
            max_mag[i, j] = mag[ind, i, j]
            max_phase[i, j] = phase[ind, i, j]
        end
    end

    create_hog_descriptor(max_mag, max_phase, params)
end

function create_hog_descriptor(mag::AbstractArray{T, 2}, phase::AbstractArray{T, 2}, params::HOG) where T<:Images.NumberLike
    orientations = params.orientations
    cell_size = params.cell_size
    block_size = params.block_size
    block_stride = params.block_stride

    rows, cols = size(mag)
    if rows%cell_size!=0 || cols%cell_size!=0
        error("Height and Width of the image must be a multiple of cell_size.")
    end

    cell_rows::Int = rows/cell_size
    cell_cols::Int = cols/cell_size
    if (cell_rows-block_size)%block_stride!=0 || (cell_cols-block_size)%block_stride!=0
        error("Block size and block stride don't match.")
    end

    phase = abs.(phase*180/pi)

    #orientation binning for each cell
    hist = zeros(Float64, (orientations, cell_rows, cell_cols))
    R = CartesianIndices(axes(mag))

    for i in R
        trilinear_interpolate!(hist, mag[i], phase[i], orientations, i, cell_size, cell_rows, cell_cols, rows, cols)
    end

    function normalize(v, method)
        if method == "L2-norm"
            return v./sqrt(sum(abs2, v) + 1e-5)
        elseif method == "L2-hys"
            v = v./(sum(abs2, v) + 1e-5)
            v = min.(v, 0.2)
            return v./(sum(abs2, v) + 1e-5)
        elseif method == "L1-norm"
            return v./(sum(abs, v) + 1e-5)
        elseif method == "L1-sqrt"
            return sqrt.(v./(sum(abs, v) + 1e-5))
        end
    end

    #contrast normalization for each block
    descriptor_size::Int = ((cell_rows-block_size)/block_stride + 1) * ((cell_cols-block_size)/block_stride + 1) * (block_size*block_size) * orientations
    descriptor = Vector{Float64}(undef, descriptor_size)
    block_vector_size = block_size * block_size * orientations
    k = 1
    for j in 1:block_stride:cell_cols-block_size+1
        for i in 1:block_stride:cell_rows-block_size+1
            H = @view hist[:, i:i+block_size-1, j:j+block_size-1]
            h = @view H[:]
            descriptor[block_vector_size*(k-1)+1 : block_vector_size*k] = normalize(h, params.norm_method)
            k += 1
        end
    end

    return descriptor
end

function trilinear_interpolate!(hist, w, θ, orientations, i, cell_size, cell_rows, cell_cols, rows, cols)
    bin_θ1 = min(floor(Int, θ*orientations/180) + 1, orientations)
    bin_θ2 = bin_θ1%orientations + 1
    b_θ = 180/orientations

    if bin_θ1 != orientations
        θ1 = (bin_θ1-1)*180/orientations
        θ2 = (bin_θ2-1)*180/orientations
    else
        θ1 = (bin_θ1-1)*180/orientations
        θ2 = 180.0;
    end

    if (i[1]<=cell_size/2 || i[1]>=rows-cell_size/2) && (i[2]<=cell_size/2 || i[2]>=cols-cell_size/2)
        #linear interpolation for corner points
        bin_x = i[1] < cell_size/2 ? 1 : cell_rows
        bin_y = i[2] < cell_size/2 ? 1 : cell_cols

        hist[bin_θ1, bin_x, bin_y] += w*(1-(θ-θ1)/b_θ)
        hist[bin_θ2, bin_x, bin_y] += w*(1-(θ2-θ)/b_θ)

    elseif i[1]<=cell_size/2 || i[1]>=rows-cell_size/2
        #bilinear interpolation for (top/bottom) edge points
        bin_x = i[1] < cell_size/2 ? 1 : cell_rows
        bin_y1 = floor(Int, (i[2]+cell_size/2)/cell_size)
        bin_y2 = bin_y1 + 1

        y1 = (bin_y1-1)*cell_size+cell_size/2
        y2 = (bin_y2-1)*cell_size+cell_size/2
        b_y = cell_size

        hist[bin_θ1, bin_x, bin_y1] += w*(1-(θ-θ1)/b_θ)*(1-(i[2]-y1)/b_y)
        hist[bin_θ1, bin_x, bin_y2] += w*(1-(θ-θ1)/b_θ)*(1-(y2-i[2])/b_y)
        hist[bin_θ2, bin_x, bin_y1] += w*(1-(θ2-θ)/b_θ)*(1-(i[2]-y1)/b_y)
        hist[bin_θ2, bin_x, bin_y2] += w*(1-(θ2-θ)/b_θ)*(1-(y2-i[2])/b_y)

    elseif  i[2]<=cell_size/2 || i[2]>=cols-cell_size/2
        #bilinear interpolation for (left/right) edge points
        bin_x1 = floor(Int, (i[1]+cell_size/2)/cell_size)
        bin_x2 = bin_x1 + 1
        bin_y = i[2] < cell_size/2 ? 1 : cell_cols

        x1 = (bin_x1-1)*cell_size+cell_size/2
        x2 = (bin_x2-1)*cell_size+cell_size/2
        b_x = cell_size

        hist[bin_θ1, bin_x1, bin_y] += w*(1-(θ-θ1)/b_θ)*(1-(i[1]-x1)/b_x)
        hist[bin_θ1, bin_x2, bin_y] += w*(1-(θ-θ1)/b_θ)*(1-(x2-i[1])/b_x)
        hist[bin_θ2, bin_x1, bin_y] += w*(1-(θ2-θ)/b_θ)*(1-(i[1]-x1)/b_x)
        hist[bin_θ2, bin_x2, bin_y] += w*(1-(θ2-θ)/b_θ)*(1-(x2-i[1])/b_x)
    else
        #trilinear interpolation
        bin_x1 = floor(Int, (i[1]+cell_size/2)/cell_size)
        bin_x2 = bin_x1 + 1
        bin_y1 = floor(Int, (i[2]+cell_size/2)/cell_size)
        bin_y2 = bin_y1 + 1

        x1 = (bin_x1-1)*cell_size+cell_size/2
        x2 = (bin_x2-1)*cell_size+cell_size/2
        b_x = cell_size

        y1 = (bin_y1-1)*cell_size+cell_size/2
        y2 = (bin_y2-1)*cell_size+cell_size/2
        b_y = cell_size

        hist[bin_θ1, bin_x1, bin_y1] += w*(1-(θ-θ1)/b_θ)*(1-(i[1]-x1)/b_x)*(1-(i[2]-y1)/b_y)
        hist[bin_θ1, bin_x1, bin_y2] += w*(1-(θ-θ1)/b_θ)*(1-(i[1]-x1)/b_x)*(1-(y2-i[2])/b_y)
        hist[bin_θ1, bin_x2, bin_y1] += w*(1-(θ-θ1)/b_θ)*(1-(x2-i[1])/b_x)*(1-(i[2]-y1)/b_y)
        hist[bin_θ1, bin_x2, bin_y2] += w*(1-(θ-θ1)/b_θ)*(1-(x2-i[1])/b_x)*(1-(y2-i[2])/b_y)
        hist[bin_θ2, bin_x1, bin_y1] += w*(1-(θ2-θ)/b_θ)*(1-(i[1]-x1)/b_x)*(1-(i[2]-y1)/b_y)
        hist[bin_θ2, bin_x1, bin_y2] += w*(1-(θ2-θ)/b_θ)*(1-(i[1]-x1)/b_x)*(1-(y2-i[2])/b_y)
        hist[bin_θ2, bin_x2, bin_y1] += w*(1-(θ2-θ)/b_θ)*(1-(x2-i[1])/b_x)*(1-(i[2]-y1)/b_y)
        hist[bin_θ2, bin_x2, bin_y2] += w*(1-(θ2-θ)/b_θ)*(1-(x2-i[1])/b_x)*(1-(y2-i[2])/b_y)
    end
end


#=
ControlFlowCamberFormula.jl
Jacob Child
Flow Lab Onboarding
Feb 3, 2022
Background: A Camber line is "a measure of the curvature of an airfoil"
    It is halfway between the upper and lower curves of the airfoil

    Equation
    if X<=p
        zbar = c*(2*p*x-x^2)/(p^2)
    else if x > p
        zbar = c*(1-2*p+2*p*x-x^2)/((1-p)^2)
    else
        println("error")

Pseudocode - ask for the c and p inputs , run an if statement and have A
for loop run through x values, 0:.1:1;
=#

println("Hello! We are going to calculate the the camber of an airfoil \n")
println("\n input c \n")
input1 = readline()
c = parse(Float64, input1)
println("\n input p \n")
input2 = readline()
p = parse(Float64, input2)
#println(p) #to check if it parsed correctly
x = [0:.1:1.1;]    #makes an array stopping at 1 and step size .1
global zbar = x[1:11]
global k = 1
while x[k] <= 1
    if x[k] <= p
        global zbar[k] = c*(2*p*x[k]-x[k]^2)/(p^2)
        global k = k + 1 #iterate

    elseif x[k] > p
        global zbar[k] = c*(1-2*p+2*p*x[k]-x[k]^2)/((1-p)^2)
        global k = k + 1 #iterate

    else
        println("error")
        break
    end

end
println("\n")
println(zbar)

pop!(x) #removes the last value of the x array to allow DataFrames to work
using DataFrames #only works in Julia REPL?
DataFrame()
DataFrame("X" => x, "zbar" => zbar)


export forecast, simulate

"""
    forecast(ss::StateSpace{Typ}, N::Int) where Typ

Obtain the minimum mean square error forecasts N steps ahead. Returns the forecasts and the predictive distributions
at each time period.
"""
function forecast(ss::StateSpace{Typ}, N::Int) where Typ

    # Load estimated covariance matrices
    H = ss.model.H
    Q = ss.model.Q

    # Load system
    n, p, m, r    = size(ss.model)
    Z, T, R, d, c = prepare_forecast(ss, N)

    # Load a, c, P, and F at last in-sample instant
    a0 = ss.smoother.alpha[end, :]
    c0 = ss.model.c[end, :]
    P0 = ss.filter.P[:, :, end]
    F0 = ss.filter.F[:, :, end]

    # State and variance forecasts
    a = Matrix{Typ}(undef, N, m)
    P = Array{Typ, 3}(undef, m, m, N)
    F = Array{Typ, 3}(undef, p, p, N)

    # Probability distribution
    dist = Vector{Distribution}(undef, N)

    # Initialization
    a[1, :]    = T*a0 + c0
    P[:, :, 1] = T*P0*T' + R*Q*R'
    F[:, :, 1] = Z[:, :, 1]*P[:, :, 1]*Z[:, :, 1]' + H
    ensure_pos_sym!(F, 1)
    dist[1]    = MvNormal(vec(Z[:, :, 1]*a[1, :] + d[1, :]), F[:, :, 1])

    for t = 2:N
        a[t, :]    = T*a[t-1, :] + c[t-1, :]
        P[:, :, t] = T*P[:, :, t-1]*T' + R*Q*R'
        F[:, :, t] = Z[:, :, t]*P[:, :, t]*Z[:, :, t]' + H
        ensure_pos_sym!(F, t)
        dist[t]    = MvNormal(vec(Z[:, :, t]*a[t, :] + d[t, :]), F[:, :, t])
    end

    forec = Matrix{Typ}(undef, N, p)
    for t = 1:N
        forec[t, :] = mean(dist[t])
    end

    return forec, dist
end

"""
    simulate(ss::StateSpace{Typ}, N::Int, S::Int) where Typ

Simulate S future scenarios up to N steps ahead. Returns a p x N x S matrix where the dimensions represent, respectively,
the number of series in the model, the number of steps ahead, and the number of scenarios.
"""
function simulate(ss::StateSpace{Typ}, N::Int, S::Int) where Typ

    # Load estimated covariance matrices
    H = ss.model.H
    Q = ss.model.Q

    # Load system
    n, p, m, r    = size(ss.model)
    Z, T, R, d, c = prepare_forecast(ss, N)

    # Distribution of the state space errors
    dist_ϵ = MvNormal(zeros(p), H)
    dist_η = MvNormal(zeros(r), Q)

    αsim = Array{Typ, 3}(undef, N, m, S)
    ysim = Array{Typ, 3}(undef, N, p, S)

    for s = 1:S

        # Sampling errors
        ϵ = rand(dist_ϵ, N)'
        η = rand(dist_η, N)'

        # Initializing simulation
        αsim[1, :, s] = T*ss.smoother.alpha[n, :] + c[1, :] + R*η[1, :]
        ysim[1, :, s] = Z[:, :, 1]*αsim[1, :, s] + d[1, :] + ϵ[1, :]

        # Simulating future scenarios
        for t = 2:N
            αsim[t, :, s] = T*αsim[t-1, :, s] + c[t-1, :] + R*η[t, :]
            ysim[t, :, s] = Z[:, :, t]*αsim[t, :, s] + d[t, :] + ϵ[t, :]
        end
    end

    return ysim
end

"""
    prepare_forecast(ss::StateSpace{Typ}, N::Int) where Typ

Adjust matrix Z for forecasting and check for dimension errors.
"""
function prepare_forecast(ss::StateSpace{Typ}, N::Int) where Typ

    # Load system
    n, p, m, r = size(ss.model)
    Z0, T, R   = ztr(ss.model)
    d0 = ss.model.d
    c0 = ss.model.c

    Z = Array{Typ, 3}(undef, p, m, N)
    d = Matrix{Typ}(undef, N, p)
    c = Matrix{Typ}(undef, N, m)
    if ss.model.mode == "time-invariant"
        for t in 1:N, i in 1:p, j in 1:m
            Z[i, j, t] = Z0[i, j, 1]
            d[t, i] = d0[1, i]
            c[t, j] = c0[1, j]
        end
    else
        size(Z0, 3) < n + N && error("Time-variant Z too short for forecasting $N steps ahead")
        size(d0, 1) < n + N && error("Time-variant d too short for forecasting $N steps ahead")
        size(c0, 1) < n + N && error("Time-variant c too short for forecasting $N steps ahead")

        for t in n + 1:n + N, i in 1:p, j in 1:m
            Z[i, j, t - n] = Z0[i, j, t]
            d[t - n, i] = d0[t, i]
            c[t - n, j] = c0[t, j]
        end
    end

    return Z, T, R, d, c
end