"""

```math
\\tilde X = \\hat X_{t+1|t} = A\\hat X_{t|t}
```
"""
function KalmanFilter(M::LGSSM, y)
    RR = M.R * M.R'
    SS = M.S * M.S'
    ydim, qdim = size(M.B)
    xdim, pdim = size(M.R)
    n = size(y, 1)

    ϵ = zeros(ydim, 1, n)
    Γ = zeros(ydim, ydim, n)
    Γ⁻¹ = zeros(ydim, ydim, n)
    K = zeros(xdim, ydim, n) # Kalman filter gain
    H = zeros(xdim, ydim, n) # Kalman prediction gain
    Λ = zeros(xdim, xdim, n)

    Xp = zeros(xdim, 1, n)
    Σp = zeros(xdim, xdim, n)
    Xf = zeros(xdim, 1, n)
    Σf = zeros(xdim, xdim, n)

    ℓ = 0.0

    for t in 1:n
        # Prediction
        if t == 1
            Xp[:,:,t] = 0.0
            Σp[:,:,t] = M.Σᵥ
        else
            Xp[:,:,t] = M.A * Xf[:,:,t-1]
            Σp[:,:,t] = M.A * Σf[:,:,t-1] * M.A' + RR
        end

        # Correction
        ϵ[:,:,t] = y[t] - M.B * Xp[:,:,t]
        Γ[:,:,t] = M.B*Σp[:,:,t]*M.B' + SS
        Γ⁻¹[:,:,t] = inv(Γ[:,:,t])
        K[:,:,t] = Σp[:,:,t]*M.B'*Γ⁻¹[:,:,t]
        H[:,:,t] = M.A*K[:,:,t]
        Λ[:,:,t] = M.A - H[:,:,t]*M.B
        Xf[:,:,t] = Xp[:,:,t] + K[:,:,t]*ϵ[:,:,t]
        Σf[:,:,t] = Σp[:,:,t] - K[:,:,t]*M.B*Σp[:,:,t]
        ℓ += logdet(Γ[:,:,t]) + ϵ[:,:,t]'*Γ⁻¹[:,:,t]*ϵ[:,:,t]
    end
    return Dict(:Xp => Xp,
                :Xf => Xf,
                :Σp => Σp,
                :Σf => Σf,
                :ϵ => ϵ,
                :Γ => Γ,
                :Γ⁻¹ => Γ⁻¹,
                :K => K,
                :H => H,
                :Λ => Λ,
                :ℓ => -ℓ/2)
end

"""

    Disturbanve Smoother

    Algorithm 5.2.15 Cappe 2005
"""
function DisturbanceSmoother(M::LGSSM, y)
    n = size(y, 1)
    pdim = size(M.B, 2)
    qdim = size(y, 2)
    RR = M.R*M.R'

    p = zeros(pdim, 1, n-1)
    C = zeros(pdim, pdim, n-1)
    U = zeros(pdim, pdim, n-1)
    Ξ = zeros(pdim, pdim, n-1)

    Xs = zeros(pdim, 1, n)
    Σs = zeros(pdim, pdim, n)

    KF = KalmanFilter(M, y)

    # Smoothed disturbances
    for t in n-1:-1:1
        if t == n-1
            p[:,:,t] = M.B'*KF[:Γ⁻¹][:,:,n]*KF[:ϵ][:,:,n]
            C[:,:,t] = M.B'*KF[:Γ⁻¹][:,:,n]*M.B
        else
            p[:,:,t] = M.B'*KF[:Γ⁻¹][:,:,t+1]*KF[:ϵ][:,:,t+1] + KF[:Λ][:,:,t+1]'*p[:,:,t+1]
            C[:,:,t] = M.B'*KF[:Γ⁻¹][:,:,t+1]*M.B + KF[:Λ][:,:,t+1]'*C[:,:,t+1]*KF[:Λ][:,:,t+1]
        end
        U[:,:,t] = M.R'*p[:,:,t]
        Ξ[:,:,t] = I - M.R'*C[:,:,t]*M.R
    end

    # Smoothed states
    Xs[:,:,1] = M.Σᵥ*(M.B'*KF[:Γ⁻¹][:,:,1]*KF[:ϵ][:,:,1] + KF[:Λ][:,:,1]*p[:,:,1])
    Σs[:,:,1] = M.Σᵥ - M.Σᵥ*(M.B'*KF[:Γ⁻¹][:,:,1]*M.B + KF[:Λ][:,:,1]'*C[:,:,1]*KF[:Λ][:,:,1])*M.Σᵥ
    for t in 1:n-1
        Xs[:,:,t+1] = M.A*Xs[:,:,t] + M.R*U[:,:,t]
        Σs[:,:,t+1] = M.A*Σs[:,:,t]*M.A' + M.R*Ξ[:,:,t]*M.R' -
                        M.A*KF[:Σp][:,:,t]*KF[:Λ][:,:,t]'*C[:,:,t]*RR -
                        RR*C[:,:,t]*KF[:Λ][:,:,t]*KF[:Σp][:,:,t]*M.A'
    end
    return Dict(:Xp => KF[:Xp],
                :Xf => KF[:Xf],
                :Xs => Xs,
                :Σp => KF[:Σp],
                :Σf => KF[:Σf],
                :Σs => Σs,
                :ϵ => KF[:ϵ],
                :Γ => KF[:Γ],
                :Γ⁻¹ => KF[:Γ⁻¹],
                :K => KF[:K],
                :H => KF[:H],
                :Λ => KF[:Λ],
                :p => p,
                :C => C,
                :U => U,
                :Ξ => Ξ,
                :ℓ => KF[:ℓ])
end

"""
    Backward Information Recursion

    Proposition 5.2.21 Cappe (2005)
"""
function BackwardInformationRecursion(M::LGSSM, y)
    n = size(y, 1)
    pdim = size(M.B, 2)
    qdim = size(y, 2)
    RR = M.R*M.R'
    SS = M.S*M.S'
    SS⁻¹ = inv(SS)

    κ = zeros(pdim, 1, n)
    κ̃ = zeros(pdim, 1, n)
    Π = zeros(pdim, pdim, n)
    Π̃ = zeros(pdim, pdim, n)

    Xs = zeros(pdim, 1, n)
    Σs = zeros(pdim, pdim, n)

    KF = KalmanFilter(M, y)

    for t in n-1:-1:1
        κ̃[:,:,t+1] = M.B'*SS⁻¹*y[t+1] + κ[:,:,t+1]
        Π̃[:,:,t+1] = M.B'*SS⁻¹*M.B + Π[:,:,t+1]
        V = inv(I + Π̃[:,:,t+1]*RR)
        κ[:,:,t] = M.A'V*κ̃[:,:,t+1]
        Π[:,:,t] = M.A'V*Π̃[:,:,t+1]*M.A
    end
    Dict(:Xp => KF[:Xp],
                :Xf => KF[:Xf],
                :Σp => KF[:Σp],
                :Σf => KF[:Σf],
                :κ => κ,
                :Π => Π)
end

"""
    Forward-backward smoother

    Algorithm 5.2.22 Cappe (2005)
"""
function ForwardBackwardSmoother(M::LGSSM, y)
    n = size(y, 1)
    pdim = size(M.B, 2)
    qdim = size(y, 2)
    RR = M.R*M.R'
    SS = M.S*M.S'
    SS⁻¹ = inv(SS)

    κ = zeros(pdim, 1, n)
    κ̃ = zeros(pdim, 1, n)
    Π = zeros(pdim, pdim, n)
    Π̃ = zeros(pdim, pdim, n)

    Xs = zeros(pdim, 1, n)
    Σs = zeros(pdim, pdim, n)

    KF = KalmanFilter(M, y)

    W = inv(I + Π[:,:,n]*KF[:Σf][:,:,n])
    Xs[:,:,n] = KF[:Xf][:,:,n] + KF[:Σf][:,:,n]*W*(κ[:,:,n] - Π[:,:,n]*KF[:Xf][:,:,n])
    Σs[:,:,n] = KF[:Σf][:,:,n] - KF[:Σf][:,:,n]*W*Π[:,:,n]*KF[:Σf][:,:,n]

    for t in n-1:-1:1
        κ̃[:,:,t+1] = M.B'*SS⁻¹*y[t+1] + κ[:,:,t+1]
        Π̃[:,:,t+1] = M.B'*SS⁻¹*M.B + Π[:,:,t+1]
        V = inv(I + Π̃[:,:,t+1]*RR)
        κ[:,:,t] = M.A'V*κ̃[:,:,t+1]
        Π[:,:,t] = M.A'V*Π̃[:,:,t+1]*M.A
        W = inv(I + Π[:,:,t]*KF[:Σf][:,:,t])
        Xs[:,:,t] = KF[:Xf][:,:,t] + KF[:Σf][:,:,t]*W*(κ[:,:,t] - Π[:,:,t]*KF[:Xf][:,:,t])
        Σs[:,:,t] = KF[:Σf][:,:,t] - KF[:Σf][:,:,t]*W*Π[:,:,t]*KF[:Σf][:,:,t]
    end
    Dict(:Xp => KF[:Xp],
                :Xf => KF[:Xf],
                :Xs => Xs,
                :Σp => KF[:Σp],
                :Σf => KF[:Σf],
                :Σs => Σs,
                :κ => κ,
                :Π => Π)
end

"""
    Backward Information Recursion

    Algorithm 6.1.2 Cappe (2005)
"""
function BackwardMarkovianStateSampling(M::LGSSM, y, N::Int)
    n = size(y, 1)
    KF = KalmanFilter(M, y)
    pdim = size(M.B, 2)
    qdim = size(y, 2)

    X = zeros(pdim, n, N)
    X[:,1,:] = rand(MvNormal(reshape(KF[:Xf][:,:,1],1), KF[:Σf][:,:,1]), N)
    for t in 2:n
        X[:,t,:] = rand(MvNormal(reshape(KF[:Xf][:,:,t],1), KF[:Σf][:,:,t]), N)
    end
    return X
end

"""
    Sampling with Dual Smoothing

    Algorithm 6.1.3 Cappe (2005)
"""
function SamplingWithDualSmoothing(M::LGSSM, y)
    n = size(y, 1)
    pdim = size(M.B, 2)
    qdim = size(y, 2)

    XY′ = Generate(M, n)
    KF = DisturbanceSmoother(M, y)
    KF′ = DisturbanceSmoother(M, XY′[:Y])
    return KF[:Xf] + XY′[:X] - KF′[:Xf]
end


"""
    Exo2Real{T <: Real} = Parabolic{Exo1Real{T}}

A 2-exo-real number is a parabolic Cayley-Dickson construct with 1-exo-real numbers.
"""
const Exo2Real{T <: Real} = Parabolic{Exo1Real{T}}

function Exo2Real(z::Exo1Real{T}) where T <: Real
    Exo2Real{T}(z, zero(Exo1Real{T}))
end

function Exo2Real(a::T, b::T, c::T, d::T) where T <: Real
    Exo2Real{T}(Exo1Real{T}(a, b), Exo1Real{T}(c, d))
end

function Exo2Real(a::T, b::T, c::T) where T <: Real
    Exo2Real{T}(Exo1Real{T}(a, b), Exo1Real{T}(c, zero(T)))
end

function Exo2Real(a::T, b::T) where T <: Real
    Exo2Real{T}(Exo1Real{T}(a, b), zero(Exo1Real{T}))
end

function Exo2Real(a::T) where T <: Real
    Exo2Real{T}(Exo1Real{T}(a, zero(T)), zero(Exo1Real{T}))
end

function Exo2Real(a::Real, b::Real)
    Exo2Real(promote(a, b)...)
end

function Exo2Real(a::Real, b::Real, c::Real)
    Exo2Real(promote(a, b, c)...)
end

function Exo2Real(a::Real, b::Real, c::Real, d::Real)
    Exo2Real(promote(a, b, c, d)...)
end

function selfstar(z::Exo2Real)
    (z + star(z) + dagger(z) + star(dagger(z))) / 4
end

function anti_selfstar(z::Exo2Real)
    (z - star(z) + dagger(z) - star(dagger(z))) / 4
end

"""
    unreal(z::Exo2Real)

The unreal part of a 2-exo-real number is a 3-vector.
"""
function unreal(z::Exo2Real)
    vcat(unreal(z.l), array(z.r))
end

function show(io::IO, z::Exo2Real)
    print(io, "[1: ")
    print(io, z.l.l)
    print(io, ", a: ")
    print(io, z.l.r)
    print(io, ", b: ")
    print(io, z.r.l)
    print(io, ", ab: ")
    print(io, z.r.r)
    print(io, "]")
end

function random(::Type{Exo2Real{T}}) where T <: Real
    Exo2Real{T}(random(Exo1Real{T}), random(Exo1Real{T}))
end


# Dual infeasible test problems
# Choose A, P and q in a way such that the problem becomes unbounded below
# here the last element of x appears in the cost function with a negative sign and is unconstrained

rng = Random.MersenneTwister(555)
nn = 1

@testset "Dual infeasible QP problems - Testset 1" begin
  for iii = 1:nn

  # choose size of problem
  n = rand(rng,5:50)
  m = 2*n
  A = sprandn(m,n,0.7)*50
  A[:,end] .= 0

  P = spzeros(n,n)

  q = randn(n,1)*50
  q[end] = -1
  q = vec(q)
  xtrue = randn(n,1)*50
  strue = rand(m,1)*50
  b = A*xtrue+strue
  b = vec(b)

  constraint = COSMO.Constraint(-A,b,COSMO.Nonnegatives)

  settings = COSMO.Settings(max_iter=10000,eps_abs = 1e-5,eps_rel=1e-5)
  model = COSMO.Model()
  assemble!(model,P,q,[constraint], settings = settings)
  res = COSMO.optimize!(model);

  @test res.status == :Dual_infeasible
  end
end


using Atum
using Atum.Euler

using Test
using StaticArrays: SVector
using LinearAlgebra: norm

function wave(law, x⃗, t)
  FT = eltype(law)
  u⃗ = SVector(FT(1), FT(1), FT(1))
  ρ = 2 + sin(π * (sum(x⃗) - sum(u⃗) * t))
  ρu⃗ = ρ * u⃗
  p = FT(1)
  ρe = Euler.energy(law, ρ, ρu⃗, p)
  SVector(ρ, ρu⃗..., ρe)
end

function run(A, FT, N, K)
  Nq = N + 1

  law = EulerLaw{FT, 3}()

  cell = LobattoCell{FT, A}(Nq, Nq, Nq)
  v1d = range(FT(-1), stop=FT(1), length=K+1)
  grid = brickgrid(cell, (v1d, v1d, v1d); periodic=(true, true, true))

  dg = DGSEM(; law, cell, grid, numericalflux = RusanovFlux())

  cfl = FT(1 // 4)
  dt = cfl * step(v1d) / N / Euler.soundspeed(law, FT(1), FT(1))
  timeend = FT(0.7)

  q = wave.(Ref(law), points(grid), FT(0))

  odesolver = LSRK54(dg, q, dt)
  timeend = dt
  solve!(q, timeend, odesolver)

  qexact = wave.(Ref(law), points(grid), timeend)
  errf = map(components(q), components(qexact)) do f, fexact
    sqrt(sum(dg.MJ .* (f .- fexact) .^ 2))
  end
  norm(errf)
end

let
  A = Array
  FT = Float64
  N = 4

  nlevels = 2
  err = zeros(FT, nlevels)

  for l in 1:nlevels
    K = 5 * 2 ^ (l - 1)
    errf = run(A, FT, N, K)
    err[l] = errf
  end
  rates = log2.(err[1:(nlevels-1)] ./ err[2:nlevels])
  @test rates[end] ≈ N + 1 atol = 0.15
end


using Test
using IntegerIB

#bach chorale data, taken from Nori Jacoby's webpage.
bach_histogram = [0.00001 392 14 582 896 262 106; 12 0.0001 15 5 674 17 182; 13 6 0.0001 44 12 32 3; 239 134 16 0.0001 380 13 173; 2099 25 46 93 0.00001 211 2; 96 206 25	95	172	0.00001 36; 499 1 4 9 24 20 0.00001]
pxy = bach_histogram./sum(bach_histogram)
model = IB(pxy, 0.95, "DIB")
brute_optimize!(model)
h, ixt, iyt, L = calc_metrics(model)
@test size(model.qt_x, 1) == 5
@test round(h, digits = 2) == 2.06
@test round(ixt, digits = 2) == 2.06
@test round(iyt, digits = 2) == 0.78
@test round(L, digits = 2) == 1.32
#toy time-series presenting one predictable pattern "b" -> "a"
toy_ts = ["a", "b", "a", "b", "a", "c", "d", "b", "a", "a", "d", "b", "a", "b", "a", "d", "c", "d", "b", "a", "d", "a", "c", "b", "a", "a", "b", "a", "c", "b", "a"]
model = IB(toy_ts, 500)
IB_optimize!(model)
@test (size(model.qt_x, 1) == 2 || size(model.qt_x, 1) == 3) #we expect the algorithm to cluster at least 2 labels together.


function sigmaij{T}(g::AbstractGraph{T}, κ::Int, ρ::Int)
    kpath = erwkpath(g, κ, ρ)
    proximity = zeros(num_edges(g))
    for e in edges(g)
        v, w = source(e, g), target(e, g)
        Nv = Set(out_neighbors(v, g))
        Nw = Set(out_neighbors(w, g))
        CN = intersect(Nv, Nw)
        NvCN = setdiff(Nv, CN)
        NwCN = setdiff(Nw, CN)
        LNv = length(Nv)
        LNw = length(Nw)
        LCN = length(CN)
        sum1 = 0.0
        sum2 = 0.0
        sum3 = 0.0
        out_v = out_edges(v, g)
        out_w = out_edges(w, g)
        for e1 in out_edges(v, g)
            i = edge_index(e1, g)
            targete1 = target(e1, g)
            if in(targete1, NvCN)
                sum1 += kpath[i]^2
            end
            for e2 in out_edges(w, g)
                j = edge_index(e2, g)
                targete2 = target(e2, g)
                if in(targete2, NwCN)
                    sum2 += kpath[j]^2
                elseif target(e1, g) == target(e2, g)
                    sum3 += (kpath[i] - kpath[j])^2
                end
            end
        end
        if LCN > 0
            if LNv > LCN
                if LNw > LCN
                    proximity[edge_index(e, g)] = sqrt(sum1/(LNv-LCN) + sum2/(LNw-LCN) + sum3/(LCN))
                else
                    proximity[edge_index(e, g)] = sqrt(sum1/(LNv-LCN) + sum3/(LCN))
                end
            else
                if LNw > LCN
                    proximity[edge_index(e, g)] = sqrt(sum2/(LNw-LCN) + sum3/(LCN))
                else
                    proximity[edge_index(e, g)] = sum3/(LCN)
                end
            end
        else
            if LNv > LCN
                if LNw > LCN
                    proximity[edge_index(e, g)] = sqrt(sum1/(LNv-LCN) + sum2/(LNw-LCN))
                else
                    proximity[edge_index(e, g)] = sqrt(sum1/(LNv-LCN))
                end
            else
                if LNw > LCN
                    proximity[edge_index(e, g)] = sqrt(sum2/(LNw-LCN))
                else
                    proximity[edge_index(e, g)] = 0.0
                end
            end
        end
    end
    proximity
end


# This table represents the optimal "power tree"
# based on Knuth's "TAOCP vol 2: Seminumerical Algorithms",
# third edition, section 4.6.3, Fig. 15.  It was
# transcribed into this array form in the gcc source
# code (tree-ssa-math-opts.c), but since this is just
# a table of mathematical facts it should not be copyrightable.
#
# To compute x^q in a minimal number of multiplications
# for 1 ≤ q ≤ 255, you compute x^r * x^s for
# r = power_tree[i] and s = i-q, recursively
# (memoizing each power computation), and we implicitly
# define power_tree[0] = 0.
const power_tree = [
      1,   1,   2,   2,   3,   3,   4,       #   1 -   7
      4,   6,   5,   6,   6,  10,   7,   9,  #   8 -  15
      8,  16,   9,  16,  10,  12,  11,  13,  #  16 -  23
     12,  17,  13,  18,  14,  24,  15,  26,  #  24 -  31
     16,  17,  17,  19,  18,  33,  19,  26,  #  32 -  39
     20,  25,  21,  40,  22,  27,  23,  44,  #  40 -  47
     24,  32,  25,  34,  26,  29,  27,  44,  #  48 -  55
     28,  31,  29,  34,  30,  60,  31,  36,  #  56 -  63
     32,  64,  33,  34,  34,  46,  35,  37,  #  64 -  71
     36,  65,  37,  50,  38,  48,  39,  69,  #  72 -  79
     40,  49,  41,  43,  42,  51,  43,  58,  #  80 -  87
     44,  64,  45,  47,  46,  59,  47,  76,  #  88 -  95
     48,  65,  49,  66,  50,  67,  51,  66,  #  96 - 103
     52,  70,  53,  74,  54, 104,  55,  74,  # 104 - 111
     56,  64,  57,  69,  58,  78,  59,  68,  # 112 - 119
     60,  61,  61,  80,  62,  75,  63,  68,  # 120 - 127
     64,  65,  65, 128,  66, 129,  67,  90,  # 128 - 135
     68,  73,  69, 131,  70,  94,  71,  88,  # 136 - 143
     72, 128,  73,  98,  74, 132,  75, 121,  # 144 - 151
     76, 102,  77, 124,  78, 132,  79, 106,  # 152 - 159
     80,  97,  81, 160,  82,  99,  83, 134,  # 160 - 167
     84,  86,  85,  95,  86, 160,  87, 100,  # 168 - 175
     88, 113,  89,  98,  90, 107,  91, 122,  # 176 - 183
     92, 111,  93, 102,  94, 126,  95, 150,  # 184 - 191
     96, 128,  97, 130,  98, 133,  99, 195,  # 192 - 199
    100, 128, 101, 123, 102, 164, 103, 138,  # 200 - 207
    104, 145, 105, 146, 106, 109, 107, 149,  # 208 - 215
    108, 200, 109, 146, 110, 170, 111, 157,  # 216 - 223
    112, 128, 113, 130, 114, 182, 115, 132,  # 224 - 231
    116, 200, 117, 132, 118, 158, 119, 206,  # 232 - 239
    120, 240, 121, 162, 122, 147, 123, 152,  # 240 - 247
    124, 166, 125, 214, 126, 138, 127, 153,  # 248 - 255
]

# return the inlined/unrolled expression to compute x^p
# by repeated multiplications (using an optimal addition
# chain for |p|<256 and power-by-squaring thereafter.
function addition_chain_pow(x::Symbol, p::Int)
    p == 0 && return :(one($x))
    ex = Union{Expr,Symbol}[] # expressions to compute intermediate powers
    if p < 0
        x′ = gensym()
        push!(ex, :($x′ = inv($x)))
        x = x′
        p = -p
    end
    pows_computed = Dict(1 => x) # powers q => variable storing x^q
    pows_to_compute = [p]
    while !isempty(pows_to_compute)
        q = pop!(pows_to_compute)
        if !haskey(pows_computed, q)
            if q ≤ length(power_tree) # use optimal addition chain
                q1 = power_tree[q]
                q2 = q-q1
                @assert q1 > 0 && q2 > 0
                has1 = haskey(pows_computed, q1)
                has2 = haskey(pows_computed, q2)
                if has1 && has2
                    xq = gensym()
                    push!(ex, :($xq = $(pows_computed[q1]) * $(pows_computed[q2])))
                    pows_computed[q] = xq
                else
                    push!(pows_to_compute, q) # try again after computing q1, q2
                    has1 || push!(pows_to_compute, q1)
                    has2 || push!(pows_to_compute, q2)
                end
            else # q too big, use power-by-squaring algorithm
                q1 = q >> 1
                if haskey(pows_computed, q1)
                    xq = gensym()
                    xq1 = pows_computed[q1]
                    push!(ex, iseven(q) ? :($xq = $xq1 * $xq1) : :($xq = $xq1 * $xq1 * $x))
                    pows_computed[q] = xq
                else
                    push!(pows_to_compute, q) # try again after computing q1
                    push!(pows_to_compute, q1)
                end
            end
        end
    end
    push!(ex, pows_computed[p]) # the variable for the final result x^p
    return Expr(:block, ex...)
end
addition_chain_pow(x::Symbol, p::Integer) = addition_chain_pow(x, Int(p))

function addition_chain_pow(expr, p::Integer)
    x = gensym()
    return Expr(:let, Expr(:(=), x, expr), Expr(:block, addition_chain_pow(x, p)))
end


using StaticArrays

const inv_2 = 1.0 / 2.0
const inv_3 = 1.0 / 3.0
const inv_4 = 1.0 / 4.0
const inv_6 = 1.0 / 6.0
const inv_8 = 1.0 / 8.0
const inv_12 = 1.0 / 12.0
const inv_18 = 1.0 / 18.0
const inv_20 = 1.0 / 20.0
const inv_24 = 1.0 / 24.0
const inv_30 = 1.0 / 30.0
const inv_36 = 1.0 / 36.0
const inv_48 = 1.0 / 48.0
const inv_72 = 1.0 / 72.0
const inv_120 = 1.0 / 120.0
const inv_144 = 1.0 / 144.0
const inv_720 = 1.0 / 720.0

"""
    SplinePP( degree, ncells)

- `degree` : degree of 1d spline
- `poly_coeffs` : `poly_coeffs[i,j]` coefficient of ``x^{deg+1-j}`` for ith B-spline function  size= (degree+1, degree+1)
- `poly_coeffs_fp` : `poly_coeffs[i,j]` coefficient of ``x^{deg+1-j}`` for ith B-spline function  size= (degree+1, degree+1)
- `ncells` : number of gridcells
- `scratch_b` : scratch data for `b_to_pp-converting`
- `scratch_p` : scratch data for `b_to_pp-converting`
"""
struct SplinePP{N}
    degree::Int
    poly_coeffs::SMatrix{N,N,Float64}
    poly_coeffs_fp::SMatrix{N,N,Float64}
    ncells::Int

    function SplinePP(degree, ncells)
        @assert (ncells >= degree)

        if degree == 1
            poly_coeffs = SMatrix{2,2}(-1.0, 1.0, 1.0, 0.0)
            poly_coeffs_fp = SMatrix{2,2}(-inv_2, 1.0, inv_2, 0.0)

        elseif degree == 2
            poly_coeffs = SMatrix{3,3}(
                inv_2, -1.0, inv_2, -1.0, 1.0, inv_2, inv_2, 0.0, 0.0
            )
            poly_coeffs_fp = SMatrix{3,3}(
                inv_6, -inv_2, inv_2, -inv_3, inv_2, inv_2, inv_6, 0.0, 0
            )

        elseif degree == 3
            poly_coeffs = SMatrix{4,4}(
                -inv_6,
                inv_2,
                -inv_2,
                inv_6,
                inv_2,
                -1.0,
                0.0,
                4 * inv_6,
                -inv_2,
                inv_2,
                inv_2,
                inv_6,
                inv_6,
                0.0,
                0.0,
                0.0,
            )

            poly_coeffs_fp = SMatrix{4,4}(
                -inv_24,
                inv_6,
                -inv_4,
                inv_6,
                inv_8,
                -inv_3,
                0.0,
                4 * inv_6,
                -inv_8,
                inv_6,
                inv_4,
                inv_6,
                inv_24,
                0.0,
                0.0,
                0.0,
            )

        elseif degree == 4
            poly_coeffs = SMatrix{5,5}(
                inv_24,
                -inv_6,
                inv_4,
                -inv_6,
                inv_24,
                -inv_6,
                inv_2,
                -inv_4,
                -inv_2,
                11 * inv_24,
                inv_4,
                -inv_2,
                -inv_4,
                inv_2,
                11 * inv_24,
                -inv_6,
                inv_6,
                inv_4,
                inv_6,
                inv_24,
                inv_24,
                0.0,
                0.0,
                0.0,
                0.0,
            )

            poly_coeffs_fp = SMatrix{5,5}(
                inv_120,
                -inv_24,
                inv_12,
                -inv_12,
                inv_24,
                -inv_30,
                inv_8,
                -inv_12,
                -inv_2,
                11 * inv_24,
                inv_20,
                -inv_8,
                -inv_12,
                inv_4,
                11 * inv_24,
                -inv_30,
                inv_24,
                inv_12,
                inv_12,
                inv_24,
                inv_120,
                0.0,
                0.0,
                0.0,
                0.0,
            )

        elseif degree == 5
            poly_coeffs = SMatrix{6,6}(
                -inv_120,
                inv_24,
                -inv_12,
                inv_12,
                -inv_24,
                inv_120,
                inv_24,
                -inv_6,
                inv_6,
                inv_6,
                -5 * inv_12,
                26 * inv_120,
                -inv_12,
                inv_4,
                0.0,
                -inv_2,
                0.0,
                11 * inv_20,
                inv_12,
                -inv_6,
                -inv_6,
                inv_6,
                5 * inv_12,
                26 * inv_120,
                -inv_24,
                inv_24,
                inv_12,
                inv_12,
                inv_24,
                inv_120,
                inv_120,
                0.0,
                0.0,
                0.0,
                0.0,
                0.0,
            )

            poly_coeffs_fp = SMatrix{6,6}(
                -inv_720,
                inv_120,
                -inv_48,
                inv_36,
                -inv_48,
                inv_120,
                inv_144,
                -inv_30,
                inv_24,
                inv_18,
                -5 * inv_24,
                26 * inv_120,
                -inv_72,
                inv_20,
                0.0,
                -inv_6,
                0.0,
                11 * inv_20,
                inv_72,
                -inv_30,
                -inv_24,
                inv_18,
                5 * inv_24,
                26 * inv_120,
                -inv_144,
                inv_120,
                inv_48,
                inv_36,
                inv_48,
                inv_120,
                inv_720,
                0.0,
                0.0,
                0.0,
                0.0,
                0.0,
            )
        else
            throw(ArgumentError(" degree $degree not implemented"))
        end

        N = degree + 1

        return new{N}(degree, poly_coeffs, poly_coeffs_fp, ncells)
    end
end

"""
    b_to_pp( SplinePP, ncells, b_coeffs)

Convert 1d spline in B form to spline in pp form with
periodic boundary conditions
"""
function b_to_pp(self::SplinePP, ncells::Int, b_coeffs::Vector{Float64})
    dp1 = self.degree + 1
    pp_coeffs = zeros(Float64, (dp1, ncells))
    coeffs = zeros(Float64, dp1)

    @inbounds for i in 1:(self.degree)
        coeffs .= vcat(b_coeffs[(end - self.degree + i):end], b_coeffs[1:i])
        for j in 1:dp1
            pp_coeffs[j, i] = sum(coeffs .* self.poly_coeffs[j, :])
        end
    end

    @inbounds for i in (self.degree + 1):ncells
        coeffs .= b_coeffs[(i - self.degree):i]
        for j in 1:dp1
            pp_coeffs[j, i] = sum(coeffs .* self.poly_coeffs[j, :])
        end
    end

    return pp_coeffs
end

"""
    horner_1d(degree, pp_coeffs, x, index)

Perform a 1d Horner schema on the `pp_coeffs` at index
"""
function horner_1d(degree::Int, pp_coeffs, x::Float64, index::Int)
    res = pp_coeffs[1, index]
    @inbounds for i in 1:degree
        res = res * x + pp_coeffs[i + 1, index]
    end
    return res
end

"""
    horner_primitive_1d(val, degree, pp_coeffs, x)

Perform a 1d Horner schema on the `pp_coeffs` evaluate at x
"""
function horner_primitive_1d(val::Vector{Float64}, degree, pp_coeffs, x)
    @inbounds for i in eachindex(val)
        val[i] = horner_1d(degree, pp_coeffs, x, i) * x
    end
end

"""
    horner_m_2d!(val, spl1, spl2, degree, x)

Perform two times a 1d hornerschema on the poly_coeffs
- val : array of values
- degree : degree of the spline
- x : point at which we evaluate our spline
"""
function horner_m_2d!(val, spl1::SplinePP, spl2::SplinePP, degree, x, y)
    for i in 1:(degree + 1)
        val[i, 1] = horner_1d(degree, spl1.poly_coeffs, x, i)
        val[i, 2] = horner_1d(degree, spl2.poly_coeffs, y, i)
    end
end

"""
    b_to_pp_1d_cell( self, b_coeffs, pp_coeffs )

Convert 1d spline in B form in a cell to spline in pp form with periodic boundary conditions
- spline : arbitrary degree 1d spline
- b_coeffs(self%degree+1) : coefficients of spline in B-form
- pp_coeffs(self%degree+1) : coefficients of spline in pp-form
"""
function b_to_pp_1d_cell!(pp, spline, b)
    dp1 = spline.degree + 1
    fill!(pp, 0.0)
    for i in 1:dp1, j in 1:dp1
        pp[j] += b[i] * spline.poly_coeffs[j, i]
    end
end

"""
    b_to_pp_2d_cell(spline1, spline2, b_coeffs, pp_coeffs, i, j)

Convert 2d spline in B form in a cell to spline in pp form with periodic boundary conditions

- spline1 : arbitrary degree 1d spline
- spline2 : arbitrary degree 1d spline
- n_cells(2) : number of gridcells
- b_coeffs(n_cells(1)*n_cells(2)) : coefficients of spline in B-form
- pp_coeffs((spline1.degree+1)*(spline2.degree+1),n_cells(1)*n_cells(2)) : coefficients of spline in pp-form
"""
function b_to_pp_2d_cell!(pp, spline1::SplinePP, spline2::SplinePP, b, i, j)
    n1 = spline1.ncells
    n2 = spline2.ncells
    d1 = spline1.degree
    d2 = spline2.degree
    dp1 = d1 + 1
    dp2 = d2 + 1

    b1 = zeros(dp1)
    b2 = zeros(dp2)
    p2 = zeros(dp2)
    # convert b-coefficients in pp-coefficients in first dimension
    if i > d1
        if j > d2
            for l in 0:d2
                k = i + (j - dp2 + l) * n1
                b1 .= b[(k - d1):k]
                b_to_pp_1d_cell!(
                    view(pp, (1 + l * dp1):(dp1 * (l + 1)), i + n1 * (j - 1)), spline1, b1
                )
            end
        else
            # use of modulo for boundary cells in second dimension
            for l in 0:d2
                k = i + mod(j - dp2 + l, n2) * n1
                b1 .= b[(k - d1):k]
                b_to_pp_1d_cell!(
                    view(pp, (1 + l * dp1):(dp1 * (l + 1)), i + n1 * (j - 1)), spline1, b1
                )
            end
        end
    else
        # use of modulo for boundary cells in both dimensions
        for l in 0:d2
            for k in 0:d1
                b1[k + 1] = b[mod(i - dp1 + k, n1) + 1 + mod(j - dp2 + l, n2) * n1]
            end
            b_to_pp_1d_cell!(
                view(pp, (1 + l * dp1):(dp1 * (l + 1)), i + n1 * (j - 1)), spline1, b1
            )
        end
    end

    # convert b-coefficients in pp_coefficients in second dimension
    for k in 1:dp1
        for l in 1:dp2
            b2[l] = pp[k + dp1 * (l - 1), i + n1 * (j - 1)]
        end
        b_to_pp_1d_cell!(p2, spline2, b2)
        for l in 1:dp2
            pp[k + dp1 * (l - 1), i + n1 * (j - 1)] = p2[l]
        end
    end
end

"""
    b_to_pp_2d!( pp, spl1, spl2, b)

Convert 2d spline in B form to spline in pp form
- n_cells(2) : number of gridcells
- b_coeffs   : coefficients of spline in B-form
- pp_coeffs  : coefficients of spline in pp-form
"""
function b_to_pp_2d!(pp, spl1::SplinePP, spl2::SplinePP, b)
    ncells1 = spl1.ncells
    ncells2 = spl2.ncells

    for j in 1:ncells2, i in 1:ncells1
        b_to_pp_2d_cell!(pp, spl1, spl2, b, i, j)
    end
end

"""
    horner_2d(degrees, pp_coeffs, position, indices, ncells)

Perform a 2d hornerschema on the pp_coeffs at the indices
- degree : degree of the spline
- pp_coeffs : coefficients of spline in pp-form
- position(2) : point at which we evaluate our spline
- indices(2) : indices of cell in which is x
- ncells(2) : number of gridcells
- res : value of the splinefunction at position
"""
function horner_2d(degrees, pp, position, indices, ncells)
    x1, x2 = position
    d1, d2 = degrees
    i1, i2 = indices
    n1, n2 = ncells

    pp_1d = zeros(d2 + 1, 1)

    for i in 0:d2
        istart = 1 + i * (d1 + 1)
        istop = (d1 + 1) * (i + 1)
        jstart = 1 + (i2 - 1) * n1
        jstop = n1 * i2
        pp_1d[i + 1, 1] = horner_1d(d1, view(pp, istart:istop, jstart:jstop), x1, i1)
    end

    res = pp_1d[1, 1]
    for i in 1:d2
        res = res * x2 + pp_1d[i + 1, 1]
    end

    return res
end


using NetCDF, LinearAlgebra
using Statistics, StatsBase
using PyCall
xr = pyimport("xarray")
#pygui(:qt)
using PyPlot
using MultivariateStats
# eofs = pyimport("eofs")
using BFloat16s
using Sonums

path = "/Users/milan/Downloads/cams/ieee"
filelist = filter(x->endswith(x,"no2.grib"),readdir(path))
gribfile = xr.open_dataset(joinpath(path,filelist[1]),engine="cfgrib")
no2 = gribfile.no2.values
level = 1:size(no2)[1]

# train sonums
trainSonum16(no2)

no2lin24 = Array{Float32}(LinQuant24Array(no2))
Q = LogQuant16Array(no2);
no2log16 = Array{Float32}(LogQuant16Array(no2))
no2bf16 = Float32.(BFloat16.(no2'))
no2s16 = Float32.(Sonum16.(no2))

## error 24bit
de = abs.(log10.(no2 ./ no2lin24))

de24m = mean(de,dims=2)[:,1]
de24mi = median(de,dims=2)[:,1]
de24min = minimum(de,dims=2)[:,1]
de24max = maximum(de,dims=2)[:,1]
de24p10 = [percentile(vec(de[i,:]),10) for i in level]
de24p90 = [percentile(vec(de[i,:]),90) for i in level]

# and with 16
de = abs.(log10.(no2 ./ no2log16))

de16m = mean(de,dims=2)[:,1]
de16mi = median(de,dims=2)[:,1]
de16min = minimum(de,dims=2)[:,1]
de16max = maximum(de,dims=2)[:,1]
de16p10 = [percentile(vec(de[i,:]),10) for i in level]
de16p90 = [percentile(vec(de[i,:]),90) for i in level]

# and for bfoat16 = bitgrooming
de = abs.(log10.(no2 ./ no2bf16))

debf16m = mean(de,dims=2)[:,1]
debf16mi = median(de,dims=2)[:,1]
debf16min = minimum(de,dims=2)[:,1]
debf16max = maximum(de,dims=2)[:,1]
debf16p10 = [percentile(vec(de[i,:]),10) for i in level]
debf16p90 = [percentile(vec(de[i,:]),90) for i in level]

# and for bfoat16 = bitgrooming
de = abs.(log10.(no2 ./ no2s16))

des16m = mean(de,dims=2)[:,1]
des16mi = median(de,dims=2)[:,1]
des16min = minimum(de,dims=2)[:,1]
des16max = maximum(de,dims=2)[:,1]
des16p10 = [percentile(vec(de[i,:]),10) for i in level]
des16p90 = [percentile(vec(de[i,:]),90) for i in level]

## plot
fig,ax = subplots(1,1,figsize=(6,10))
ax.invert_yaxis()

ax.semilogx(de24m,level,"C0",lw=3,label="24-bit lin")
ax.semilogx(de16m,level,"C1",lw=3,label="16-bit log")
ax.semilogx(debf16m,level,"C2",lw=3,label="bfloat16")
ax.semilogx(des16m,level,"C3",lw=3,label="15-bit max-entropy")

ax.semilogx(de24p90,level,"C0",lw=2)
ax.semilogx(de16p90,level,"C1",lw=2)
ax.semilogx(debf16p90,level,"C2",lw=2)
ax.semilogx(des16p90,level,"C3",lw=2)

ax.semilogx(de24max,level,"C0",lw=1)
ax.semilogx(de16max,level,"C1",lw=1)
ax.semilogx(debf16max,level,"C2",lw=1)
ax.semilogx(des16max,level,"C3",lw=1)

ax.plot(0,0,"grey",lw=3,label="mean")
ax.plot(0,0,"grey",lw=2,label="90%")
ax.plot(0,0,"grey",lw=1,label="max")

ax.set_title(L"NO$_2$ compression error")
ax.set_ylabel("model level")
ax.set_xlabel("decimal error")
ax.set_ylim(137,1)
ax.legend(loc=3,ncol=2)

# a‚x.set_xlim(1e-7,1)
tight_layout()


type Spline
  d
  U #knots
  C #coefficients
  function Spline(d,U,C)
    p = sortperm(U)
    U,C = U[p], C[p]
    U = [ fill(U[1],d); U; fill(U[end],d) ]
    C = [ fill(C[1],d); C; fill(C[end],d) ]
    return new(d,U,C)
  end
end

test = Spline(1,linspace(0,1,10),rand(10))

function fastfind(A,x)
  i,j = 1,length(A)
  A[i] > x && return 0
  A[j] < x && return j
  while abs(i-j) > 1
    k = (i+j) ÷ 2
    if A[k] < x; i = k
    else j = k end
  end
  return i
end

function splinematrix(S::Spline,i,k,x)
  d,U = S.d,S.U
  M = zeros(k,k+1)
  for a in 1:k
    M[a,a] = (U[i+a] - x) / (U[i+a] - U[i+a-k])
    M[a,a+1] = (x - U[i+a-k]) / (U[i+a] - U[i+a-k])
  end
  return M
end

splinematrix(test,2,1,.1)

@noinline function call(S::Spline, x)
  core(S::Spline, x)
end

@noinline function core(S::Spline, x)
  d,U,C = S.d,S.U,S.C
  i = fastfind(U,x)
  # i = clamp(i,d+1,length(U)-d-1)
  i = max(i,d+1)
  println("x")
  c = C[i-d:i]
  for k in d:-1:1
    c = splinematrix(S,i,k,x) * c
  end
  return c[1]
end

using PyPlot
plot(linspace(0,1,length(test.C)), test.C)
plot(linspace(0,1,100), map(x->core(test, x), linspace(test.U[1],test.U[end],100)))

splinematrix(test, 10, 1, 1.0)

x = sort(rand(10))

fastfind(x,.0)


generic_sphere(ref_dirs) = ref_dirs ./ ([norm(v) for v in ref_dirs])

function DTLZ_hypersphere!(fx, x; α = 1)
    m = length(fx)
    for i = 1:m
        if i < m
            fx[i] *= prod(cos.((π*0.5) * x[1:m-i] .^ α ))
        end
        if i > 1
            fx[i] *= sin((π*0.5) * x[1+m - i] .^ α)
        end
    end

    return fx
end

function DTLZ_g1(x, m)
    y = view(x, m:length(x)) .- 0.5
    return 100*( length(y) + sum( y.^2 - cos.(20π*y) ))
end

function DTLZ_g2(x, m)
    return sum( (view(x, m:length(x)) .- 0.5).^2 )
end

"""
    DTLZ1(m = 3, ref_dirs = gen_ref_dirs(m, 12))

DTLZ1 returns `(f::function, bounds::Matrix{Float64}, pareto_set::Array{xFgh_indiv})`
where `f` is the objective function and `pareto_set` is an array with optimal Pareto solutions
with `n_solutions`.

### Parameters
- `m` number of objective functions
- `ref_dirs` number of Pareto solutions (default: Das and Dennis' method).

Main properties:
- convex
- multifrontal
"""
function DTLZ1(m=3, ref_dirs = gen_ref_dirs(m, 12))
    f(x,m=m) = begin
        g = DTLZ_g1(x, m)
        D = length(x)

        fx = fill(0.5*(1 + g), m)
        for i in 1:m
            fx[i] *= prod(x[1:m-i])
            if i > 1
                fx[i] *= 1 - x[1+m - i]
            end
        end

        return fx, [0.0], [0.0]
    end

    D = 10 + m - 1

    bounds = Array([zeros(D) ones(D)]')

    pf = 0.5ref_dirs
    pareto_set = [ generateChild(zeros(0), (fx, [0.0], [0.0])) for fx in pf ]

    return f, bounds, pareto_set
end

"""
    DTLZ2(m = 3, ref_dirs = gen_ref_dirs(m, 12))

DTLZ2 returns `(f::function, bounds::Matrix{Float64}, pareto_set::Array{xFgh_indiv})`
where `f` is the objective function and `pareto_set` is an array with optimal Pareto solutions
with `n_solutions`.

### Parameters
- `m` number of objective functions
- `ref_dirs` number of Pareto solutions (default: Das and Dennis' method).

Main properties:
- nonconvex
- unifrontal
"""
function DTLZ2(m=3, ref_dirs = gen_ref_dirs(m, 12))
    f(x,m=m) = begin
        g = DTLZ_g2(x, m)
        fx = fill(1.0 + g, m)
        DTLZ_hypersphere!(fx, x)

        return fx, [0.0], [0.0]
    end

    D = 10 + m - 1

    bounds = Array([zeros(D) ones(D)]')

    pf = generic_sphere(ref_dirs)
    pareto_set = [ generateChild(zeros(0), (fx, [0.0], [0.0])) for fx in pf ]

    return f, bounds, pareto_set
end

"""
    DTLZ3(m = 3, ref_dirs = gen_ref_dirs(m, 12))

DTLZ3 returns `(f::function, bounds::Matrix{Float64}, pareto_set::Array{xFgh_indiv})`
where `f` is the objective function and `pareto_set` is an array with optimal Pareto solutions
with `n_solutions`.

### Parameters
- `m` number of objective functions
- `ref_dirs` number of Pareto solutions (default: Das and Dennis' method).

Main properties:
- nonconvex
- multifrontal
"""
function DTLZ3(m = 3, ref_dirs = gen_ref_dirs(m, 12))
    f(x,m=m) = begin
        g = DTLZ_g1(x, m)
        fx = fill(1 + g, m)
        DTLZ_hypersphere!(fx, x)

        return fx, [0.0], [0.0]
    end

    D = 10 + m - 1

    bounds = Array([zeros(D) ones(D)]')

    pf = generic_sphere(ref_dirs)
    pareto_set = [ generateChild(zeros(0), (fx, [0.0], [0.0])) for fx in pf ]

    return f, bounds, pareto_set
end

"""
    DTLZ4(m = 3, ref_dirs = gen_ref_dirs(m, 12))

DTLZ4 returns `(f::function, bounds::Matrix{Float64}, pareto_set::Array{xFgh_indiv})`
where `f` is the objective function and `pareto_set` is an array with optimal Pareto solutions
with `n_solutions`.

### Parameters
- `m` number of objective functions
- `ref_dirs` number of Pareto solutions (default: Das and Dennis' method).

Main properties:
- nonconvex
- unifrontal
"""
function DTLZ4(m = 3, ref_dirs = gen_ref_dirs(m, 12))
    f(x,m=m) = begin
        g = DTLZ_g2(x, m)
        fx = fill(1.0 + g, m)
        DTLZ_hypersphere!(fx, x; α = 100)

        return fx, [0.0], [0.0]
    end

    D = 10 + m - 1

    bounds = Array([zeros(D) ones(D)]')

    pf = generic_sphere(ref_dirs)
    pareto_set = [ generateChild(zeros(0), (fx, [0.0], [0.0])) for fx in pf ]

    return f, bounds, pareto_set
end

"""
    DTLZ5(m = 3)

DTLZ5 returns `(f::function, bounds::Matrix{Float64}, pareto_set::Array{xFgh_indiv})`
where `f` is the objective function and `pareto_set` is an array with optimal Pareto solutions
with `n_solutions`.

### Parameters
- `m` number of objective functions

"""
function DTLZ5(m = 3)
    f(x,m=m) = begin
        g = DTLZ_g2(x, m)
        fθ = fill(1.0 + g, m)
        θ = @. 1 / (2*(1 + g)) * (1 + 2g * x[1:m-1] )
        θ[1] = x[1]

        DTLZ_hypersphere!(fθ, θ)

        return fθ, [0.0], [0.0]
    end

    D = 10 + m - 1

    bounds = Array([zeros(D) ones(D)]')

    if m == 3
        ref_dirs = gen_ref_dirs(2, 100)
        X = fill(0.5, length(ref_dirs), D)

        for i in eachindex(ref_dirs)
            X[i,1:2] = ref_dirs[i]
        end
        pareto_set = [ generateChild(X[i,:], f(X[i,:])) for i in eachindex(ref_dirs) ]

    else
        pareto_set = []
    end

    return f, bounds, pareto_set
end

"""
    DTLZ6(m = 3)

DTLZ6 returns `(f::function, bounds::Matrix{Float64}, pareto_set::Array{xFgh_indiv})`
where `f` is the objective function and `pareto_set` is an array with optimal Pareto solutions
with `n_solutions`.

### Parameters
- `m` number of objective functions

"""
function DTLZ6(m = 3)
    f(x,m=m) = begin
        g = sum(x[m:end] .^ 0.1)
        fθ = fill(1.0 + g, m)
        θ = @. 1 / (2*(1 + g)) * (1 + 2g * x[1:m-1] )
        θ[1] = x[1]

        DTLZ_hypersphere!(fθ, θ)

        return fθ, [0.0], [0.0]
    end

    D = 10

    bounds = Array([zeros(D) ones(D)]')

    if m == 3
        ref_dirs = gen_ref_dirs(2, 100)
        X = fill(0.0, length(ref_dirs), D)

        for i in eachindex(ref_dirs)
            X[i,1:2] = ref_dirs[i]
        end
        pareto_set = [ generateChild(X[i,:], f(X[i,:])) for i in eachindex(ref_dirs) ]

    else
        pareto_set = []
    end

    return f, bounds, pareto_set
end


y = Vector{Int}(df[:y])
X = Matrix(Matrix{Float64}(df[:,[:x1,:x2]])')

# initial vectors
k,n = size(X)
py = proportions(y)
γ0 = norminvcdf.(cumsum(py))[1:end-1]
β0 = zeros(k)
θ0 = [β0..., γ0...]

# tmp arrays
tmpη = Array{eltype(X)}(undef,n)
tmpgrad = Vector{Float64}(undef,length(θ0))

# --------- check that LL functions are the same ---------

for model in (:logit,:probit,)

    @show model

    @testset "testing cdf/pdf derivatives for $model" begin
        for z in -10.:10.
            fd = Calculus.gradient((x) -> OrderedResponse.cdf(x, Val{model}), z)
            ad = OrderedResponse.pdf(z, Val{model})

            fh = Calculus.hessian( (x) -> OrderedResponse.cdf(x, Val{model}), z)
            ah = OrderedResponse.dpdf(z, Val{model})

            @test isapprox(fd, ad, atol=1e-9, rtol=1e-8)
            @test isapprox(fh, ah, atol=3.79e-7, rtol=1e-5)
            # @test fd ≈ ad
            # @test fh ≈ ah
            @test abs(fd - ad) < 1e-5
            @test abs(fh - ah) < 1e-5
        end
    end

    # ------------- closures ---------------

    # function LL!(grad::Vector{T}, hess::Matrix{T}, tmpgrad::Vector, η::Vector, y::Vector{<:Integer}, X::Matrix, β::Vector, γ::Vector, model::Type{Val{D}}, sgn::Real=-one(T)) where {D,T}

    f(θ::Vector)                = OrderedResponse.orLL!(zeros(0), zeros(0,0), zeros(0), tmpη, y, X, θ[1:2], θ[3:4], Val{model}, -1.0)
    g!(grad::Vector, θ::Vector) = OrderedResponse.orLL!(grad    , zeros(0,0), tmpgrad , tmpη, y, X, θ[1:2], θ[3:4], Val{model}, -1.0)
    h!(hess::Matrix, θ::Vector) = OrderedResponse.orLL!(zeros(4),       hess, tmpgrad , tmpη, y, X, θ[1:2], θ[3:4], Val{model}, -1.0)

    function g(θ::Vector)
        grad = similar(θ)
        g!(grad, θ)
        return grad
    end

    function h(θ::Vector)
        grad = similar(θ)
        hess = Matrix{eltype(grad)}(undef,length(grad), length(grad))
        h!(hess, θ)
        return hess
    end

    # --------- derivative -free search ---------

    optf = Optim.optimize(f, θ0)
    @show optf
    @test_broken optf.minimizer ≈ θpolr(model)
    @test norm(optf.minimizer .- θpolr(model), Inf) < 3e-5

    # --------- opt with derivatives ---------

    @test Calculus.derivative(f, θ0) ≈ g(θ0)
    @test_broken Calculus.derivative(f, optf.minimizer) ≈ g(optf.minimizer)
    @test norm(Calculus.derivative(f, optf.minimizer) .- g(optf.minimizer), Inf) < 1e-5

    optg = Optim.optimize(f, g!, θ0)
    @show optg
    # @test_broken optf.minimizer ≈ optg.minimizer
    # @test_broken optg.minimizer ≈ θpolr(model)
    @test norm(optf.minimizer .- optg.minimizer, Inf) < 1e-4
    @test norm(optg.minimizer .- θpolr(model), Inf) < 1e-7
    @show optg.minimizer .- θpolr(model)

    # -------- newton-rhapson ------------

    maxhessdiff = maximum(abs.(Calculus.hessian(f, θ0) .- h(θ0)))
    println("\nMaximum FD for Hessian in $model model is $maxhessdiff\n")
    @test maxhessdiff < 0.2

    opth = Optim.optimize(f, g!, h!, θ0)
    vcov = h(opth.minimizer)\Matrix(I,4,4)

    @test norm(opth.minimizer .- θpolr(model), Inf) < 1e-7
    @test norm((vcov .- vcovpolr(model))[:], Inf) < 2.7e-4
    # @test_broken opth.minimizer ≈ θpolr(model)
    @test_broken vcov ≈ vcovpolr(model)

    @show opth.minimizer .- θpolr(model)
    @show sqrt.(diag(vcov)) .- sqrt.(diag(vcovstata(model)))

end


using SparseArrays
using LinearAlgebra
using JuMP

include("proof_helper.jl")
gurobi_env = Gurobi.Env()
k = 5
r = round(Int64,(k+1)/2)
alph = 0.57
standard = false

if standard
    n = 200
    n1 = round(Int64,floor(n*alph))
    n2 = n - n1
    b1, b2 = n1_n2_baselines(k,n1,n2)
else
    b1 = arbitrary_baselines(k,alph)
    b2 = arbitrary_baselines(k,1-alph)
end

X,eps = monotonic_lp_jump(b1,b2,k)

Dk, Db, R, B = Bmat(b1,b2)

Xs = Xstanard(k,n1,n2)
Xa = Xasym(k,alph)

# Try another way
B = monotonic_LP_B(b1,b2,k)
Xopt, Yopt, eps = PrimalDualXY(gurobi_env,B,k)

if standard
    type = "Standard"
else
    type = "Asymptotic"
end
println("")

if eps> 0
    println("\tMonotonic homophily is possible!")
else
    println("\n\tMonotonic homophily is impossible.")
end

## Check alternate dual variables
x = Xa
x = X
D1 = sum(i*x[i+1] for i = 1:k)
D2 = sum(i*x[k-i+1] for i = 1:k)
r = round(Int64,(k+1)/2)
for t = r:k
    @assert((t)*x[t+1]/b1[t] - (t-1)*x[t]/b1[t-1] >= 0 )
    @assert((t)*x[k-t+1]/b2[t] - (t-1)*x[k-t+2]/b2[t-1] >= 0)
end

## Check more complicated baselines (from other hypergraph)

include("../src/helper.jl")
data = matread("../data/Polblogs_H.mat")

H = data["H"]; classes = vec(round.(Int64,data["labels"]))

## Get scores
r = 13                    # maximum k size we care about
Hw, Hm = relative_affinities(H,classes,r)
N = get_hyperedge_counts(H,classes)
Nmat = N[1:r,1:r+1]
# Get baselines
b1c = Hw[k,1:k]
b2c = Hm[k,1:k]

## Find solution to the LP

k = 11
N = Nmat[k,1:k+1]
N = N.+1
r = round(Int64,(k+1)/2)

b1 = zeros(k)
b2 = zeros(k)
D1 = sum(i*N[i+1] for i = 1:k)
D2 = sum(i*N[k-i+1] for i = 1:k)
for i = 1:k
    b1[i] = i*N[i+1]/D1
    b2[i] = i*N[k-i+1]/D2
end

B = monotonic_LP_B(b1,b2,k)
Xopt, Yopt, eps = PrimalDualXY(gurobi_env,B,k)

## Check dual variables

S = [r/b2[r] -(r-1)/b1[r-1];
    -(r-1)/b2[r-1] r/b1[r]]

S*[D1;D2]


module fTst

export f

function f(x::AbstractArray)::Real
    f = 2.0*x[1]^2 + 3.0*x[2]
    return f
end

end


#Create a true low-rank model
Xtrue = randn(100,10)
Ytrue = randn(10, 60)
A = Xtrue*Ytrue + 0.01*randn(100,60)
obs = [(i,j) for i in 1:100 for j in 1:60 if !(j in [3,59])]

#Test that the results from LowRankModels are approximately equal to GraphGLRM
glrm_quad = GLRM(A, QuadLoss(), QuadReg(1.), QuadReg(1.), 5)
gg = GGLRM(A, QuadLoss(), QuadReg(1.), QuadReg(1.), 5)
fit!(gg); fit!(glrm_quad)
ggobj = whole_objective(gg, gg.X'*gg.Y)
glrmobj = whole_objective(gg, glrm_quad.X'*glrm_quad.Y)
@test_approx_eq_eps(ggobj, glrmobj, 0.1*length(A))

#Try for a different loss function and regularizers
glrm_hquad = GLRM(A, HuberLoss(), OneReg(1.), OneReg(1.), 5)
ggh = GGLRM(A, HuberLoss(), OneReg(1.), OneReg(1.), 5)
fit!(ggh); fit!(glrm_hquad)
gghobj = whole_objective(ggh, ggh.X'*ggh.Y)
glrmhobj = whole_objective(ggh, glrm_hquad.X'*glrm_hquad.Y)
@test_approx_eq_eps(gghobj, glrmhobj, 0.1*length(A))

#Make sure that it works with missing data
glrm_missing = GLRM(A, L1Loss(), NonNegConstraint(), NonNegConstraint(), 5, obs=obs)
gg_missing = GGLRM(A, L1Loss(), NonNegConstraint(), NonNegConstraint(), 5, obs=obs)
fit!(gg_missing); fit!(glrm_missing);
ggmobj = whole_objective(gg_missing, gg_missing.X'gg_missing.Y)
glrmmobj = whole_objective(gg_missing, glrm_missing.X'glrm_missing.Y)
@test_approx_eq_eps(ggmobj, glrmmobj, 0.1*length(A))


### A Pluto.jl notebook ###
# v0.14.2

using Markdown
using InteractiveUtils

# This Pluto notebook uses @bind for interactivity. When running this notebook outside of Pluto, the following 'mock version' of @bind gives bound variables a default value (instead of an error).
macro bind(def, element)
    quote
        local el = $(esc(element))
        global $(esc(def)) = Core.applicable(Base.get, el) ? Base.get(el) : missing
        el
    end
end

# ╔═╡ da1d65a0-ec42-11ea-0141-334c9eeeb035
begin
	using Pkg
	Pkg.activate(mktempdir())
	Pkg.add(["PlutoUI", "Images", "ImageMagick"])
	using PlutoUI
	using Images
end

# ╔═╡ 5ef51c3a-70a7-11eb-2023-31113399a57f
html"""

Section 1.2

 Intro to Abstractions

Lecture Video

"""

# ╔═╡ 60ae819a-70a7-11eb-31d4-750c7f5dc6ca
PlutoUI.TableOfContents(aside=true)

# ╔═╡ 792c6a62-ec41-11ea-01f3-73e7eee23cc7
md"""
#### Intializing packages

_When running this notebook for the first time, this could take up to 15 minutes. Hang in there!_
"""

# ╔═╡ ef1bfa16-70ea-11eb-189c-a54db292cd6f
md"
## Introduction

The goal of this section is to introduce you to the notion of abstraction. You can think of abstraction as an opposite to specialization. We will illustrate this by looking at the following example.

### What is _one_?

Before we get lost talking about the foundations of number theory, I will present you with a few examples that represent one to me.
"

# ╔═╡ 6fcac482-70ee-11eb-0b80-ff41c708053b
md"Each of the items in this list is a specific, or **_specialized_** representation of _one_:
1. as an integer
1. as a float
1. as a string
1. as a rational number
1. as a cute picture
1. as an 2x2 identity matrix
1. as a singular dog

Of course, these are just a few examples of _one_. People have been representing _one_ for ages in different langauges, scripts, artistic expression, etc.

The difference between these ones to me is clear. In fact, I just articulated it to you. Now, let's turn to how a computer sees _one_ differently based on what I type.
"

# ╔═╡ 9ebc079a-70f0-11eb-07d9-f9e80f3f4584
md"So to a computer, all of these are different types."

# ╔═╡ 15f7f90a-70f0-11eb-0d41-63677e4023f4
md"### What is a collection of _one_s?

Now, I want to make a collection of ones for some reason. Below is a way for you to experiment building this collection with different _one_s. As you do this experiment, I want you to look at what stays in the same in the Julia output, and what doesn't."

# ╔═╡ f6886d90-70ed-11eb-07c4-471ee267e7c1
md"""
Before we even look at the output, I am amazed that this code even ran. Are you telling me that the computer doesn't care which _one_ I am using in my array?

Yes! That's exactly what abstraction is. By stepping back, we can now think and operate at a level that doesn't care about which _specific_ one I am using. This is what we mean by **abstraction is the opposite of specialization**.

The information that Julia gives back is quite informative. Here is an example of the first line of the output for a few different types:

```
array = 3x4 Array{Int64, 2}
array = 3x4 Array{Float64, 2}
array = 3x4 Array{Rational{Int64}, 2}
```

Notice that for all of these, we have the same `3x4 Array{***, 2}`.
"""

# ╔═╡ 3c1a3cf8-70f8-11eb-3c18-375207f321eb
md"""
## First Taste of Abstraction

Now, I want to do something to a collection of ones, that doesn't care about which one I'm using. So I'm going to write a function that takes in my collection, and add a corgi whereever I desire.
"""

# ╔═╡ 19f4ddb0-ec44-11ea-20b9-5d97fb2b1cf4
function insert(new, A, i, j)
	B = copy(A)
	B[i,j] = new
	return B
end

# ╔═╡ 424f5f10-ec44-11ea-076d-f3cba4435e0c
begin
	A = fill(0, 3, 4)
	md"""
	$(@bind i Slider(1: size(A,1), show_value=true))
	$(@bind j Slider(1: size(A,2), show_value=true))
	"""
end

# ╔═╡ 71ac08ea-7145-11eb-237d-5506adfb9533
begin
	one_number_array = fill(1,3,4)
	insert(5, one_number_array, i, j)
end

# ╔═╡ ee43d808-70fa-11eb-0cc6-337279f41494
md"This is still amazing. I wrote one function that just cares about how to insert an object into an array, without knowing anything about what's inside, and it worked for two completely different arrays, _collections of ones of **any kind**_."

# ╔═╡ 263a8a0a-70ee-11eb-236d-c941ba63dff3
md"
## Conclusion
The key idea here is that a computer language should allow you to do operations that make sense. Often times, an operation can make sense for many different objects. So we can abstract away the specifics of the object in our implementation. It should let you step back from there.
"

# ╔═╡ 52461588-ea1a-4e7d-aec2-3de388d31656
md"""
## Appendix
"""

# ╔═╡ 1a2a9000-ec43-11ea-3f39-8312ea286a92
begin
	oneimage = load(download("https://gallery.yopriceville.com/var/albums/Free-Clipart-Pictures/Decorative-Numbers/Cute_Number_One_PNG_Clipart_Image.png?m=1437447301"))
	corgi = load(download("https://i.barkpost.com/wp-content/uploads/2015/01/corgi2.jpg?q=70&fit=crop&crop=entropy&w=808&h=500"))
	nothing
end

# ╔═╡ 0504ac94-70ee-11eb-1c4e-977d9e7d35c9
one = [
	1,
	1.0,
	"one",
	1//1,
	oneimage,
	[1 0; 0 1],
	corgi,
]

# ╔═╡ 0b1668ba-ec42-11ea-3e50-ed97c5b17ced
computer_ones = typeof.(one)

# ╔═╡ b2239b96-70ef-11eb-0b85-21ecab25dc9f
begin
	one_keys = ["1", "1.0", "one", "1//1", "Cute One", "2x2 Identity", "One Corgi"]
	selections = one_keys .=> one_keys
	lookup_element = Dict(one_keys .=> one)
	md"$(@bind element_key Select(selections))"
end

# ╔═╡ 4251f668-70aa-11eb-3d89-35f8d53b7d9b
# your chosen one
element = lookup_element[element_key]

# ╔═╡ f1568d10-ec41-11ea-3dd2-a9cb273ce5b8
#its type
typeof(element)

# ╔═╡ ab02d850-ec41-11ea-10b2-a1b600b12658
# a 3x4 array of this one.
array = fill(element,3,4)

# ╔═╡ 5363a400-ec44-11ea-284e-d13a8872551c
begin
	one_image_array = fill(oneimage,3,4)
	insert(corgi, one_image_array, i, j)
end

# ╔═╡ Cell order:
# ╟─5ef51c3a-70a7-11eb-2023-31113399a57f
# ╟─60ae819a-70a7-11eb-31d4-750c7f5dc6ca
# ╟─792c6a62-ec41-11ea-01f3-73e7eee23cc7
# ╟─ef1bfa16-70ea-11eb-189c-a54db292cd6f
# ╠═0504ac94-70ee-11eb-1c4e-977d9e7d35c9
# ╟─6fcac482-70ee-11eb-0b80-ff41c708053b
# ╠═0b1668ba-ec42-11ea-3e50-ed97c5b17ced
# ╟─9ebc079a-70f0-11eb-07d9-f9e80f3f4584
# ╟─15f7f90a-70f0-11eb-0d41-63677e4023f4
# ╠═b2239b96-70ef-11eb-0b85-21ecab25dc9f
# ╠═4251f668-70aa-11eb-3d89-35f8d53b7d9b
# ╠═f1568d10-ec41-11ea-3dd2-a9cb273ce5b8
# ╠═ab02d850-ec41-11ea-10b2-a1b600b12658
# ╟─f6886d90-70ed-11eb-07c4-471ee267e7c1
# ╟─3c1a3cf8-70f8-11eb-3c18-375207f321eb
# ╠═19f4ddb0-ec44-11ea-20b9-5d97fb2b1cf4
# ╠═424f5f10-ec44-11ea-076d-f3cba4435e0c
# ╠═5363a400-ec44-11ea-284e-d13a8872551c
# ╠═71ac08ea-7145-11eb-237d-5506adfb9533
# ╟─ee43d808-70fa-11eb-0cc6-337279f41494
# ╟─263a8a0a-70ee-11eb-236d-c941ba63dff3
# ╟─52461588-ea1a-4e7d-aec2-3de388d31656
# ╠═da1d65a0-ec42-11ea-0141-334c9eeeb035
# ╠═1a2a9000-ec43-11ea-3f39-8312ea286a92


using Base: Bool
using LinearAlgebra
using SparseArrays

dim_1 = 2
dim_2 = 3

function unity(i,n)
    E = spzeros(Bool,n,n)
    E[i] = true
    return E
end

# xs = [rand(3,4) for _ in 1:5]
# reduce((x,y) -> cat(x,y,dims=3), A)
# idea: https://github.com/JuliaLang/julia/pull/37196
# reduce(cat(dims=4), [ones(2,3) for _ in 1:5]) |> size
#`reduce(vcat, [rand(2,3) for _ = 1:4]): A[i][a,b] -> A[(i-1)*len_a + a, b]`
#which is the same as restructuring to` B[i,a][b] via B = [A[i][a,:] for i = 1:dim_i, a = 1:dim_a] `
#and then taking `reduce(hcat,B'[:]')'`
#`reduce(hcat, [rand(2,3) for _ = 1:4]): A[i][a,b] -> A[a, (i-1)*len_b + b]`

function partial_trace(dim_1, dim_2, traced_system)

    if traced_system == 1
        T = [kron(sparse(I, dim_1,dim_1), unity(i,dim_2))[:] for i = 1:dim_2^2]
        col = reduce(vcat,[findnz(T[i])[1] for i = 1:dim_2^2])
        row = kron(1:dim_2^2, ones(dim_1))
        return sparse(row, col, trues(18))
    else
        return    Tr_B = [reshape(kron(reshape(unity(i,dim_1), dim_1, dim_1), I(dim_2)), 1, dim_2^2 * dim_1^2)[1,j] for i = 1:dim_1^2, j =1:dim_1^2*dim_2^2]
    end
end

function choi(A, dim_1)
mm, nn = size(A)
n = mm ÷ dim_1
return reshape(permutedims(reshape(A,n,dim_1,n,dim_1),(1,3,2,4)), mm,mm)
end

function choi_rect(A,dim_1, dim_2, dim_3, dim_4)
return reshape(permutedims(reshape(A, dim_1, dim_2, dim_3, dim_4), (1,3,2,4)), dim_1*dim_3, dim_2*dim_4)
end

m = 7
n = 3

A = rand(m,m)
B = rand(n,n)

C = kron(A,B)

A_sup = kron(A[:], I(n^2))
C_sup = choi(C,m)

sum(abs.(A_sup * B[:] - C_sup[:]))

A_sup * B[:] - C[:]
reshape(A_sup * B[:],n*m,n*m) - C_sup
sum(abs.(ans[:]))

# create superoperator without applying Choi on the right hand side

G = reshape(permutedims(reshape(kron(A[:], I(n^2)), (n,n,m,m,n^2)), (1,3,2,4,5)), n^2*m^2,n^2)

reshape(G*B[:], m*n, m*n)
C
sum(abs.(G * B[:] - C[:]))

kron_fac_B = reshape(permutedims(reshape(kron(I(m^2), B[:]), (n,n,m,m,m^2)), (1,3,2,4,5)), n^2*m^2, m^2)
norm(reshape(kron_fac_B * A[:], n*m, n*m) - C)

function I_rect(n1,n2)
    if n1 >= n2
        temp = I(n1)
        temp = temp[:,1:n2]
    else
        temp = I(n2)
        temp = temp[1:n1,:]
    end
    return temp
end

m1 = 3
m2 = 4

n1 = 5
n2 = 6

A = rand(m1,m2)
B = rand(n1,n2)

C = kron(A,B)

#G = reshape(permutedims(reshape(kron(A[:], I(n1*n2)), (n1,n2,m1,m2,n1*n2)), (1,3,2,4,5)), n1*n2*m1*m2,n1*n2)

S_left = reshape(permutedims(reshape(kron(I(n1*n2), A[:]), (m1,m2,n1,n2,n1*n2)), (3,1,4,2,5)), prod([m1,m2,n1,n2]), n1*n2)
norm(S_left * B[:] - C[:])

S_right = reshape(permutedims(reshape(kron(B[:], I(m1*m2)), (m1,m2,n1,n2,m1*m2)), (3,1,4,2,5)), prod([m1,m2,n1,n2]), m1*m2)
norm(S_right * A[:] - C[:])

norm(kron(A[:], I(n1*n2)) * B[:] - choi_rect(C, n1,m1, n2, m2)[:])

norm(kron(I(m1*m2), B[:]) * A[:] - choi_rect(C,n1,m1,n2,m2)[:])

norm(kron(A'[:], I(n1*n2)) * B'[:] - C'[:])

# elegant version to show right hand side as a Choi matrix for first and second factor:
norm(kron(I(n1*n2) ,A[:]) * B[:] - choi_rect(C, n1, m1, n2, m2)'[:])
norm(kron(B[:], I(m1*m2)) * A[:] - choi_rect(C, n1, m1, n2, m2)'[:])

# check embeddings

A = rand(2)
B = rand(3)
C = rand(4)
D = rand(5)

Q = kron(A, B')
kron(kron(A, B'), ones(4,5))

kron(kron(A,B'), kron(C,D')) - kron(kron(A, B'), I(4)) * kron(I(4), kron(C,D'))

Q = kron(A, C')
kron(kron(A, ones(3)'), kron(C, ones(5)'))- kron(Q'[:], ones(3*5)')

Q = kron(A, D')
kron(kron(A, ones(3)'), kron(ones(4), D'))-kron(kron(ones(3)', Q), ones(4))

Q = kron(B, C')
kron(kron(ones(2), B'), kron(C, ones(5)')) - kron(kron(ones(2),Q'), ones(5)')

Q = kron(B, D')
kron(kron(ones(2), B'), kron(ones(4), D'))-kron(ones(2*4), Q[:]')

# multiplication table for nxn unity base matrices

n = 4
A = zeros(Int64,n^2,n^2)
function unity(i,n)
    E = spzeros(Bool,n,n)
    E[i] = true
    return E
end

for i = 1:n^2, j=1:n^2
    L = findall((unity(i,n)*unity(j,n))[:] .> 0)
    if isempty(L)
        A[i,j] = 0
    else
        A[i,j] = L[1]
    end
end
A

### density matrix

function density_matrix(n, neg = 0)
    #trace 1 remains preserved!
    dia = rand(n)
    if neg != 0
        dia[rand(1:n, neg)] .*= -1
    end
    dia = dia/sum(dia)
    ew, ev = eigen(rand(n,n))
    d_mat = ev* Diagonal(dia) * ev'
    return d_mat
end

# try to check \sum_{i != j} a_ii a_jj - abs(a_ij)^2 == sum_{i \neq j} \lambda_i lambda_j

A = density_matrix(10,0);
#Diagonal(real(A))
ew,ev = eigen(A);
L1 = [ A[i,i]*A[j,j] - abs(A[i,j])^2 for i = 1:5, j = 1:5];
L2 = [ew[i] * ew[j] * (i != j) for i = 1:5, j=1:5];

sum(real(L1) .< 0)
real(ew)'

# can every density matrix be represented as the sum of tensor products of valid density matrices?

A = density_matrix(4)
B = density_matrix(3)

ew, ev = eigen(kron(A,B))
sum(ew .< 0)


"""
    R = dss2rm(sys; fast = true, atol = 0, atol1 = atol, atol2 = atol, gaintol = atol, rtol = min(atol1,atol2) > 0 ? 0 : n*ϵ, val)

Build for the descriptor system `sys = (A-λE,B,C,D)` the rational matrix `R(λ) = C*inv(λE-A)*B+D` representing the
transfer function matrix of the system `sys`.

The keyword arguments `atol1` and `atol2` specify the absolute tolerances for the elements of `A`, `B`, `C`, `D`, and,
respectively, of `E`, and `rtol` specifies the relative tolerances for the nonzero elements of `A`, `B`, `C`, `D` and `E`.
The default relative tolerance is `n*ϵ`, where `n` is the maximal dimension of state, input and output vectors,
and `ϵ` is the working machine epsilon.
The keyword argument `atol` can be used to simultaneously set `atol1 = atol`, `atol2 = atol` and `gaintol = atol`.

The keyword argument `gaintol` specifies the threshold for the magnitude of the nonzero elements of the gain matrix
`C*inv(γE-A)*B+D`, where `γ = val` if `val` is a number or `γ` is a randomly chosen complex value of unit magnitude,
if `val = missing`. Generally, `val` should not be a zero of any of entries of `R`.

_Method:_ Each rational entry of `R(λ)` is constructed from its numerator and denominator polynomials corresponding to
its finite zeros, finite poles and gain using the method of [1].

_References:_

[1] A. Varga Computation of transfer function matrices of generalized state-space models.
    Int. J. Control, 50:2543–2561, 1989.
"""
function dss2rm(sys::DescriptorStateSpace{T}; fast::Bool = true,
                atol::Real = zero(real(T)), atol1::Real = atol, atol2::Real = atol,
                gaintol::Real = atol, val::Union{Number,Missing} = missing,
                rtol::Real = (max(sys.nx,sys.nu,sys.ny)*eps(real(float(one(T)))))*iszero(min(atol1,atol2))) where T
   NUM, DEN =  ls2rm(dssdata(sys)...; fast, atol1, atol2, rtol, gaintol, val)
   return rtf.(pm2poly(NUM)./pm2poly(DEN),Ts = sys.Ts, var = sys.Ts == 0 ? (:s) : (:z))
end
"""
    P = dss2pm(sys; fast = true, atol = 0, atol1 = atol, atol2 = atol, gaintol = 0, rtol = min(atol1,atol2) > 0 ? 0 : n*ϵ, val)

Build for the descriptor system `sys = (A-λE,B,C,D)` the polynomial matrix `P(λ) = C*inv(λE-A)*B+D` representing the
transfer function matrix of the system `sys`.

The keyword arguments `atol1` and `atol2` specify the absolute tolerances for the elements of `A`, `B`, `C`, `D`, and,
respectively, of `E`, and `rtol` specifies the relative tolerances for the nonzero elements of `A`, `B`, `C`, `D` and `E`.
The default relative tolerance is `n*ϵ`, where `n` is the maximal dimension of state, input and output vectors,
and `ϵ` is the working machine epsilon.
The keyword argument `atol` can be used to simultaneously set `atol1 = atol`, `atol2 = atol` and `gaintol = atol`.

The keyword argument `gaintol` specifies the threshold for the magnitude of the nonzero elements of the gain matrix
`C*inv(γE-A)*B+D`, where `γ = val` if `val` is a number or `γ` is a randomly chosen complex value of unit magnitude,
if `val = missing`. Generally, `val` should not be a zero of any of entries of `P`.

_Method:_ Each entry of `P(λ)` is constructed from the polynomial corresponding
to its finite zeros and gain using the method of [1].

_References:_

[1] A. Varga Computation of transfer function matrices of generalized state-space models.
    Int. J. Control, 50:2543–2561, 1989.
"""
function dss2pm(sys::DescriptorStateSpace{T}; fast::Bool = true,
                atol::Real = zero(real(T)), atol1::Real = atol, atol2::Real = atol,
                gaintol::Real = atol, val::Union{Number,Missing} = missing,
                rtol::Real = (max(sys.nx,sys.nu,sys.ny)*eps(real(float(one(T)))))*iszero(min(atol1,atol2))) where T
   sys.nx > 0 && sys.E == I && error("the given realization cannot be converted to a polynomial form")
   A, E, B, C, D = dssdata(sys)
   E == I && (E = Matrix{eltype(A)}(I, sys.nx, sys.nx))
   try
      P =  ls2pm(A, E, B, C, D; fast, atol1, atol2, rtol, gaintol, val)
      return pm2poly(P, sys.Ts == 0 ? (:s) : (:z))
   catch err
      findfirst("linearization cannot",string(err)) === nothing ? error("$err") :
         error("the given realization cannot be converted to a polynomial form")
   end
end
"""
    c2d(sysc, Ts, meth = "zoh"; x0, u0, standard = true, fast = true, prewarp_freq = freq,
               state_mapping = false, simple_infeigs = true,
               atol = 0, atol1 = atol, atol2 = atol, rtol = n*ϵ) -> (sysd, xd0, Mx, Mu)

Compute for the continuous-time descriptor system `sysc = (A-sE,B,C,D)` with the proper
transfer function matrix `Gc(λ)` and for a sampling time `Ts`, the corresponding discretized
descriptor system `sysd = (Ad-zEd,Bd,Cd,Dd)` with the transfer function matrix `Gd(z)`
according to the selected discretization method specified by `meth`.
The keyword argument `standard` specifies the option to compute a standard state-space realization
of `sysd` (i.e., with `Ed = I`), if `standard = true` (default),
or a descriptor system realization if `standard = false`.
The keyword argument `simple_infeigs = true` indicates that only simple infinite eigenvalues
of the pair `(A,E)` are to be expected (default). The setting `simple_infeigs = false`
indicates that possible uncontrollable or unobservable
higher order infinite generalized eigenvalues of the pair `(A,E)` are present and have to be removed.
`xd0` is the mapped initial condition of the state of the discrete-time system `sysd` determined from the
initial conditions of the state `x0` and input `u0` of the continuous-time system `sysc`.
The keyword argument `state_mapping = true` specifies the option to compute the state mapping matrices `Mx` and `Mu` such that
the values `xc(t)` and `xd(t)` of the system state vectors of the continuous-time system `sysc` and of the discrete-time system
`sysd`, respectively, and of the input vector `u(t)` are related as `xc(t) = Mx*xd(t)+Mu*u(t)`.
If `state_mapping = false` (the default option), then `Mx = nothing` and `Mu = nothing`.

The following discretization methods can be performed by appropriately selecting `meth`:

    "zoh"     - zero-order hold on the inputs (default);

    "foh"     - linear interpolation of inputs (also known as first-order hold);

    "impulse" - impulse-invariant discretization;

    "Tustin"  - Tustin transformation (also known as trapezoidal integration): a nonzero prewarping frequency
                `freq` can be specified using the keyword parameter `prewarp_freq = freq` to ensure
                `Gd(exp(im*freq*Ts)) = Gc(im*freq)`.

The underlying pencil manipulation algorithms employ rank determinations based on either the use of
rank revealing QR-decomposition with column pivoting, if `fast = true` (default), or the SVD-decomposition,
if `fast = false`. The rank decision based on the SVD-decomposition is generally more reliable,
but the involved computational effort is higher.

The keyword arguments `atol1`, `atol2` and `rtol` specify, respectively,
the absolute tolerance for the nonzero elements of `A`, `B`, `C`, `D`, the absolute tolerance for the nonzero elements of `E`,
and the relative tolerance for the nonzero elements of `A`, `B`, `C`, `D` and `E`.
The default relative tolerance is `n*ϵ`, where `n` is the order of the square matrices `A` and `E`, and  `ϵ` is the working machine epsilon.
The keyword argument `atol` can be used to simultaneously set `atol1 = atol` and `atol2 = atol`.
"""
function c2d(sysc::DescriptorStateSpace{T}, Ts::Real, meth::String = "zoh"; simple_infeigs::Bool = true,
             x0::Vector = zeros(T,sysc.nx), u0::Vector = zeros(T,sysc.nu), state_mapping::Bool = false,
             prewarp_freq::Real = 0, standard::Bool = true, fast::Bool = true,
             atol::Real = zero(float(real(T))), atol1::Real = atol, atol2::Real = atol,
             rtol::Real = sysc.nx*eps(real(float(one(T))))*iszero(min(atol1,atol2))) where T

    sysc.Ts == 0 || error("c2d can not be applied to a discrete-time system")
    Ts > 0 || error("the sampling time Ts must be positive")
    T1 = T <: BlasFloat ? T : promote_type(Float64,T)
    ONE = one(T1)
    n = sysc.nx
    m = sysc.nu
    length(x0) == n || error("initial state vector and system state vector dimensions must coincide")
    length(u0) == m || error("initial input vector and system input vector dimensions must coincide")

    state_mapping || ( Mx = nothing; Mu = nothing)
    # quick exit in constant case
    n == 0 && (return dss(sysc.D; Ts), x0, state_mapping ? zeros(T1,0,0) : Mx, state_mapping ? zeros(T1,0,m) : Mu)

    meth = lowercase(meth)

    if meth == "zoh" || meth == "foh" || meth == "impulse"
       if sysc.E != I
          # eliminate (if possible) all infinite eigenvalues in the continuous-time case with singular E
          syscr, xt0, Mx, Mu = dss2ss(sysc, x0; state_mapping, simple_infeigs, fast, atol1, atol2, rtol)
          isnothing(Mx) && (state_mapping = false)
          state_mapping && norm(Mx*xt0+Mu*u0-x0,Inf) >= eps(norm(x0,Inf)*100) && (@warn "Inconsistent initial state")
          x0 = copy(xt0)
          A, _, B, C, D = dssdata(syscr)
          n, m = size(B)
         # if rcond(sysc.E) >= n*eps(float(real(T1)))
         #     # E invertible
         #     A, E, B, C, D = dssdata(T1,sysc)
         #     F = lu!(E)
         #     # get rid of E matrix
         #     ldiv!(F,A); ldiv!(F,B)
         #     state_mapping && (Mx = I; Mu = zeros(T1,n,m))
         #  else
         #     # E singular
         #     syscr, xt0, Mx, Mu = dss2ss(sysc, x0; state_mapping, simple_infeigs, fast, atol1, atol2, rtol)
         #     isnothing(Mx) && (state_mapping = false)
         #     state_mapping && norm(Mx*xt0+Mu*u0-x0,Inf) >= eps(norm(x0,Inf)*100) && (@warn "Inconsistent initial state")
         #     x0 = copy(xt0)
         #     A, _, B, C, D = dssdata(syscr)
         #     n, m = size(B)
         #  end
       else
          A, _, B, C, D = dssdata(T1,sysc)
          state_mapping && (Mx = I; Mu = zeros(T1,n,m))
       end
       m = size(D,2)
       if meth == "zoh"
          G = exp([ rmul!(A,Ts) rmul!(B,Ts); zeros(T1,m,n+m)])
          return (dss(view(G,1:n,1:n), view(G,1:n,n+1:n+m), C, D; Ts), x0, Mx, Mu)
       elseif meth == "foh"
          G = exp([ rmul!(A,Ts) rmul!(B,Ts) zeros(T1,n,m); zeros(T1,m,n+m) 1/Ts*I; zeros(T1,m,n+2m)])
          Ad = view(G,1:n,1:n)
          G1 = view(G,1:n,n+1:n+m)
          G2 = view(G,1:n,n+m+1:n+2m)
          # discrete to continuous state map Mx = M1 Mu = Mu+Mx*G2
          state_mapping && mul!(Mu, Mx, G2, ONE, ONE)
          return (dss(Ad, G1+(Ad-I)*G2, C, D+C*G2; Ts), x0-G2*u0, Mx, Mu)
       else # meth == "impulse"
         G = exp(rmul!(A,Ts))
         return (dss(G, G*B, C, C*B; Ts), x0, Mx, Mu)
       end
    elseif meth == "tustin"
       A, E, B, C, D = dssdata(T1,sysc)
       prewarp_freq == 0 ? t = Ts : t = 2*tan(prewarp_freq*Ts/2)/prewarp_freq
       if standard
          Ed = E-t/2*A
          xd0 = Ed*x0
          state_mapping ? F = lu(Ed) : F = lu!(Ed)
          X = ldiv!(F,B*t)
          Ad = rdiv!(E+t/2*A,F)    # Ad = (E + A*T/2)/(E - A*T/2)
          Bd = E*X                 # Bd = E * (E - A*T/2) \ B*T
          mul!(D,C,X,0.5,1)        # Dd = D + C*(E - A*T/2)\B*(T/2) = D + C*X/2
          rdiv!(C,F)               # Cd = C/(E - A*T/2)
          xd0 -= (X*u0)/2          # xd0 = (E - A*T/2)*x0 - (E - A*T/2)\B*(T/2)*u0 = (E - A*T/2)*x0 - X*u0/2
          state_mapping && ( Mx = inv(F); Mu = copy(ldiv!(F,X/2)))
          return (dss(Ad, Bd, C, D; Ts), xd0, Mx, Mu)
       else
          Ed = E-t/2*A              # Ed = E - A*T/2
          X = Ed\(B*t)              # X = (E - A*T/2)\B*T
          mul!(D,C,X,0.5,1.)        # Dd = D + (T/2)*C*(E - A*T/2)\B = D + C*X/2
          Ad = E+t/2*A              # Ad = E + A*T/2;  Cd = C
          Bd = E*X                  # Bd = E * (E - A*T/2) \ B*T
          xd0 = x0 - (X*u0)/2       # xd0 = x0 - (E - A*T/2)\B*(T/2)*u0 = x0 - X*u0/2
          state_mapping && ( Mx = I; Mu = X/2)
          return (dss(Ad, Ed, Bd, C, D; Ts), xd0, Mx, Mu)
       end
    else
       error("no such method")
    end
    # end C2D
end
"""
    rd = c2d(rc, Ts, meth = "zoh"; prewarp_freq = freq, atol = 0, rtol = n*ϵ)

Compute for the continuous-time rational transfer function `rc` and for a sampling time `Ts`,
the corresponding discretized rational transfer function `rd`
according to the selected discretization method specified by `meth`.

The following discretization methods can be performed by appropriately selecting `meth`:

    "zoh"     - zero-order hold on the inputs (default);

    "foh"     - linear interpolation of inputs (also known as first-order hold);

    "impulse" - impulse-invariant discretization;

    "matched" - matched pole-zero method (see [1]);

    "Tustin"  - Tustin transformation (also known as trapezoidal integration): a nonzero prewarping frequency
                `freq` can be specified using the keyword parameter `prewarp_freq = freq` to ensure
                `rd(exp(im*freq*Ts)) = rc(im*freq)`.

The keyword arguments `atol` and `rtol` specify, respectively,
the absolute and relative tolerances, respectively,
for the nonzero coefficients of the numerator and denominator polynomials of  `rc`.
The default relative tolerance is `n*ϵ`, where `n` is the order of `rc`
(i.e., the maximum degree of the numerator and denominator polynomials) and  `ϵ` is the working machine epsilon.

_References_:

[1] G.F. Franklin, D.J. Powell and  M.L. Workman, Digital Control of Dynamic Systems (3rd Edition), Prentice Hall, 1997.
"""
function c2d(rc::RationalTransferFunction, Ts::Real, meth::String = "zoh";
             prewarp_freq::Real = 0, atol::Real = zero(float(real(eltype(eltype(rc))))),
             rtol::Real = order(rc)*eps(real(float(one(eltype(eltype(rc))))))*iszero(atol))
    rc.Ts == 0 || error("c2d can not be applied to a discrete-time system")
    Ts > 0 || error("the sampling time Ts must be positive")
    T = eltype(eltype(rc))

    # quick exit in constant case
    m = degree(rc.num)
    n = degree(rc.den)
    m <= 0 && n == 0 && (return rtf(rc; Ts, var = :z))

    meth = lowercase(meth)

    if meth == "zoh" || meth == "foh" || meth == "impulse"
       m > n && error("the selected discretization method is not applicable to improper rational transfer functions")
       return dss2rm(c2d(dss(rc), Ts, meth)[1]; fast = true, atol1 = atol, atol2 = atol, rtol, gaintol = atol)[1,1]
    elseif meth == "tustin"
       return confmap(rc,rtfbilin(meth; Ts, prewarp_freq)[1])
    elseif meth == "matched"
       zd = exp.(Ts*rc.zeros)
       m < n-1 && (zd = [zd; -ones(n-1-m)]; m = n-1)
       num = m == 0 ? one(Polynomial{Float64,:z}(1)) : fromroots(zd; var = :z)
       den = n == 0 ? one(Polynomial{Float64,:z}(1)) : fromroots(exp.(Ts*rc.poles); var = :z)
      #  t = dcgain(rc)
      #  isfinite(t) ? k = t*den(1)/num(1) : (f1 = 0.001*rand(); f2 = exp(f1); k = evalfr(rc,f1)*den(f2)/num(f2))
       k = dcgain(rc)*den(1)/num(1)
       isfinite(k) || (f1 = 0.001*rand(); f2 = exp(f1); k = evalfr(rc,f1)*den(f2)/num(f2))
       T <: Complex || (num = Polynomial(real(num.coeffs), :z); den = Polynomial(real(den.coeffs), :z); k = real(k))
       return rtf(k*num,den; Ts)
    else
       error("no such method")
    end
    # end C2D
end
"""
    Rd = c2d(Rc, Ts, meth = "zoh"; prewarp_freq = freq, atol = 0, rtol = n*ϵ)

Compute for the continuous-time rational transfer function matrix `Rc` and for a sampling time `Ts`,
the corresponding discretized rational transfer function matrix `Rd`
according to the selected discretization method specified by `meth`.

The following discretization methods can be performed by appropriately selecting `meth`:

      "zoh"     - zero-order hold on the inputs (default);

      "foh"     - linear interpolation of inputs (also known as first-order hold);

      "impulse" - impulse-invariant discretization;

      "matched" - matched pole-zero method (see [1]);

      "Tustin"  - Tustin transformation (also known as trapezoidal integration): a nonzero prewarping frequency
                  `freq` can be specified using the keyword parameter `prewarp_freq = freq` to ensure
                  `Rd(exp(im*freq*Ts)) = Rc(im*freq)`.

The keyword arguments `atol` and `rtol` specify, respectively,
the absolute and relative tolerances, respectively,
for the nonzero coefficients of the numerator and denominator polynomials of the elements of `Rc`.
The default relative tolerance is `10*ϵ`, where `ϵ` is the working machine epsilon.

_References_:

[1] G.F. Franklin, D.J. Powell and  M.L. Workman, Digital Control of Dynamic Systems (3rd Edition), Prentice Hall, 1997.
"""
function c2d(Rc::VecOrMat{<:RationalTransferFunction}, Ts::Real, meth::String = "zoh";
                 prewarp_freq::Real = 0, atol::Real = zero(float(real(_eltype(Rc)))),
                 rtol::Real = 10*eps(real(float(one(_eltype(Rc)))))*iszero(atol))
    nrow = size(Rc,1)
    ncol = size(Rc,2)
    T = _eltype(Rc)
    T1 = T <: BlasFloat ? T : promote_type(Float64,T)
    Rd = similar(Rc, RationalTransferFunction{T1,:z,Polynomial{T1,:z},Ts}, nrow, ncol)
    for j = 1:ncol
        for i = 1:nrow
            Rd[i,j] = c2d(Rc[i,j], Ts, meth; prewarp_freq, atol, rtol)
        end
    end
    return Rd
end

"""
    gbilin(sys, g; compact = true, minimal = false, standard = true, atol = 0, atol1 = atol, atol2 = atol, rtol = n*ϵ) -> (syst, ginv)

Compute for the descriptor system `sys = (A-λE,B,C,D)` with the transfer function matrix `G(λ)` and
a first degree real rational transfer function `g = g(δ)`,
the descriptor system realization `syst = (At-δEt,Bt,Ct,Dt)` of `G(g(δ))` corresponding to the bilinear transformation
`λ = g(δ) = (aδ+b)/(cδ+d)`. For a continuous-time transfer function `g(δ)`, `δ = s`, the complex variable in
the Laplace transform, while for a discrete-time transfer function,
`δ = z`, the complex variable in the `Z`-transform. `syst` inherits the sampling-time of `sys1`.
`sysi1` is the transfer function `ginv(λ) = (d*λ-b)/(-c*λ+a)` representing the inverse of the bilinear transformation `g(δ)`
(i.e., `g(ginv(λ)) = 1`).

The keyword argument `compact` can be used to specify the option to compute a compact descriptor realization
without non-dynamic modes, if `compact = true` (the default option) or to disable the ellimination of non-dynamic modes if `compact = false`.

The keyword argument `minimal` specifies the option to compute minimal descriptor realization, if  `minimal = true`, or
a nonminimal realization if `minimal = false` (the default option).

The keyword argument `standard` specifies the  option to compute a standard state-space (if possible)
realizations of `syst`, if `standard = true` (default), or a descriptor system realization if `standard = false`.

The keyword arguments `atol1`, `atol2` and `rtol` specify, respectively,
the absolute tolerance for the nonzero elements of `A`, `B`, `C`, `D`, the absolute tolerance for the nonzero elements of `E`,
and the relative tolerance for the nonzero elements of `A`, `B`, `C`, `D` and `E`.
The default relative tolerance is `n*ϵ`, where `n` is the order of the square matrices `A` and `E`, and  `ϵ` is the working machine epsilon.
The keyword argument `atol` can be used to simultaneously set `atol1 = atol` and `atol2 = atol`.
"""
function gbilin(sys::DescriptorStateSpace{T},g::RationalTransferFunction;
                compact::Bool = true, minimal::Bool = false, standard::Bool = true,
                atol::Real = zero(float(real(T))), atol1::Real = atol, atol2::Real = atol,
                rtol::Real = sys.nx*eps(real(float(one(T))))*iszero(min(atol1,atol2))) where T

    # check g is first order
    num = g.num; degn = degree(num)
    den = g.den; degd = degree(den)

    (degn > 1 || degd > 1 || max(degn,degd) == 0) &&
        error("The McMillan degree of g must be one")

    iszero(num) && error("g must be nonzero")

    Ts = sys.Ts;
    Ts1 = g.Ts;
    Ts > 0 && Ts1 > 0 && Ts != Ts1 && error("sys and g must have the same sampling periods")

    # assume g(delta) = (a*delta+b)/(c*delta+d)
    degn > 0 ? (a = num[1]; b = num[0]) : (a = 0; b = num[0])
    degd > 0 ? (c = den[1]; d = den[0]) : (a = a/den[0]; b = b/den[0]; c = 0; d = 1)

    A, E, B, C, D = dssdata(sys);
    n, m = size(B); p = size(C,1);

    if degd > 0
       # rational case
       At = [-b*E+d*A d*B; zeros(m,n) -eye(m)];
       Et = [a*E-c*A -c*B; zeros(m,n+m)];
       Bt = [zeros(n,m); eye(m)];
       Ct = [C D]; Dt = zeros(p,m);
       syst = dss(At,Et,Bt,Ct,Dt, Ts = Ts1);
       if minimal
          if standard
             syst = gss2ss(gir(syst,atol1 = atol1, atol2 = atol2, rtol = rtol),atol1 = atol1, atol2 = atol2, rtol = rtol)[1];
          else
             syst = gminreal(syst,atol1 = atol1, atol2 = atol2, rtol = rtol);
          end
       else
          syst = gss2ss(syst, atol1 = atol1, atol2 = atol2, rtol = rtol, Eshape = standard ? "ident" : "triu")[1];
       end
    else
       # polynomial case
       if E == I
          # preserve standard system form
          syst = dss((A-b*E)/a,B/a,C,D,Ts=Ts1);
       else
          syst = dss(A-b*E,a*E,B,C,D,Ts=Ts1);
          standard && (syst = gss2ss(syst, atol1 = atol1, atol2 = atol2, rtol = rtol)[1] )
       end
    end

    if Ts == 0
       Tsi = 0
    elseif Ts != 0 && Ts1 == 0
       Tsi = Ts
    else # Ts != 0 && Ts1 != 0
       Ts1 < 0 ? Tsi = Ts : Tsi = Ts1;
    end
    #ginv = (d*s-b)/(-c*s+a)

    ginv = rtf(Polynomial([-b, d]), Polynomial([a, -c]), Ts=Tsi, var = Tsi == 0 ? :s : :z)

    return syst, ginv
    # end GBILIN
end
"""
    timeresp(sys, u, t, x0 = 0; interpolation = "zoh", state_history = false, simple_infeigs = true,
             fast = true, atol = 0, atol1 = atol, atol2 = atol, rtol = n*ϵ) -> (y, tout, x)

Compute the time response of a proper descriptor system `sys = (A-λE,B,C,D)` to the input signals
described by `u` and `t`. The time vector `t` consists of regularly spaced time samples. The
matrix `u` has as many columns as the inputs of `sys` and its `i`-th row specifies
the input values at time `t[i]`. For discrete-time models, `u` should be sampled at the same rate as `sys`
if `sys.Ts > 0` and `t` must have all time steps equal to `sys.Ts` or can be set to an empty vector.
The vector `x0` specifies the initial state vector at time `t[1]` and is set to zero when omitted.

The matrix `y` contains the resulting time history of the outputs of `sys` and
the vector `tout` contains the corresponding values of the time samples.
The `i`-th row of `y` contains the output values at time `tout[i]`.
If the keyword parameter value `state_history = false` is used, then the matrix `x` contains
the resulting time history of the state vector and its `i`-th row contains
the state values at time `tout[i]`. By default, the state history is not computed and `x = nothing`.

For continuous-time models, the input values are interpolated between samples. By default,
zero-order hold based interpolation is used. The linear interpolation method can be selected using
the keyword parameter `interpolation = "foh"`.

By default, the uncontrollable infinite eigenvalues and simple infinite eigenvalues of the pair `(A,E)`
are eliminated.
The underlying pencil manipulation algorithms employ rank determinations based on either the use of
rank revealing QR-decomposition with column pivoting, if `fast = true` (default), or the SVD-decomposition,
if `fast = false`. The rank decision based on the SVD-decomposition is generally more reliable,
but the involved computational effort is higher.

The keyword arguments `atol1`, `atol2` and `rtol` specify, respectively,
the absolute tolerance for the nonzero elements of `A`, `B`, `C`, `D`, the absolute tolerance for the nonzero elements of `E`,
and the relative tolerance for the nonzero elements of `A`, `B`, `C`, `D` and `E`.
The default relative tolerance is `n*ϵ`, where `n` is the order of the square matrices `A` and `E`, and  `ϵ` is the working machine epsilon.
The keyword argument `atol` can be used to simultaneously set `atol1 = atol` and `atol2 = atol`.
"""
function timeresp(sys::DescriptorStateSpace{T}, u::AbstractVecOrMat{<:Number}, t::AbstractVector{<:Real},
                  x0::AbstractVector{<:Number} = zeros(T,sys.nx); interpolation::String = "zoh",
                  state_history::Bool = false, fast::Bool = true,
                  atol::Real = zero(float(real(T))), atol1::Real = atol, atol2::Real = atol,
                  rtol::Real = sys.nx*eps(real(float(one(T))))*iszero(min(atol1,atol2))) where T

    T1 = T <: BlasFloat ? T : promote_type(Float64,T)
    T2 = promote_type(T1,eltype(x0),eltype(u))
    n = sys.nx
    p, m = sys.ny, sys.nu

    N, m1 = size(u,1), size(u,2)
    m == m1 || error("u must have as many columns as system inputs")
    n == length(x0) || error("x0 must have the same length as the system state vector")

    disc = !iszero(sys.Ts)
    ns = length(t)
    ns > 0 && ns != N && error("u must have the same number of rows as the number of values in t")
    if ns > 1
       dt = t[2]-t[1]
       disc && sys.Ts > 0 && abs.(sys.Ts-dt) > 0.0000001*dt
       (any(diff(t) .<= 0) || any(isinf.(t)) || any(abs.(diff(t).-dt).> 0.00001*dt)) &&
            error("time vector t must contain monotonically increasing and evenly spaced time samples")
    end
    if disc
       # set tout
       ns <= 1 && (dt = abs(sys.Ts) )
       tout = Vector{real(T1)}(0:dt:(N-1)*dt)
    else
       tout = t
    end
    p == 1 ? y = Vector{T2}(undef, N) : y = Matrix{T2}(undef, N, p)
    state_history ?  x = Matrix{T2}(undef, N, n) : x = nothing
    if disc
       if sys.E == I
          idmap = true
          xt = copy(x0)
          A, _, B, C, D = dssdata(T1,sys)
       else
          F = lu(sys.E;check=false)
          if issuccess(F) && rcond(UpperTriangular(F.U)) > n*eps(float(real(T1)))
             A, _, B, C, D = dssdata(T1,sys)
             ldiv!(F,A); ldiv!(F,B)
             idmap = true
             xt = copy(x0)
          else
            sys, xt, Mx, Mu = dss2ss(sys, x0; state_mapping = state_history, simple_infeigs = false, fast, atol1, atol2, rtol)
            state_history = !isnothing(Mx)
            idmap = state_history && Mx == I && iszero(Mu)
            A, _, B, C, D = dssdata(T1,sys)
         end
       end
    else
       sysd, xt, Mx, Mu = c2d(sys, dt, interpolation; x0, u0 = u[1,:],
                              state_mapping = state_history, simple_infeigs = false, atol1, atol2, rtol, fast)
       A, _, B, C, D = dssdata(sysd)
       idmap = state_history && Mx == I && iszero(Mu)
   end
   for i = 1:N
      ut = view(u,i,:)
      y[i,:] = C*xt + D*ut
      state_history && (x[i,:] = idmap ? xt : Mx*xt + Mu*ut)
      xt = A*xt + B*ut
   end
   return y, tout, x
end
"""
    rt = confmap(r, f)

Apply the conformal mapping transformation `λ = f(δ)` to the rational transfer function `r(λ)`
and return `rt(δ) = r(f(δ))`. The resulting `rt` inherits the sampling time and variable of `f`.
"""
function confmap(pol::Polynomial,f::RationalTransferFunction)
    # perform Horner's algorithm
    n = length(pol)-1
    s = pol[n]*one(f)
    for i in n-1:-1:0
        s = s*f + pol[i]
    end
    return rtf(s,Ts = f.Ts)
end
function confmap(r::RationalTransferFunction,f::RationalTransferFunction)
    m = degree(r.num)
    n = degree(r.den)
    pol = f.den
    return m >= n ? rtf(confmap(r.num,f).num,confmap(r.den,f).num*pol^(m-n),Ts = f.Ts,var = f.var) :
                    rtf(confmap(r.num,f).num*pol^(n-m),confmap(r.den,f).num,Ts = f.Ts,var = f.var)
end
"""
    Rt = confmap(R, f)

Apply elementwise the conformal mapping transformation `λ = f(δ)` to the rational transfer function matrix `R(λ)`
and return `Rt(δ) = R(f(δ))`. The resulting elements of `Rt` inherit the sampling time and variable of `f`.
"""
function confmap(R::VecOrMat{<:RationalTransferFunction},f::RationalTransferFunction)
    nrow = size(R,1)
    ncol = size(R,2)
    T = promote_type(_eltype(R[1]),_eltype(f))
    Rt = similar(R,RationalTransferFunction{T,f.var,Polynomial{T,f.var},sampling_time(R[1])}, nrow, ncol)
    for j = 1:ncol
        for i = 1:nrow
            Rt[i,j] = confmap(R[i,j],f)
        end
    end
    return Rt
end


n = rand(1:10)
m = rand(1:20)
A = matrixdepot("pei", n, m)
B = ones(n,n)
@test issym(A)
@test_approx_eq matrixdepot("pei", n) - triu(B) - tril(B) zeros(n,n)
println("'pei' passed test...")


type PSRL<:FiniteHorizonAgent
    policy::Matrix{UInt64}
    ns::Array{UInt64, 4}
    n::Array{UInt64, 3}
    rewards::Array{Float64, 3}
    reward_precisions::Array{Float64, 3}
    Pcur::Array{Float64, 4}
    Rcur::Array{Float64, 3}
    α::Float64 # Dirichlet prior for transitions
    τ::Float64 # Precision of Gaussian prior for mean returns
    rewards_known::Bool
    Vopt::Matrix{Float64}
    Qopt::Array{Float64, 3}
end
function PSRL(S, A, H, α::Real, τ0::Real, τ::Real)
    policy = zeros(UInt64, S, H)
    ns = zeros(UInt64, S, S, A, H)
    Pcur = zeros(S, S, A, H)
    Pcur[1, :, :, :] = 1.
    n = zeros(UInt64, S, A, H)
    Rcur = zeros(S, A, H)
    m = PSRL(policy, ns, n, zeros(S, A, H), fill(τ0, S, A, H), Pcur, Rcur, α, τ, false, zeros(S, H+1), zeros(S, A, H+1))
    rand!(m.policy, 1:A) # Initialize with uniformly random policy
    m
end
function PSRL(S, A, H, rewards::Array, α)
    policy = zeros(UInt64, S, H)
    ns = zeros(UInt64, S, S, A, H)
    Pcur = zeros(S, S, A, H)
    Pcur[1, :, :, :] = 1.
    n = zeros(UInt64, S, A, H)
    m = PSRL(policy, ns, n, rewards, zeros(S, A, H), Pcur, rewards, α, 0., true, zeros(S, H+1), zeros(S, A, H+1))
    rand!(m.policy, 1:A) # Initialize with uniformly random policy
    m
end

function observe!(m::PSRL, s, a, t, r, sn)
    m.ns[sn, s, a, t] += 1
    m.n[s, a, t] +=  1
    if !m.rewards_known
        τ1 = m.τ + m.reward_precisions[s,a,t]
        m.rewards[s, a, t] = (m.reward_precisions[s,a,t] * m.rewards[s,a,t] + m.τ * r) / τ1
        m.reward_precisions[s,a,t] = τ1
    end
end

function sample_MDP!(m::PSRL)
    S = nS(m)
    A = nA(m)
    H = horizon(m)
    P = m.Pcur
    αs = fill(m.α, S)
    for s=1:S, a=1:A, t=1:H
        αs .= m.ns[:, s, a, t] + m.α
        P[:, s, a, t] = rand(Dirichlet(αs))
    end

    R = m.Rcur
    if !m.rewards_known
        for s=1:S, a=1:A, t=1:H
            R[s,a,t] = randn() * 1 / √(m.reward_precisions[s,a,t]) + m.rewards[s,a,t]
        end
    else
        R = m.rewards
    end
    P, R
end

function update_policy!(m::PSRL)
    H = horizon(m)
    S = nS(m)
    A = nA(m)
    P, R = sample_MDP!(m)
    Qnew = zeros(S, A)

    for t=H:-1:1
        Vold = m.Vopt[:, t+1]
        Qnew = R[:, :, t] + squeeze(sum(Vold .* P[:, :, :, t], 1), 1)
        for s=1:S
            m.policy[s, t] = indmax(Qnew[s, :])
            m.Vopt[s, t] = Qnew[s, m.policy[s, t]]
        end
        m.Qopt[:,:,t] = Qnew
    end
end