struct SRKFTUIntermediate{T}
    x_apri::Vector{T}
    p_apri::Matrix{T}
    qr_zeros::Vector{T}
    qr_space::Vector{T}
    puft_vcat_q::Matrix{T}
end

function SRKFTUIntermediate(T::Type, num_x::Number)
    qr_zeros = zeros(T, 2 * num_x)
    puft_vcat_q = Matrix{T}(undef, 2 * num_x, num_x)
    qr_space_length = calc_gels_working_size(puft_vcat_q, qr_zeros)
    SRKFTUIntermediate(
        Vector{T}(undef, num_x),
        Matrix{T}(undef, num_x, num_x),
        qr_zeros,
        Vector{T}(undef, qr_space_length),
        puft_vcat_q
    )
end

SRKFTUIntermediate(num_x::Number) = SRKFTUIntermediate(Float64, num_x)

function calc_upper_triangular_of_qr!(A)
    R, = LAPACK.gels!('N', A, zeros(size(A, 1), 1)) # Q, R = qr(A)
    R
end

function calc_upper_triangular_of_qr_inplace!(res, A, B, space)
    mygels!(res, A, B, space)
end

# This implementation is based on
# https://github.com/rlabbe/filterpy/blob/master/filterpy/kalman/square_root.py
@inline function calc_apriori_covariance(
    P::Cholesky,
    F::Union{Number, AbstractMatrix},
    Q::Cholesky
)
    A = vcat(P.U * F', Q.U)
    R = calc_upper_triangular_of_qr!(A)
    Cholesky(R, 'U', 0)
end

function time_update!(tu::SRKFTUIntermediate, x, P::Cholesky, F::Union{Number, AbstractMatrix}, Q::Cholesky)
    x_apri = calc_apriori_state!(tu.x_apri, x, F)
    tu.puft_vcat_q[1:size(F, 1),:] .= @~ P.U * F'
    tu.puft_vcat_q[size(F, 1) + 1:end,:] .= Q.U
    R = calc_upper_triangular_of_qr_inplace!(tu.p_apri, tu.puft_vcat_q, tu.qr_zeros, tu.qr_space)
    P_apri = Cholesky(R, 'U', 0)
    KFTimeUpdate(x_apri, P_apri)
end

function measurement_update(
    x,
    P::Cholesky,
    y,
    H::Union{Number, AbstractVector, AbstractMatrix},
    R::Cholesky;
    consider = nothing
)
    ỹ = calc_innovation(H, x, y)
    PHᵀ, S, P_post = calc_cross_cov_innovation_posterior(P, H, R, consider)
    K = calc_kalman_gain(PHᵀ, S.L, consider)
    x_post = calc_posterior_state(x, K, ỹ, consider)
    KFMeasurementUpdate(x_post, P_post, ỹ, S, K)
end

function create_matrix_for_qr(P::Cholesky, H, R::Cholesky)
    num_y = size(R, 1)
    num_x = size(P, 1)
    M = zeros(num_y + num_x, num_y + num_x)
    M[1:num_y, 1:num_y] .= R.U
    M[num_y + 1:end, 1:num_y] .= P.U * H'
    M[num_y + 1:end, num_y + 1:end] .= P.U
    M
end

function calc_cross_cov_innovation_posterior(P::Cholesky, H, R::Cholesky, consider)
    num_y = size(R, 1)
    M = create_matrix_for_qr(P, H, R)
    RU = calc_upper_triangular_of_qr!(M)
    PHᵀ = extract_cross_covariance(RU, num_y)
    S = extract_innovation_covariance(RU, num_y)
    P_post = extract_posterior_covariance(RU, num_y, P, consider)
    PHᵀ, S, P_post
end

@inline extract_cross_covariance(RU, num_y) = transpose(RU[1:num_y, num_y + 1:end])
@inline extract_innovation_covariance(RU, num_y) = Cholesky(RU[1:num_y, 1:num_y], 'U', 0)
@inline extract_posterior_covariance(RU, num_y, P, consider::Nothing) =
    Cholesky(RU[num_y + 1:end, num_y + 1:end], 'U', 0)

struct SRKFMUIntermediate{T,K<:Union{<:AbstractVector{T},<:AbstractMatrix{T}}}
    innovation::Vector{T}
    kalman_gain::K
    m::Matrix{T}
    qr_zeros::Vector{T}
    qr_space::Vector{T}
    R::Matrix{T}
    x_posterior::Vector{T}
end

function SRKFMUIntermediate(T::Type, num_x::Number, num_y::Number)
    qr_zeros = zeros(T, num_x + num_y)
    M = Matrix{T}(undef, num_x + num_y, num_x + num_y)
    qr_space_length = calc_gels_working_size(M, qr_zeros)
    SRKFMUIntermediate(
        Vector{T}(undef, num_y),
        Matrix{T}(undef, num_x, num_y),
        M,
        qr_zeros,
        Vector{T}(undef, qr_space_length),
        Matrix{T}(undef, num_x + num_y, num_x + num_y),
        Vector{T}(undef, num_x)
    )
end

SRKFMUIntermediate(num_x::Number, num_y::Number) = SRKFMUIntermediate(Float64, num_x, num_y)

function measurement_update!(
    mu::SRKFMUIntermediate,
    x,
    P::Cholesky,
    y,
    H::Union{Number, AbstractVector, AbstractMatrix},
    R::Cholesky
)
    ỹ = calc_innovation!(mu.innovation, H, x, y)
    dim_y = length(y)
    M = mu.m
    M[1:dim_y, 1:dim_y] .= R.U
    M[dim_y + 1:end, 1:dim_y] .= @~ P.U * H'
    M[dim_y + 1:end, dim_y + 1:end] .= P.U
    RU = calc_upper_triangular_of_qr_inplace!(mu.R, M, mu.qr_zeros, mu.qr_space)
    PHᵀ = transpose(@view(RU[1:dim_y, dim_y + 1:end]))
    S = Cholesky(@view(RU[1:dim_y, 1:dim_y]), 'U', 0)
    K = calc_kalman_gain!(mu.kalman_gain, PHᵀ, S.L)
    x_post = calc_posterior_state!(mu.x_posterior, x, K, ỹ)
    P_post = Cholesky(@view(RU[dim_y + 1:end, dim_y + 1:end]), 'U', 0)
    KFMeasurementUpdate(x_post, P_post, ỹ, S, K)
end

function calc_kalman_gain!(K, PHᵀ, SL::LowerTriangular)
    K .= PHᵀ
    rdiv!(K, SL)
end


module ArensdorfOrbit
 #=
 In my ongoing project to get up to speed in Julia, I'm now
 taking a look at "arensdorf orbits", that were designed
 by Richard Arensdorf while he was figuring out how to
 put men on the moon.

 It's a simple diferential equation, and it gives nice plots
 so it's perfect to exercise two parts of my Julia-Fu

 https://www.johndcook.com/blog/2020/02/08/arenstorf-orbit/

=#

  using DifferentialEquations, ForwardDiff, LinearAlgebra
  using OrdinaryDiffEq
  using Plots

  # Bootstrapping using Lorentz attractor.  This is a first order
  # differential equation, but many of the same
  # techniques are used, so it's a good starting point
  function lorenz!(du,u,p,t)
     x, y, z  = u

     du[1] = 10.0*(y-x)
     du[2] = x*(28.0-z) - y
     du[3] = x*y - (8/3)*z
  end

  # ... works abslutely awsomely!
  function plot_lorenz()
    u0 = [1.0;0.0;0.0]
    tspan = (0.0,100.0)
    prob = ODEProblem(lorenz!,u0,tspan)
    sol = solve(prob)
    plot(sol,vars=(1,2,3))
  end

   # .... work nicely too.
   function plot_hh()
     function HH_acceleration(dv,v,u,p,t)
       x,y  = u
       dx,dy = dv
       dv[1] = -x - 2x*y
       dv[2] = y^2 - y -x^2
     end
     tspan = (0.0,100.0)
     initial_positions = [0.0,0.1]
     initial_velocities = [0.5,0.0]
     prob = SecondOrderODEProblem(HH_acceleration,initial_velocities,initial_positions,tspan)
     sol = solve(prob, KahanLi8(), Dt=1/10);
     plot(sol,vars=(1,2,3))
   end

   #  This also works now
   function plot_arensdorf()
     μ   = 0.012277471
     μ′  = 1 - μ

     function arensdorf_orbit(du, u, z, p, t)
         x, y  = z
	 x′,y′ = u

	 D(z) = ((x + z)^2  + y^2)^(3/2)
	 D1 = D( μ)
	 D2 = D(-μ′)

	 x′′ = x + 2y′  - (μ′*(x + μ)/D1) - (μ*(x - μ′)/D2)
	 y′′ = y - 2x′  - (μ′*y/D1)       - (μ*y/D2)

	 du[1] = x′′
	 du[2] = y′′
     end

     # Sensitivity analysis must be added to this code

     initial_positions =  [0.994,  0]
     initial_velocities = [0.0,   -2.001585106]
     tspan = (0.0, 10.0)
     prob = SecondOrderODEProblem(arensdorf_orbit, initial_velocities, initial_positions, tspan)
     sol = solve(prob, KahanLi8(), dt=1/100);
     plot(sol,vars=(1,2))
   end
end


function PR(A, α = 0.85, maxiters = 100, ϵ = 1.0e-4)
    d = reduce(+, A; dims=2)
    n = size(A, 1)
    r = GBVector(n, 1.0 / n)
    t = GBVector{Float64}(n)
    d[:, accum=/] = α
    teleport = (1 - α) / n
    for _ ∈ 1:maxiters
        temp = t; t = r; r = temp
        w = t ./ d
        r[:] = teleport
        mul!(r, A', w, (+, second), accum=+)
        t .-= r
        map!(abs, t)
        if reduce(+, t) <= ϵ
            break
        end
    end
    return r
end


using CSDP, JuMP
optim = optimizer_with_attributes(CSDP.Optimizer, MOI.Silent() => true)

@testset "Spectral radius" begin
    @test spectral_radius(I(2)) == 1.0
    A = [1 2; 3 0]
    @test spectral_radius(A) == 3.0
    A = [1 0 1; 0 1 0; 1 0 1]
    @test spectral_radius(A) == 2.0
end

E1, E2, E3, E4 = [1 0.; 0 1.], [1 0.; 0 -1.], [-1 0.; 0 1.], [-1 0.; 0 -1.]
D1 = [0. 0.]; D2, D3, D4 = copy(D1), copy(D1), copy(D1)
X = [[E1, D1], [E2, D2], [E3, D3], [E4, D4]]

Σ1 = [[1. -1.; 2. 1.], [1. -1.; 1. 1.], [3. -2.; 1. 0.], [-2. 0.; 1. -1.]]
G1 = automaton_constructor(Σ1)
s1 = discreteswitchedsystem(Σ1, G1, X)
Σ2 = [[1.34 -1.34; 1. 0.], [1.34 -0.67; 1. 0.], [0.67 -1.34; 1. 0.], [0.67 -0.67; 1. 0.]]
G2 = automaton_constructor(Σ2)
s2 = discreteswitchedsystem(Σ2, G2, X)
Σ3 = [[1. -5.; 2. 0.], [2. -3.; 1. 0.], [0 -1.; 0. 1.], [2. 3.; -1. -4.]]
G3 = automaton_constructor(Σ3)
s3 = discreteswitchedsystem(Σ3, G3, X)

γsdp1 = sdpbound_γ(s1)
gammasdp1 = sdpbound_gamma(s1)
γsos1 = sosbound_γ(s1, 2)
gammasos1 = sosbound_gamma(s1, 2)
γsdp2 = sdpbound_γ(s2, optimizer=optim, verbose=2)
γsos2 = sosbound_γ(s2, 2, optimizer=optim, verbose=2)
γsdp3 = sdpbound_γ(s3, optimizer=optim, verbose=4)
γsos3 = sosbound_γ(s3, 2, optimizer=optim, verbose=4)

@testset "SDP program" begin
    @test γsdp1 == gammasdp1
    @test γsos1 == gammasos1
    @test (γsdp1 ≥ 2 && γsdp1 ≤ 2.1)
    @test (γsdp2 ≥ 1.1 && γsdp2 ≤ 1.2)
    @test (γsdp3 ≥ 3.1 && γsdp3 ≤ 3.2)
end

@testset "SOS program" begin
    @test (γsos1 ≥ 2 && γsos1 ≤ 2.1)
    @test (γsos2 ≥ 1.1 && γsos2 ≤ 1.2)
    @test (γsos3 ≥ 3.1 && γsos2 ≤ 3.2)
end

@testset "(C)JSR calculation" begin
    @test jsr(s1) == jsr(Σ1)
    @test cjsr(s1) == cjsr(Σ1, G1)
end


@inline function fit_cubic(; x0, xf, y0, yd0, yf, ydf)
    num_coeffs = Val(4)
    A = hcat(
        SVector(SUP.coefficient_gradient(x0, num_coeffs, Val(0))),
        SVector(SUP.coefficient_gradient(x0, num_coeffs, Val(1))),
        SVector(SUP.coefficient_gradient(xf, num_coeffs, Val(0))),
        SVector(SUP.coefficient_gradient(xf, num_coeffs, Val(1))),
    ) |> transpose
    b = SVector(tuple(y0, yd0, yf, ydf))
    Polynomial(Tuple(A \ b))
end

@inline function fit_quintic(; x0, xf, y0, yd0, ydd0, yf, ydf, yddf)
    num_coeffs = Val(6)
    A = hcat(
        SVector(SUP.coefficient_gradient(x0, num_coeffs, Val(0))),
        SVector(SUP.coefficient_gradient(x0, num_coeffs, Val(1))),
        SVector(SUP.coefficient_gradient(x0, num_coeffs, Val(2))),
        SVector(SUP.coefficient_gradient(xf, num_coeffs, Val(0))),
        SVector(SUP.coefficient_gradient(xf, num_coeffs, Val(1))),
        SVector(SUP.coefficient_gradient(xf, num_coeffs, Val(2))),
    ) |> transpose
    b = SVector(tuple(y0, yd0, ydd0, yf, ydf, yddf))
    Polynomial(Tuple(A \ b))
end


#############################################################################
# trace_mpower(A, t, C) returns tr(C*A^t) where A and C are positive definite
# matrices and C is constant and t ∈ [-1, 2].
#
# When t ∈ [0,1], trace_mpower(A, t, C) is concave in A (for fixed
# positive semidefinite matrix C) and convex for t ∈ [-1,0] or [1,2].
#
# All expressions and atoms are subtypes of AbstractExpr.
# Please read expressions.jl first.
#
#REFERENCE
#   Ported from CVXQUAD which is based on the paper: "Lieb's concavity
#   theorem, matrix geometric means and semidefinite optimization" by Hamza
#   Fawzi and James Saunderson (arXiv:1512.03401)
#############################################################################

struct TraceMpower <: AbstractExpr
    head::Symbol
    id_hash::UInt64
    children::Tuple{AbstractExpr,}
    size::Tuple{Int, Int}
    C::AbstractMatrix
    t::Rational

    function TraceMpower(A::AbstractExpr, t::Rational, C::AbstractMatrix)
        children = (A,)
        if size(A) != size(C)
            throw(DimensionMismatch("A and C must be the same size"))
        end
        n = size(A)[1]
        if size(A) != (n, n)
            throw(DimensionMismatch("A and C must be square"))
        end
        if norm(C - C') > 1e-6
            throw(DomainError(C, "C must be Hermitian"))
        end
        if any(eigvals(Hermitian(C)) .< -1e-6)
            throw(DomainError(C, "C must be positive semidefinite"))
        end
        if t < -1 || t > 2
            throw(DomainError(t, "t must be in the range [-1, 2]"))
        end
        return new(:trace_mpower, hash(children), children, (1, 1), C, t)
    end
end

function sign(atom::TraceMpower)
    return NoSign()
end

function monotonicity(atom::TraceMpower)
    return (NoMonotonicity(),)
end

function curvature(atom::TraceMpower)
    t = atom.t
    if t >= 0 && t <= 1
        return ConcaveVexity()
    else
        return ConvexVexity()
    end
end

function evaluate(atom::TraceMpower)
    A = evaluate(atom.children[1])
    return trace_mpower(A, atom.t, atom.C)
end

const MatrixOrConstant = Union{AbstractMatrix, Constant}

function trace_mpower(A::AbstractExpr, t::Rational, C::MatrixOrConstant)
    #println("trace_mpower general case")
    return TraceMpower(A, t, evaluate(C))
end

function trace_mpower(A::MatrixOrConstant, t::Rational, C::MatrixOrConstant)
    #println("trace_mpower constant A")
    return tr(C * A^t)
end

trace_mpower(A::AbstractExprOrValue, t::Integer, C::MatrixOrConstant) = trace_mpower(A, t//1, C)

function conic_form!(atom::TraceMpower, unique_conic_forms)
    if !has_conic_form(unique_conic_forms, atom)
        A = atom.children[1]
        C = atom.C
        t = atom.t
        eye = Matrix(1.0*I, size(A))

        is_complex = sign(A) == ComplexSign()
        if is_complex
            make_temporary = () -> HermitianSemidefinite(size(A)[1])
        else
            make_temporary = () -> Semidefinite(size(A)[1])
        end

        T = make_temporary()

        if t >= 0 && t <= 1
            conic_form!(T in GeomMeanHypoCone(eye, A, t, false), unique_conic_forms)
            # It's already a real mathematically, but need to make it a real type.
            u = real(tr(C * T))
            cache_conic_form!(unique_conic_forms, atom, maximize(u))
        else
            conic_form!(T in GeomMeanEpiCone(eye, A, t, false), unique_conic_forms)
            # It's already a real mathematically, but need to make it a real type.
            u = real(tr(C * T))
            cache_conic_form!(unique_conic_forms, atom, minimize(u))
        end
    end
    return get_conic_form(unique_conic_forms, atom)
end


using Plots, ApproxFun

#The following samples eigenvalues of an n = 2 Gaussian Unitary Ensemble:

ff = (x,y)->(x-y)^2*exp(-x^2/2-y^2/2)
f = Fun(ff,Domain(-4..4)^2)
r=ApproxFun.sample(f,5000)

#We can compare the histogram to the 1-point correlation
plot(sum(f,1)/sum(f))
histogram!(vec(r);normed=true)

#We can compare the histograms of x with the GUE:
using RandomMatrices

histogram(vcat([sqrt(2)eigvalrand(GaussianHermite(2),2) for k=1:10000]...);normed=true)
plot!(sum(f,1)/sum(f))


using Distributions

#Compute Mauchly's statistic (valid under assumption of multinormality)
#Mauchly, 1940; Kendall and Stuart, 1968
#n is the number of data points (samples)
#This returns both the value of the test statistic and the expected distribution to test against
function MauchlySphericityTestStatistic{T<:Real}(PopulationCovariance::AbstractMatrix{T}, SampleCovariance::AbstractMatrix{T}, n::Integer)
    p = size(SampleCovariance)[1]
    C = inv(PopulationCovariance) * SampleCovariance
    l = (det(C)/(trace(C)/p)^p)
    x = -n*log(l)
    f = p*(p+1)/2-1
    return x, Chisq(f)
end

#Johnstone's variant of the sphericity test
#n, p>= 10 recommended
#Johnstone (2001)
function JohnstoneSphericityTestStatistic{T<:Real}(PopulationCovariance::AbstractMatrix{T}, SampleCovariance::AbstractMatrix{T}, n::Integer)
    C = inv(PopulationCovariance) * SampleCovariance
    v = max(eigvals(C))
    mu=(sqrt(n-1)+sqrt(p))^2
    sigma=sqrt(mu)*(1/sqrt(n-1)+1/sqrt(p))^(1/3)
    (n*l-mu)/sigma #To be tested against Tracy-Widom with beta=1
end


# ------------------------------------------------------------------
# Licensed under the MIT License. See LICENCE in the project root.
# ------------------------------------------------------------------

"""
  log_gpd_pdf(arg, μ, σ, ξ)

Log density function of the generalized Pareto distribution,
with an expansion with ξ near zero.
"""
function log_gpd_pdf(_x, μ, σ, ξ)
  x = (_x-μ)/σ
  expn = if abs(ξ) < 1e-5
    # expansion for ξ near zero.
    -x*(ξ+1) + (x^2)*ξ*(ξ+1)/2 - (x^3)*(ξ^2)*(ξ+1)/3 + (x^4)*(ξ^3)*(ξ+1)/4
  else
    (-(ξ+1)/ξ)*log(max(0, 1 + x*ξ))
  end
  expn - log(σ)
end

"""
  log_gev_pdf(arg, μ, σ, ξ)

Log density function of the generalized extreme value distribution,
with an expansion with ξ near zero.
"""
function log_gev_pdf(_x, μ, σ, ξ)
  x = (_x-μ)/σ
  tx = if abs(ξ) < 1e-10 # this cutoff is _not_ the same as for log_gpd_pdf.
    # expansion near zero.
    -x + (x^2)*ξ/2 - (x^3)*(ξ^2)/3 + (x^4)*(ξ^3)/4
  else
    (-1/ξ)*log(max(0, 1 + x*ξ))
  end
  (ξ+1)*tx - exp(tx) - log(σ)
end

"""
    fit_mle(gev, bm)

Fit generalized extreme value distribution `gev` to block maxima
`bm` with constrained maximum likelihood estimation.
"""
function fit_mle(::Type{GeneralizedExtremeValue}, bm::BlockMaxima;
                 init=(μ=0.0, σ=1.0, ξ=0.05))
  # retrieve maxima values
  x = collect(bm)
  n = length(x)

  # set up the problem
  mle = Model(optimizer_with_attributes(Ipopt.Optimizer, "print_level"=>0,
                                        "sb"=>"yes", "max_iter"=>250))
  @variable(mle, μ, start=init.μ)
  @variable(mle, σ, start=init.σ)
  @variable(mle, ξ, start=init.ξ)
  JuMP.register(mle, :log_gev_pdf, 4, log_gev_pdf, autodiff=true)
  @NLobjective(mle, Max, sum(log_gev_pdf(z, μ, σ, ξ) for z in x))
  @NLconstraint(mle, [i=1:n], 1 + ξ*(x[i]-μ)/σ ≥ 0.0)
  @constraint(mle, σ ≥ 1e-10)
  @constraint(mle, ξ ≥ -1/2)

  # attempt to solve
  optimize!(mle)

  # retrieve solver status
  status = termination_status(mle)

  # acceptable statuses
  OK = (MOI.OPTIMAL, MOI.LOCALLY_SOLVED)

  if status ∈ OK
    GeneralizedExtremeValue(value(μ), value(σ), value(ξ))
  else
    error("could not fit distribution. Exited with status $status.")
  end
end

"""
    fit_mle(gp, pm)

Fit generalized Pareto distribution `gp` to peak over threshold
maxima `pm` with constrained maximum likelihood estimation.
"""
function fit_mle(::Type{GeneralizedPareto}, pm::PeakOverThreshold;
                 init=(σ=1.0, ξ=0.05))
  # retrieve maxima values
  x = collect(pm)
  n = length(x)

  # set up the problem
  mle = Model(optimizer_with_attributes(Ipopt.Optimizer, "print_level"=>0,
                                        "sb"=>"yes", "max_iter"=>250))
  @variable(mle, σ, start=init.σ)
  @variable(mle, ξ, start=init.ξ)
  JuMP.register(mle, :log_gpd_pdf, 4, log_gpd_pdf, autodiff=true)
  @NLobjective(mle, Max, sum(log_gpd_pdf(z, pm.u, σ, ξ) for z in x))
  @NLconstraint(mle, [i=1:n], 1 + ξ*(x[i]-pm.u)/σ ≥ 0.0)
  @constraint(mle, σ ≥ 1e-10)
  @constraint(mle, ξ ≥ -1/2)

  # attempt to solve both cases
  optimize!(mle)

  # retrieve solver status
  status  = termination_status(mle)

  # acceptable statuses
  OK = (MOI.OPTIMAL, MOI.LOCALLY_SOLVED)

  if status ∈ OK
    GeneralizedPareto(pm.u, value(σ), value(ξ))
  else
    error("could not fit distribution. Exited with status $status.")
  end
end


@testset "Testing operations and Functions" begin

    layout(3,1,"Conformal")
    u=1.0*id+2.0*e1-e0
    v=2.0*id+1.0*e1+e∞
    # sum
    @test u+v == 3.0*id + 3.0*e1 + 1.5*ep + 0.5*en

    # difference
    @test u-v ==-1.0*id + 1.0*e1 - 0.5*ep - 1.5*en

    # scalar
    @test 1.5*v ==3.0*id + 1.5*e1 + 1.5*ep + 1.5*en
    @test scalar(1.5,v) == 3.0*id + 1.5*e1 + 1.5*ep + 1.5*en

    # reverse
    @test reverse(v) == 2.0*id + 1.0*e1 + 1.0*ep + 1.0*en

    # geometric
    @test u∘v == 5.0*id + 5.0*e1 + 2.0*ep + 1.5*e1p + 2.5*e1n + 1.0*epn
    @test geometric(u,v) == 5.0*id + 5.0*e1 + 2.0*ep + 1.5*e1p + 2.5*e1n + 1.0*epn

    # inner
    @test u⋅v == 3.0*id
    @test inner(u,v) == 3.0*id

    # outer
    @test u^v == 2.0*id + 5.0*e1 + 2.0*ep + 1.5*e1p + 2.5*e1n + 1.0*epn
    @test outer(u,v) == 2.0*id + 5.0*e1 + 2.0*ep + 1.5*e1p + 2.5*e1n + 1.0*epn

    # gradeprojection
    @test gradeprojection(u,1) == 2.0*e1 + 0.5*ep - 0.5*en
    @test gradeprojection(v,1) == 1.0*e1 + 1.0*ep + 1.0*en

    # reverse
    @test reverse(u) == 1.0*id + 2.0*e1 + 0.5*ep - 0.5*en
    @test reverse(v) == 2.0*id + 1.0*e1 + 1.0*ep + 1.0*en

    # dual
    @test dual(u) ==  0.5*e12p - 0.5*e12n - 2.0*e2pn - 1.0*e12pn
    @test dual(v) == - 1.0*e12p - 1.0*e12n - 1.0*e2pn - 2.0*e12pn

    # inverse
    @test inverse(u)∘u ≈ 1.0*id
    @test inverse(v)∘v ≈ 1.0*id

end


include("./cmd.jl")
include("./Potentials.jl")

using Random
using Plots

# Bilinear problem:
f(x, y) = 2 * (x .- 0.00)' * (y .- 0.00) - sum((y .- 1.0).^2)
g(x, y) = -f(x, y)

m = 1
n = 1

α = 4.0

ψ_x = squared_distance(α)
ψ_y = squared_distance(α)
x = ones(m) * α
y = ones(n) * α

# ψ_x = shannon_entropy(α)
# ψ_y = shannon_entropy(α)
# x = zeros(m) * α
# y = zeros(n) * α

cmd = CMD(x, ψ_x, f, y, ψ_y, g)

max_iter = 100
xHist = zeros(m, max_iter + 1)
yHist = zeros(n, max_iter + 1)
for k = 1 : 100
  xHist[:, k], yHist[:, k] = iterate_primal(cmd)
  step!(cmd)

  cmd.x̄ .= max.(cmd.x̄, 0.0)
  cmd.ȳ .= max.(cmd.ȳ, 0.0)
end
xHist[:, end], yHist[:, end]= iterate_primal(cmd)


function MakeModel(A,b,C,XLB,XUB,ObjInd;Cuts = [])
    m,n = size(A);
    ObjNum = size(C)[1];
    MyModel = Model();
    @variable(MyModel, XLB[i] <= X[i=1:n] <= XUB[i])
    @objective(MyModel, Min, sum(C[ObjInd,i]*X[i] for i=1:n))
    for i = 1:m
        @constraint(MyModel,sum(A[i,j]*X[j] for j = 1:n) >= b[i] )
    end
    # for c = 1:length(Cuts)
    #     @constraint(MyModel,sum(Cuts[c].Coeff[i]*(sum(C[i,j]*X[j] for j = 1:n )+d[i])  for i = 1:ObjNum) >= Cuts[c].RHS)
    # end
    return MyModel;
end


module GaussianMixture
    using DataModel
    using MLUtilities

    export GaussianMixtureModel

    type GaussianMixtureModel <: AbstractDataModel
        #currently just 1D
        mu::Array{Float64}
        var::Array{Float64}
        weights::Array{Float64}
    end

    normldf(x,mu,var) =  -.5*log(2.0*pi*var) -.5*((x-mu).*(x-mu))/var
    normpdf(x,mu,var) = exp(normldf(x,mu,var))

    function DataModel.getgrad(dm::GaussianMixtureModel)
        function gradient(x)
            z = exp(logsumexp([log(dm.weights[i])+normldf(x,dm.mu[i],dm.var[i]) for i in 1:length(dm.mu)]...))
            d = sum([dm.weights[i].*normpdf(x,dm.mu[i],dm.var[i]).*(dm.mu[i]-x)./dm.var[i] for i in 1:length(dm.mu)])
            return d./z
        end
        return gradient
    end

    function DataModel.getllik(dm::GaussianMixtureModel)
        function logdensity(x)
            logsumexp([log(dm.weights[i])+normldf(x,dm.mu[i],dm.var[i]) for i in 1:length(dm.mu)]...)[1]
        end
        return logdensity
    end
end


function poly(x, c)
    out = x .* 0. .+ c[1]
    for i in 2:length(c)
        out .+= c[i] .* x.^(i-1)
    end
    return out
end

function ccm_unred(wave, ebv; R_V=3.1)
    x = 10000 ./ wave    # Convert to inverse microns
    npts = length( x )
    a = fill(0., npts)
    b = fill(0., npts)

    good = findall((x .>  0.3)  .&   (x .< 1.1)) #Infrared
    a[good] =  0.574 .* x[good].^(1.61)
    b[good] = -0.527 .* x[good].^(1.61)

    good = findall((x .>= 1.1)  .&   (x .< 3.3)) #Optical/NIR
    #Use new constants from O'Donnell (1994)
    y = x[good] .- 1.82
#     c1 = [ 1. , 0.17699, -0.50447, -0.02427,  0.72085,      #Original
#                 0.01979, -0.77530,  0.32999 ]               #coefficients
#     c2 = [ 0.,  1.41338,  2.28305,  1.07233, -5.38434,      #from CCM89
#                -0.62251,  5.30260, -2.09002 ]
      c1 = [ 1. , 0.104,   -0.609,    0.701,  1.137,         #New coefficients
                 -1.718,   -0.827,    1.647, -0.505 ]        #from O'Donnell
      c2 = [ 0.,  1.952,    2.908,   -3.989, -7.985,         #(1994)
                 11.102,    5.491,  -10.805,  3.347 ]
     a[good] = poly(y, c1)
     b[good] = poly(y, c2)

    good = findall((x .>= 3.3)  .&   (x .< 8.0)) #Mid-UV
    y = x[good]
    F_a = fill(0., length(good))
    F_b = fill(0., length(good))
    good1 = findall(y .> 5.9)
    y1 = y[good1] .- 5.9
    F_a[good1] = -0.04473 .* y1.^2 .- 0.009779 .* y1.^3
    F_b[good1] =   0.2130 .* y1.^2 .+   0.1207 .* y1.^3

    a[good] =  1.752 .- 0.316.*y .- (0.104 ./ ( (y.-4.67).^2 .+ 0.341 )) .+ F_a
    b[good] = -3.090 .+ 1.825.*y .+ (1.206 ./ ( (y.-4.62).^2 .+ 0.263 )) .+ F_b

    good = findall((x .>= 8.0)  .&   (x .< 11.)) #Far-UV
    y = x[good] .- 8.
    c1 = [ -1.073, -0.628,  0.137, -0.070 ]
    c2 = [ 13.670,  4.257, -0.420,  0.374 ]
    a[good] = poly(y, c1)
    b[good] = poly(y, c2)

    # Now apply extinction correction to input flux vector
    A_V = R_V * ebv
    A_lambda = A_V .* (a .+ b./R_V)
    return 10 .^(0.4.*A_lambda)
 end


import JuMP as J
using Ipopt

# Dados de entrada

Sy=350.0;
r1=0.4;
E=2000.0;
P=200.0;
B=1.0;

f(r2,H) = pi*(r2^2)*(H^2+B^2)^(1/2)

g1(r2,H) = P*B/(pi*H*(r1^2)*Sy) - 1;
g2(r2,H) = P*((B^2+H^2)^(1/2))/(H*pi*r2^2*Sy)-1;
g3(r2,H) = 4*P*(B^2+H^2)^(3/2)/(H*pi^3*r2^4*E)-1;

model = J.Model(Ipopt.Optimizer)

J.set_optimizer_attribute(model, "print_level",0)

#registrando o número de variáveis

J.register(model, :g1, 2, g1; autodiff = true)
J.register(model, :g2, 2, g2; autodiff = true)
J.register(model, :g3, 2, g3; autodiff = true)
J.register(model, :f, 2, f; autodiff = true)

J.@variable(model, 0.4 <= r2 <= 0.8);
J.@variable(model, 1.0 <= H <= 6.0);

J.@NLobjective(model, Min, f(r2,H));

J.@NLconstraint(model, g1(r2,H) <= 0);
J.@NLconstraint(model, g2(r2,H) <= 0);
J.@NLconstraint(model, g3(r2,H) <= 0);

println(model)
J.optimize!(model)

println("Termination status: ", J.termination_status(model))
println("Primal status: ", J.primal_status(model))
println("f*: ", J.objective_value(model))
println("r2* : ", J.value(r2), ", H* : ", J.value(H))

a = 4;


function state_transition(speed, yaw_rate, time, pose)
  theta = pose[3]

  # yaw rate is almost zero or not
  if abs(yaw_rate) < 1e-10
      return pose + [speed*cos(theta)*time,
                     speed*sin(theta)*time,
                     yaw_rate*time]
  else
      return pose + [speed/yaw_rate*(sin(theta+yaw_rate*time)-sin(theta)),
                     speed/yaw_rate*(-cos(theta+yaw_rate*time)+cos(theta)),
                     yaw_rate*time]
  end
end


function energy(v, S)
    1/2*(dot(v,inv(S)*v) + log(det(S)))
end

#to do: add prior
function energy(estimates)
    foldxl(+, estimates |> Map(x -> x[end-1:end]) |> Map(x -> energy(x...)))
end


# solve the minimum length Hamiltonian problem; in this algorithm, the
# initial and final stops are *fixed* and the feasible region consists of alternating
# intermediate stops
struct MIP_IF <: HamiltonianSolver end

min_path(T, stops, ::MIP_IF) = _min_length_path_mip(T, stops)

function _min_length_path_mip(T::AbstractMatrix, stops::Vector{<:AbstractStop}; optimizer)
     stop_names = [name(s) for s in stops]
     _min_length_path_mip(T, stop_names, optimizer=optimizer)
end

# solve the minimum length Hamiltonian problem using a MIP formulation
# *where the first and last stops are fixed*
function _min_length_path_mip(T::AbstractMatrix, stops::Vector{<:String}; optimizer)
    M = view(T, stops, stops)

    model = Model(optimizer)

    # number of stops
    N = length(stops)

    # binary decision variables: 1 if x_{st} stop s goes to stop t and 0 otherwise
    @variable(model, x[1:N, 1:N], Bin)

    # cost function
    @objective(model, Min, sum(x[s, t] * M[s, t] for s=1:N, t=1:N))

    for s = 1:N  # loop over source stop
        # no edge reaches the initial stop
        @constraint(model, x[s, 1] == 0)

        # no self-loops
        @constraint(model, x[s, s] == 0)

        # each stop arrives to one and only one stop (unless the last one)
        if s < N
            @constraint(model, sum(x[s, t] for t in 2:N) == 1)
        end
    end

    # can't jump directly from first to last stop
    @constraint(model, x[1, N] == 0)

    for t = 1:N  # loop over target stop
        # no edge after the final stop
        @constraint(model, x[N, t] == 0)

        # each stop receives one and only one edge (unless the first stop)
        if t > 1
           @constraint(model, sum(x[s, t] for s in 1:N) == 1)
        end
    end

    # no repetitions (not required in ppple)
    for  s= 1:N, t = 1:N
        @constraint(model, x[s, t] + x[t, s] <= 1)
    end

    optimize!(model)

    while !is_mip_solved(model, x)
        optimize!(model)
    end

    minvalue = JuMP.objective_value(model)
    Mpath = JuMP.getvalue.(x)

    result = similar(stops)
    result[1] = stops[1]
    row = 1
    for i in 1:N-1
        idx = findfirst(==(1,), view(Mpath, row, :))
        @assert !isnothing(idx)
        result[i+1] = stops[idx]
        row = idx
    end
    return minvalue, result
end

# Check whether the Hamiltonian problem is solved,
# which requires that there is a unique chain of length N
# if there is no such chain => we add it as a constraint to the problem
#
# For instance, if there are 5 stops we may find a chain of length 3:
#
# 3 -> 4 -> 3
# 1 -> 2 -> 5 : x[1, 2] + x[2, 5] = 2
# The idea is to add a new constraint that excludes the chain 1 -> 2 -> 5
function is_mip_solved(model, x)
    minvalue = JuMP.objective_value(model)
    Mpath = JuMP.getvalue.(x)

    # check if the problem is solved <=> the path starting at 1 covers all stops
    N = size(Mpath, 1)
    chain_idx = Vector{Int}()
    push!(chain_idx, 1)
    while true
        idx = findfirst(==(1,), view(Mpath, chain_idx[end], :))
        push!(chain_idx, idx)
        idx == N && break
    end

    if length(chain_idx) < N
        @constraint(model, sum(x[chain_idx, chain_idx]) <= length(chain_idx) - 2)
        return false
    end
    return true
end

# solve the minimum length Hamiltonian problem; in this algorithm, the
# initial stop is *fixed*, and optimization is based on the remaining m-1 stops
struct MIP_I <: HamiltonianSolver end

# TODO add MIP_I naive implementation is to iteratively call MIP_IF for each
# of the remaining n - 1 stops


function freqt!(wc::AbstractVector, c::AbstractVector, α;
                prev=Array(eltype(wc), length(wc)))
    fill!(wc, zero(eltype(wc)))
    dst_order = length(wc) - 1

    m1 = length(c)-1
    for i in -m1:0
        copy!(prev, wc)
        if dst_order >= 0
            @inbounds wc[1] = c[-i+1] + α*prev[1]
        end
        if dst_order >= 1
            wc[2] = (1.0-α*α)*prev[1] + α*prev[2]
        end
        for m=3:dst_order+1
            @inbounds wc[m] = prev[m-1] + α*(prev[m] - wc[m-1])
        end
    end

    wc
end

function freqt(c::AbstractVector, order=25, α=0.35)
    wc = Array(eltype(c), order+1)
    freqt!(wc, c, α)
end

function freqt{T<:MelGeneralizedCepstrum}(state::SpectralParamState{T},
                                          order=param_order(paramdef(state)),
                                          α²=allpass_alpha(paramdef(state)))
    assert_not_ready_to_filt(state)

    def = paramdef(state)
    α¹ = allpass_alpha(def)
    α = (α²-α¹) / (1.0-α²*α¹)
    γ = glog_gamma(def)
    data = rawdata(state)
    newdef = MelGeneralizedCepstrum(order, α², γ)
    SpectralParamState(newdef, freqt(data, order, α),
                       has_loggain(state), gain_normalized(state))
end


@symbol_func function P_LH(cur_reactor::AbstractReactor)
  cur_P = 0.0976

  cur_P *= cur_reactor.n_bar ^ 0.72

  cur_P *= cur_reactor.B_0 ^ 0.80

  cur_P *= surface_area(cur_reactor) ^ 0.94

  cur_P /= cur_reactor.A

  cur_P
end


module Discrete
using Printf
using LinearAlgebra
using ..Spectral
using ..Interp
include("kernel.jl")

function ωQR(kernel, rtol, print::Bool = true)
    # print && println(τ.grid[end], ", ", τ.panel[end])
    # print && println(ω.grid[end], ", ", ω.panel[end])
    ################# find the rank ##############################
    """
    For a given index k, decompose R=[R11, R12; 0, R22] where R11 is a k×k matrix.
    If R11 is well-conditioned, then
    σᵢ(R11) ≤ σᵢ(kernel) for 1≤i≤k, and
    σⱼ(kernel) ≤ σⱼ₋ₖ(R22) for k+1≤j≤N
    See Page 487 of the book: Golub, G.H. and Van Loan, C.F., 2013. Matrix computations. 4th. Johns Hopkins.
    Thus, the effective rank is defined as the minimal k that satisfy rtol≤ σ₁(R22)/σ₁(kernel)
    """
    Nτ, Nω = size(kernel)

    u, σ, v = svd(kernel)
    rank, err = 1, 0.0
    for (si, s) in enumerate(σ)
        # println(si, " => ", s / σ[1])
        if s / σ[1] < rtol
            rank = si - 1
            err = s[1] / σ[1]
            break
        end
    end
    print && println("Kernel ϵ-rank = ", rank, ", rtol ≈ ", err)

    Q, R, p = qr(kernel, Val(true)) # julia qr has a strange design, Val(true) will do a pivot QR
    # size(R) == (Nτ, Nω) if Nω>Nτ
    # or size(R) == (Nω, Nω) if Nω<Nτ

    for idx = rank:min(Nτ, Nω)
        if Nω > Nτ
            R22 = R[idx:Nτ, idx:Nω]
        else
            R22 = R[idx:Nω, idx:Nω]
        end
        u2, s2, v2 = svd(R22)
        # println(idx, " => ", s2[1] / σ[1])
        if s2[1] / σ[1] < rtol
            rank = idx
            err = s2[1] / σ[1]
            break
        end
    end
    print && println("DLR rank      = ", rank, ", rtol ≈ ", err)

    # @assert err ≈ 4.58983288255442e-13

    return p[1:rank]
end

function τnQR(kernel, rank, print::Bool = true)
    ###########  dlr grid for τ  ###################
    print && println("Calculating τ and ωn grid ...")
    @assert rank == size(kernel)[2] #the ω dimension of the τkernel should be the effective rank
    τnqr = qr(transpose(kernel), Val(true)) # julia qr has a strange, Val(true) will do a pivot QR
    return τnqr.p[1:rank]
end

function buildωn(dlrGrid, print::Bool = true)
    ###########  dlr grid for ωn  ###################
    print && println("Calculating ωn grid ...")
    symmetry = dlrGrid.symmetry
    rank = dlrGrid.size

    if symmetry == :ph || symmetry == :pha
        Nωn = Int(ceil(Λ)) * 2 # expect Nω ~ para.Λ/2π, drop 2π on the safe side
        ωnkernel = zeros(Float64, (rank, Nωn + 1))
        ωnGrid = [w for w = 0:Nωn]
        # fermionic Matsubara frequency ωn=(2n+1)π for type==:acorr
        # bosonic Matsubara frequency ωn=2nπ for type==:corr
    else
        Nωn = Int(ceil(Λ)) * 2 # expect Nω ~ para.Λ/2π, drop 2π on the safe side
        ωnkernel = zeros(Complex{Float64}, (rank, 2Nωn + 1))
        ωnGrid = [w for w = -Nωn:Nωn] # fermionic Matsubara frequency ωn=(2n+1)π
    end

    for (ni, n) in enumerate(ωnGrid)
        for r = 1:rank
            ωnkernel[r, ni] = Spectral.kernelΩ(type, n, ωGridDLR[r])
        end
    end
    nqr = qr(ωnkernel, Val(true)) # julia qr has a strange, Val(true) will do a pivot QR
    nGridDLR = sort(ωnGrid[nqr.p[1:rank]])
    return nGridDLR, nqr.p[1:rank]
end

"""
function build(dlrGrid, print::Bool = true)
    Construct discrete Lehmann representation

#Arguments:
- `dlrGrid`: struct that contains the information to construct the DLR grid. The following entries are required:
   Λ: the dimensionless scale β*Euv, rtol: the required relative accuracy, isFermi: fermionic or bosonic, symmetry: particle-hole symmetry/antisymmetry or none
- `print`: print the internal information or not
"""
function build(dlrGrid, print::Bool = true)
    degree = 24 # number of Chebyshev nodes in each panel
    Λ = dlrGrid.Λ
    rtol = dlrGrid.rtol
    τ = τChebyGrid(dlrGrid, degree, print)
    ω = ωChebyGrid(dlrGrid, degree, print)
    print && println("τ grid size = $(τ.ngrid)")
    print && println("ω grid size = $(ω.ngrid)")

    kernel = preciseKernelT(dlrGrid, τ, ω, print)
    testInterpolation(dlrGrid, τ, ω, kernel, print)

    ωIndex = ωQR(kernel, rtol, print)
    rank = length(ωIndex)
    ωGrid = sort(ω.grid[ωIndex])

    τIndex = τnQR(kernel[:, ωIndex], rank, print)
    τGrid = sort(τ.grid[τIndex])

    nFineGrid, nFermiKernel, nBoseKernel = preciseKernelΩn(dlrGrid, ωGrid, print)

    nFermiIndex = τnQR(nFermiKernel, rank, print)
    nFermiGrid = sort(nFineGrid[nFermiIndex])

    nBoseIndex = τnQR(nBoseKernel, rank, print)
    nBoseGrid = sort(nFineGrid[nBoseIndex])

    ########### output  ############################
    print && @printf("%5s  %32s  %32s  %11s  %11s\n", "index", "real freq", "tau", "fermi ωn", "bose ωn")
    for r = 1:rank
        print && @printf("%5i  %32.17g  %32.17g  %16i %16i\n", r, ωGrid[r], τGrid[r], nFermiGrid[r], nBoseGrid[r])
    end

    # dlr = Dict([(:ω, ωGrid), (:τ, τGrid), (:ωn, nFermiGrid)])
    return ωGrid, τGrid, nFermiGrid, nBoseGrid
end
end


using MinFEM
using LinearAlgebra

function solve_gradient(file_name::String)

  mesh = import_mesh(file_name)

  G = asmGradient(mesh, qdim=2)

  f(x) = [x[1]+x[2]; x[1]-x[2]]
  s = evaluateMeshFunction(mesh, f, qdim=2)

  y = G*s;

  return y

end

@test norm(abs.(solve_gradient("test_v4.msh")).-1.0, Inf) < 1e-10


using EndpointRanges
using Base.Test

r1 = -3:7
r2 = 2:5
@test ibegin(r1) == -3
@test ibegin(r2) == 2
@test iend(r1) == 7
@test iend(r2) == 5

@test (ibegin+3)(r1) == 0
@test (ibegin+2)(r2) == 4
@test (iend+3)(r1) == 10
@test (iend-2)(r2) == 3

for a in (1:20,
          range(1,2,20),      # StepRange
          1.0:2.0:39.0,       # FloatRange
          linspace(1,100,20), # LinSpace
          collect(1:20),
          view(0:21, 2:21))
    @test a[ibegin:20] == a[1:20]
    @test a[1:iend]    == a[1:20]
    @test a[ibegin+2:iend-3] == a[3:17]
    @test a[ibegin+2:iend÷2] == a[3:10]
    @test a[2*ibegin+1:iend÷2] == a[3:10]
    @test a[1:(ibegin+iend)÷2] == a[1:10]
    @test view(a, 1:(ibegin+iend)÷2) == view(a, 1:10)
    @test view(a, ibegin+2:iend-3) == view(a, 3:17)
end


#=
Copyright (c) 2015, Intel Corporation
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
- Redistributions of source code must retain the above copyright notice,
  this list of conditions and the following disclaimer.
- Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
THE POSSIBILITY OF SUCH DAMAGE.
=#

using ParallelAccelerator
using DocOpt
using Images

@acc function harrisCornerDetect(Iin)
  (w, h) = size(Iin)

  Ix  = Array(Float32, w, h)
  Iy  = Array(Float32, w, h)
  Sxx = Array(Float32, w, h)
  Syy = Array(Float32, w, h)
  Sxy = Array(Float32, w, h)

  runStencil(Ix, Iin, :oob_dst_zero) do b, a
    b[0,0] = ((a[-1,-1] * -1.0f0) + (a[-1,0] * -2.0f0) + (a[-1,1] * -1.0f0) + a[1,-1] + (a[1,0] * 2.0f0) + a[1,1]) / 12.0f0
  end
  runStencil(Iy, Iin, :oob_dst_zero) do b, a
    b[0,0] = ((a[-1,-1] * -1.0f0) + (a[0,-1] * -2.0f0) + (a[1,-1] * -1.0f0) + a[-1,1] + (a[0,1] * 2.0f0) + a[1,1]) / 12.0f0
  end

  Ixx = Ix .* Ix
  Iyy = Iy .* Iy
  Ixy = Ix .* Iy

  runStencil(Sxx, Ixx, :oob_dst_zero) do b, a
    b[0,0] = (a[-1,-1] + a[-1,0] + a[-1,1] + a[0,-1] + a[0,0] + a[0,1] + a[1,-1] + a[1,0] + a[1,1])
  end
  runStencil(Syy, Iyy, :oob_dst_zero) do b, a
    b[0,0] = (a[-1,-1] + a[-1,0] + a[-1,1] + a[0,-1] + a[0,0] + a[0,1] + a[1,-1] + a[1,0] + a[1,1])
  end
  runStencil(Sxy, Ixy, :oob_dst_zero) do b, a
    b[0,0] = (a[-1,-1] + a[-1,0] + a[-1,1] + a[0,-1] + a[0,0] + a[0,1] + a[1,-1] + a[1,0] + a[1,1])
  end

  det    = (Sxx .* Syy) .- (Sxy .* Sxy)
  trace  = Sxx .+ Syy
  harris = det .- (0.04f0 .* trace .* trace)

  return harris
end

function main()
    doc = """harris.jl

Harris corner detection.

Usage:
  harris.jl -h | --help
  harris.jl [--img-file=<img-file>]

Options:
  -h --help              Show this screen.
  --img-file=<img-file>  Specify a path to an input (grayscale) image file [default: ../example.jpg].
"""
    arguments = docopt(doc)

    dir = dirname(@__FILE__)
    img_file = joinpath(dir, arguments["--img-file"])

    (fname, ext) = splitext(img_file)
    out_file = string(fname, "-corners", ".jpg")

    println("input file = ", img_file)
    println("output file = ", out_file)

    function harris(input_fname, output_fname)
        local img = convert(Array{Float32, 2}, load(input_fname))
        tic()
        res = harrisCornerDetect(Matrix{Float32}(0, 0))
        println("SELFPRIMED ", toq())

        tic()
        res = harrisCornerDetect(img)
        selftimed = toq()
        res = map((x,y) -> x > 0.001f0 ? 1.0f0 : (y / 2.0f0), res, img)
        save(output_fname, res)
        println("checksum: ", sum(res))
        println("SELFTIMED ", selftimed)
    end

    harris(img_file, out_file)

end

main()


@testset "legendre correct values edge,origin" begin
    # legendre == jacobi(a,b=0,0); same function body
    for n in 1:10
        # design of jacobi polynomials is such that edge = 1
        @test legendre(n, [1.]) ≈ [1.]
        correct = [-1.]
        if iseven(n)
            correct = [1.]
        end
        @test legendre(n, [-1.]) ≈ correct
        if !iseven(n)
            @test legendre(n, [0.]) ≈ [0.]
        end
    end
end

@testset "legendre sum correct values" begin
    # legendre == jacobi(a,b=0,0), same function body
    terms = [1,3,5,7,9]
    weights = ones(size(terms)...)
    @test legendre_sum(terms, weights, [0.]) ≈ [0.]
    terms = [1,2,3,4,5]
    weights = ones(size(terms)...)
    # -1 accumulates 3 times and +1 two times on left edge => expect -1.
    # 1 accumulates 5 times on right edge => expect 5.
    @test legendre_sum(terms, weights, [-1., 1.]) ≈ [-1., 5.]
end

@testset "legendre series correct values contiguous" begin
    terms = [1,2,3,4,5]
    ledge = [-1.,1.,-1.,1.,-1.]
    center = [0.,-0.5,0.,0.375,0.]
    redge = [1.,1.,1.,1.,1.]
    series = legendre_series(terms, [-1., 0., 1.])
    @test series[1,:] ≈ ledge
    @test series[2,:] ≈ center
    @test series[3,:] ≈ redge
end

@testset "legendre series correct values sparse" begin
    terms = [1,3,5]
    ledge = [-1.,-1.,-1.]
    center = [0.,0.,0.]
    redge = [1.,1.,1.]
    series = legendre_series(terms, [-1., 0., 1.])
    @test series[1,:] ≈ ledge
    @test series[2,:] ≈ center
    @test series[3,:] ≈ redge
end


using Pkg; Pkg.activate(".")
include("../src/ARS.jl")
using Plots

μ, σ = 0.0, 5.0
support = (-10.0, 1.0)

f(x) = exp(-0.5(x - μ)^2 / σ^2) / sqrt(2pi * σ^2)

# Build the sampler and simulate 10,000 samples
sampler = RejectionSampler(
    f,
    support,
    (-3.0, -2.0),
    max_segments = 25,
    use_secants=true
)
@time sim = run_sampler!(sampler, 100_000);

x = range(-10.0, 10.0, length=100)
env = sampler.envelop
majorant = eval_envelop(sampler, x)
minorant = eval_secants(sampler, x)
target = f.(x)

histogram(sim, normalize = true, label = "Histogram", fillalpha=0.5)
plot!(x, [target majorant minorant], width = 5, label = ["Target" "Majorant" "Minorant"])

##

# ----------------

f2(x) = begin
    δ = x - μ
    fout = - 0.5δ^2 / σ^2
    gout = - δ / σ^2
    fout, gout
end

# Build the sampler and simulate 10,000 samples
sampler = RejectionSampler(
    f2,
    support,
    (-3.0, -2.0),
    autograd=false,
    apply_log=false,
    max_segments = 25,
    use_secants=true
)
@time sim = run_sampler!(sampler, 100_000);

x = range(-10.0, 10.0, length=100)
env = sampler.envelop
majorant = eval_envelop(sampler, x)
minorant = eval_secants(sampler, x)
target = f.(x)

histogram(sim, normalize = true, label = "Histogram", fillalpha=0.5)
plot!(x, [target majorant minorant], width = 5, label = ["Target" "Majorant" "Minorant"])


@testset "discrete uniform sampler" begin

	cc = dummyChoiceContext()

	@testset "getparams, numparams, paramranges, sample" begin

		@testset "using default construction" begin

			s = DataGenerators.DiscreteUniformSampler()

			@testset "numparams and paramranges" begin
				@test DataGenerators.numparams(s) == 2
				prs = DataGenerators.paramranges(s)
				@test typeof(prs) <: Vector{Tuple{Float64,Float64}}
				@test prs == fill((Float64(typemin(Int)), Float64(typemax(Int))), 2)
			end

			@testset "default params" begin
				@test DataGenerators.getparams(s) == [Float64(typemin(Int)), Float64(typemax(Int))]
			end

			@mtestset "default sampling" reps=Main.REPS alpha=Main.ALPHA begin
				x, trace = DataGenerators.sample(s, (0,1), cc)
				@test typeof(x) <: Int
				# @test typemin(Int) <= x <= typemax(Int)
				# @mtest_distributed_as DiscreteUniform(typemin(Int),typemax(Int)) x
				# can't do above owing to problems with DiscreteUniform and large ranges, so instead:
				@mtest_that_sometimes x < (typemin(Int)>>1)
				@mtest_that_sometimes (typemin(Int)>>1) <= x < 0
				@mtest_that_sometimes 0 <= x < (typemax(Int)>>1)
				@mtest_that_sometimes (typemax(Int)>>1) <= x
			end

		end

		@testset "non-default construction" begin

			s = DataGenerators.DiscreteUniformSampler([-100.0,129.0])

			@testset "constructor with params" begin

				@test DataGenerators.getparams(s) == [-100.0,129.0]

				@mtestset "is consistent with discrete uniform" reps=Main.REPS alpha=Main.ALPHA begin
					x, trace = DataGenerators.sample(s, (0,1), cc)
			        @mtest_distributed_as DiscreteUniform(-100,129) x
				end

			end

		end

	end

	@testset "setparams" begin

	    s = DataGenerators.DiscreteUniformSampler()
	    prs = DataGenerators.paramranges(s)
	    midparams = map(pr->robustmidpoint(pr[1],pr[2]), prs)

	    @testset "valid parameters $params" for params in [[8.0,50.0], [-3113.0, -2889.0],  [-6.0, 6.0],]

	        DataGenerators.setparams!(s, params)

			@mtestset "consistent with discrete uniform" reps=Main.REPS alpha=Main.ALPHA begin
	        	x, trace = DataGenerators.sample(s, (0,1), cc)
		        @mtest_distributed_as DiscreteUniform(Int(params[1]),Int(params[2])) x
			end

	    end

		@testset "setparams adjusts order of parameters" begin

			DataGenerators.setparams!(s, [50.0,8.0])
			@test DataGenerators.getparams(s) == [8.0,50.0]

			@mtestset "consistent with discrete uniform" reps=Main.REPS alpha=Main.ALPHA begin
	        	x, trace = DataGenerators.sample(s, (0,1), cc)
		        @mtest_distributed_as DiscreteUniform(8,50) x
			end

		end

		@testset "handles full Int range" begin
			params = [Float64(typemin(Int)), Float64(typemax(Int))]
			DataGenerators.setparams!(s, params)

			@mtestset "consistent with discrete uniform" reps=Main.REPS alpha=Main.ALPHA begin
	        	x, trace = DataGenerators.sample(s, (0,1), cc)
		        # @mtest_distributed_as DiscreteUniform(typemin(Int),typemax(Int)) x
				# can't do above owing to problems with DiscreteUniform and large ranges, so instead:
				@mtest_that_sometimes x < (typemin(Int)>>1)
				@mtest_that_sometimes (typemin(Int)>>1) <= x < 0
				@mtest_that_sometimes 0 <= x < (typemax(Int)>>1)
				@mtest_that_sometimes (typemax(Int)>>1) <= x
			end

		end

	    @testset "boundary values for parameter index $pidx" for pidx in 1:length(prs)

	        pr = prs[pidx]
	        params = copy(midparams)

			@testset "bound index $bidx" for bidx in 1:2

	            params[pidx] = pr[bidx]
		        DataGenerators.setparams!(s, params)

				sortedparams = sort(params)
				q1 = 0.75 * sortedparams[1] + 0.25 * sortedparams[2]
				q2 = 0.5 * sortedparams[1] + 0.5 * sortedparams[2]
				q3 = 0.25 * sortedparams[1] + 0.75 * sortedparams[2]
				@mtestset "consistent with discrete uniform" reps=Main.REPS alpha=Main.ALPHA begin
		        	x, trace = DataGenerators.sample(s, (0,1), cc)
					@test sortedparams[1] <= x <= sortedparams[2]
			        # @mtest_distributed_as DiscreteUniform(Int(sortedparams[1]),Int(sortedparams[2])) x
					# can't do above owing to problems with DiscreteUniform and large ranges, so instead:
					@mtest_that_sometimes x < q1
					@mtest_that_sometimes q1 <= x < q2
					@mtest_that_sometimes q2 <= x < q3
					@mtest_that_sometimes q3 <= x
				end

				# currently redundant since parameters are adjusted within setparams!
				# @testset "range check exception" begin
				# 		            params[pidx] = bidx == 1 ? prevfloat(pr[bidx]) : nextfloat(pr[bidx])
				# 		            @test_throws ErrorException DataGenerators.setparams!(s, params)
				# end

			end

	    end

		@testset "handles non-integer parameters sensibly" begin

	        DataGenerators.setparams!(s, [-2.9, 8.6])

			@mtestset "consistent with discrete uniform" reps=Main.REPS alpha=Main.ALPHA begin
	        	x, trace = DataGenerators.sample(s, (0,1), cc)
				@test -2.9 <= x <= 8.6 # lower bound rounded up, upper bound rounded down
		        @mtest_distributed_as DiscreteUniform(-2,8) x
			end

		end

	    @testset "wrong number of parameters" begin
	        @test_throws ErrorException DataGenerators.setparams!(s, midparams[1:end-1])
	        @test_throws ErrorException DataGenerators.setparams!(s, [midparams; 0.5])
	    end

	end

	@testset "estimateparams" begin

	   	otherparams = [-42.0, 42.0]

	    @testset "from parameters $params" for params in [[20.0, 29.0], [50.0, 50.0],]

	        s1 = DataGenerators.DiscreteUniformSampler(params)
	        s2 = DataGenerators.DiscreteUniformSampler(otherparams)
	        traces = map(1:100) do i
	            x, trace = DataGenerators.sample(s1, (0,1), cc)
	            trace
	        end
	        estimateparams!(s2, traces)

			@mtestset "consistent with discrete uniform" reps=Main.REPS alpha=Main.ALPHA begin
	        	x, trace = DataGenerators.sample(s2, (0,1), cc)
		        @mtest_distributed_as DiscreteUniform(Int(params[1]),Int(params[2])) x
			end

	    end

	    @testset "too few traces" begin

	        params = [1.0, 6.0]
	        s1 = DataGenerators.DiscreteUniformSampler(params)
	        s2 = DataGenerators.DiscreteUniformSampler(otherparams)
	        traces = map(1:1) do i
	            x, trace = DataGenerators.sample(s1, (0,1), cc)
	            trace
	        end
	        estimateparams!(s2, traces)

			@mtestset "consistent with discrete uniform" reps=Main.REPS alpha=Main.ALPHA begin
	        	x, trace = DataGenerators.sample(s2, (0,1), cc)
		        @mtest_distributed_as DiscreteUniform(-42,42) x
			end

	    end

	end

end


module IntegralVerification

using Statistics
using Random

export testIntegral, testExpressions

function doStatTest(testvalues, precision; verbose = true)
    output = Statistics.mean(testvalues),Statistics.std(testvalues)
    if verbose
        println("mean = ",output[1])
        println("std  = ", output[2])
    end
    if abs(output[1]) < precision && abs(output[2]) < precision
        return true
    end
    return false
end

function generateRandomArgs(randomVariables)
    vars = zeros(ComplexF64, length(randomVariables))
    for (i,f) in enumerate(randomVariables)
        vars[i] = f()
    end
    return vars
end

function testIntegral(integrator, integrand, analytic, randomVariables, Ntests,  precision = 10. ^ (-7))

    numeric(args...) = integrator(x -> integrand(x, args...))

    return testExpressions(numeric, analytic, randomVariables, Ntests,precision)

end

function testExpressions(expr1, expr2, Ntests)
    return testExpressions(expr1,expr2,[randn],Ntests)
end

function testExpressions(expr1,expr2,randomVariables,Ntests,precision = 10. ^ (-7))

    if Ntests < 2
        throw(ArgumentError)
    end

    outvalues = zeros(ComplexF64, Ntests)
    for i in 1:Ntests
        args = generateRandomArgs(randomVariables)
        outvalues[i] = expr1(args...) - expr2(args...)
    end

    passed = doStatTest(outvalues, precision)

    if passed
        println("passed")
    else
        println("failed")
    end
    return passed
end

end # module


using Test
using Distributions
using DataStructures

import GlobalSensitivityAnalysis: scale_samples!, ishigami, sample

##
## 1. utils
##

# SobolData
N = 100
calc_second_order = false
parameters = OrderedDict(
    :param1 => Normal(0,1),
    :param2 => LogNormal(5, 20),
    :param3 => TriangularDist(0, 4, 1)
)

data1 = SobolData(params = parameters, calc_second_order = calc_second_order, N = N)
@test data1.params[:param1] == parameters[:param1]
@test data1.params[:param2] == parameters[:param2]
@test data1.params[:param2] == parameters[:param2]
@test data1.params[:param3] == parameters[:param3]
@test data1.N == N
@test data1.calc_second_order == calc_second_order

data2 = SobolData()
@test data2.params == nothing
@test data2.N == 1000
@test data2.calc_second_order == true

data3 = SobolData(params = parameters)
data4 = SobolData(calc_second_order = false)
data5 = SobolData(N = 100)

##
## 2. Sample Sobol
##

samples = sample(data1)
@test size(samples, 2) == length(data1.params)
if data1.calc_second_order
    @test size(samples,1) == data1.N * (2 * length(data1.params) + 2)
else
    @test size(samples,1) == data1.N * (length(data1.params) + 2)
end

samples3 = sample(data3)
@test size(samples3, 2) == length(data3.params)
if data3.calc_second_order
    @test size(samples3,1) == data3.N * (2 * length(data3.params) + 2)
else
    @test size(samples3,1) == data3.N * (length(data3.params) + 2)
end

@test_throws ErrorException sample(data2) # params are nothing
@test_throws ErrorException sample(data4) # params are nothing
@test_throws ErrorException sample(data5) # params are nothing

##
## 3a. Analyze Sobol
##

Y1 = ishigami(samples)
results = analyze(data1, Y1)
for Si in results[:firstorder]
    @test Si <= 1
end
for CI in results[:firstorder_conf]
    @test CI > 0
end
for St in results[:totalorder]
    @test St <= 1
end
for CI in results[:totalorder_conf]
    @test CI > 0
end
@test sum(results[:totalorder]) > sum(results[:firstorder])

Y3 = ishigami(samples3)
results = analyze(data3, Y3)
for Si in results[:firstorder]
    @test Si <= 1
end
for CI in results[:firstorder_conf]
    @test ismissing(CI) || CI > 0
end
for S2i in results[:secondorder]
    @test ismissing(S2i) || S2i <= 1
end
for CI in results[:secondorder_conf]
    @test ismissing(CI) || CI > 0
end
for St in results[:totalorder]
    @test St <= 1
end
for CI in results[:totalorder_conf]
    @test ismissing(CI) || CI > 0
end
@test sum(results[:totalorder]) > sum(results[:firstorder])

##
## 3b. Analyze Sobol Optional Keyword Args
##

data = SobolData(
    params = OrderedDict(:x1 => Normal(1, 0.2),
        :x2 => Uniform(0.75, 1.25),
        :x3 => LogNormal(0, 0.5)),
    N = 1000
)
samples = sample(data)
Y = ishigami(samples)
results = analyze(data, Y)

results = analyze(data, Y; progress_meter = false) # no progress bar should show

@test length(analyze(data, Y; N_override = 10)) == 6
results_override = analyze(data, Y, N_override = data.N)
results_original = analyze(data, Y)
@test results_override[:firstorder] == results_original[:firstorder]
@test results_override[:totalorder] == results_original[:totalorder]
@test_throws ErrorException analyze(data1, Y1; N_override = data.N + 1) # N_override > N


# Create example point data
x = 8 .* rand(100) .- 4
y = 8 .* rand(100) .- 4
z = sin.(x) + cos.(y)
# Use the "hot" colormap
colormap("hot")
contourf(x, y, z)


#=
These functions are organized in sets based on input/output type. They are unary and not
in-place unless otherwised specified. These functions have been written with the following
assumptions:

- Array input is of length >= 5
- Matrix input is square
- Matrix inputs for n-ary functions are of equal shape

Some of these functions have been modified from their original form to to allow for tunable
input/output sizes, or to test certain programmatic behaviors. Thus, regardless of their
names, one should not expect these functions to be "correct" for their original purpose.
=#

########################
# f(x::Number)::Number #
########################

num2num_1(x) = sin(x)^2 / cos(x)^2
num2num_2(x) = 2*x + sqrt(x*x*x)
num2num_3(x) = 10.31^(x + x) - x
num2num_4(x) = 1
num2num_5(x) = 1. / (1. + exp(-x))

const NUMBER_TO_NUMBER_FUNCS = (num2num_1, num2num_2, num2num_3,
                                num2num_4, num2num_5, identity)

#######################
# f(x::Number)::Array #
#######################

function num2arr_1(x)
    return reshape([num2num_1(x),
                    num2num_2(x),
                    num2num_3(x),
                    num2num_1(x) - num2num_2(x),
                    num2num_2(x),
                    num2num_3(x),
                    num2num_2(x),
                    num2num_3(x)], 2, 2, 2)
end

const NUMBER_TO_ARRAY_FUNCS = (num2arr_1,)

#################################
# f!(y::Array, x::Number)::Void #
#################################

function num2arr_1!(y, x)
    fill!(y, zero(x))
    for i in 2:length(y)
        y[i] = (sin(x) + y[i-1])^2
    end
    return nothing
end

const INPLACE_NUMBER_TO_ARRAY_FUNCS = (num2arr_1!,)

########################
# f(x::Vector)::Number #
########################

vec2num_1(x) = (exp(x[1]) + log(x[3]) * x[4]) / x[5]
vec2num_2(x) = x[1]*x[2] + sin(x[1])
vec2num_3(x) = vecnorm(x' .* x)
vec2num_4(x) = ((sum(x) + prod(x)); 1)
vec2num_5(x) = sum((-x).^3)
vec2num_6(x) = sum([ifelse(i > 0, i, 0) for i in x])

function rosenbrock_1(x)
    a = one(eltype(x))
    b = 100 * a
    result = zero(eltype(x))
    for i in 1:length(x)-1
        result += (a - x[i])^2 + b*(x[i+1] - x[i]^2)^2
    end
    return result
end

function rosenbrock_2(x)
    a = x[1]
    b = 100 * a
    v = map((i, j) -> (a - j)^2 + b*(i - j^2)^2, x[2:end], x[1:end-1])
    return sum(v)
end

rosenbrock_3(x) = sum(map((i, j) -> (1 - j)^2 + 100*(i - j^2)^2, x[2:end], x[1:end-1]))

function rosenbrock_4(x)
    t1 = (1 .+ x[1:end-1]).^2
    t2 = x[2:end] .+ (x[1:end-1]).^2
    return sum(t1 .+ 100 .* (abs.(t2)).^2)
end

function ackley(x)
    a, b, c = 20.0, -0.2, 2.0*π
    len_recip = inv(length(x))
    sum_sqrs = zero(eltype(x))
    sum_cos = sum_sqrs
    for i in x
        sum_cos += cos(c*i)
        sum_sqrs += i^2
    end
    return (-a * exp(b * sqrt(len_recip*sum_sqrs)) -
            exp(len_recip*sum_cos) + a + ℯ)
end

self_weighted_logit(x) = inv(1.0 + exp(-dot(x, x)))

const VECTOR_TO_NUMBER_FUNCS = (vec2num_1, vec2num_2,  vec2num_3, vec2num_4, vec2num_5,
                                vec2num_6, rosenbrock_1, rosenbrock_2, rosenbrock_3,
                                rosenbrock_4, ackley, self_weighted_logit, first)

########################
# f(x::Matrix)::Number #
########################

mat2num_1(x) = det(first(x) * inv(x * x) + x)

function mat2num_2(x)
    a = reshape(x, length(x), 1)
    b = reshape(copy(x), 1, length(x))
    return trace(log.((1 .+ (a * b)) .+ a .- b))
end

function mat2num_3(x)
    k = length(x)
    N = isqrt(k)
    A = reshape(x, N, N)
    return sum(map(n -> sqrt(abs(n) + n^2) * 0.5, A))
end

mat2num_4(x) = mean(sum(sin.(x) * x, 2))

softmax(x) = sum(exp.(x) ./ sum(exp.(x), 2))

const MATRIX_TO_NUMBER_FUNCS = (det, mat2num_1, mat2num_2, mat2num_3, mat2num_4, softmax)

####################
# binary broadcast #
####################

if VERSION >= v"0.6.0-dev.1614"
    const BINARY_BROADCAST_OPS = ((a, b) -> broadcast(+, a, b),
                                  (a, b) -> broadcast(-, a, b),
                                  (a, b) -> broadcast(*, a, b),
                                  (a, b) -> broadcast(/, a, b),
                                  (a, b) -> broadcast(\, a, b),
                                  (a, b) -> broadcast(^, a, b))
else
    const BINARY_BROADCAST_OPS = (.+, .-, .*, ./, .\, .^)
end

#################################
# f(::Matrix, ::Matrix)::Number #
#################################

const BINARY_MATRIX_TO_MATRIX_FUNCS = (+, -, *, /, \,
                                       BINARY_BROADCAST_OPS...,
                                       A_mul_Bt, At_mul_B, At_mul_Bt,
                                       A_mul_Bc, Ac_mul_B, Ac_mul_Bc)

###########################################
# f(::Matrix, ::Matrix, ::Matrix)::Number #
###########################################

relu(x) = log.(1.0 .+ exp.(x))
sigmoid(n) = 1. / (1. + exp.(-n))
neural_step(x1, w1, w2) = sigmoid(dot(w2[1:size(w1, 2)], relu(w1 * x1[1:size(w1, 2)])))

const TERNARY_MATRIX_TO_NUMBER_FUNCS = (neural_step,)

################################
# f!(y::Array, x::Array)::Void #
################################
# Credit for `chebyquad!`, `brown_almost_linear!`, and `trigonometric!` goes to
# Kristoffer Carlsson (@KristofferC).

function chebyquad!(y, x)
    tk = 1/length(x)
    for j = 1:length(x)
        temp1 = 1.0
        temp2 = 2x[j]-1
        temp = 2temp2
        for i = 1:length(y)
            y[i] += temp2
            ti = temp*temp2 - temp1
            temp1 = temp2
            temp2 = ti
        end
    end
    iev = -1.0
    for k = 1:length(y)
        y[k] *= tk
        if iev > 0
            y[k] += 1/(k^2-1)
        end
        iev = -iev
    end
    return nothing
end

function brown_almost_linear!(y, x)
    c = sum(x) - (length(x) + 1)
    for i = 1:(length(x)-1), j = 1:(length(y)-1)
        y[j] += x[i] + c
    end
    y[length(y)] = prod(x) - 1
    return nothing
end

function trigonometric!(y, x)
    for i in x
        for j in eachindex(y)
            y[j] = cos(i)
        end
    end
    c = sum(y)
    n = length(x)
    for i in x
        for j in eachindex(y)
            y[j] = sin(i) * y[j] + n - c
        end
    end
    return nothing
end

function mutation_test_1!(y, x)
    y[1] = x[1]
    y[1] = y[1] * x[2]
    y[2] = y[2] * x[3]
    y[3] = sum(y)
    return nothing
end

function mutation_test_2!(y, x)
    y[1] *= x[1]
    y[2] *= x[1]
    y[1] *= x[2]
    y[2] *= x[2]
    return nothing
end

const INPLACE_ARRAY_TO_ARRAY_FUNCS = (chebyquad!, brown_almost_linear!, trigonometric!,
                                      mutation_test_1!, mutation_test_2!)

######################
# f(x::Array)::Array #
######################

chebyquad(x) = (y = zeros(x); chebyquad!(y, x); return y)

brown_almost_linear(x) = (y = zeros(x); brown_almost_linear!(y, x); return y)

trigonometric(x) = (y = ones(x); trigonometric!(y, x); return y)

mutation_test_1(x) = (y = zeros(x); mutation_test_1!(y, x); return y)

mutation_test_2(x) = (y = ones(x); mutation_test_2!(y, x); return y)

arr2arr_1(x) = (sum(x .* x); zeros(x))

arr2arr_2(x) = x[1, :] .+ x[1, :] .+ first(x)

const ARRAY_TO_ARRAY_FUNCS = (-, chebyquad, brown_almost_linear, trigonometric, arr2arr_1,
                              arr2arr_2, mutation_test_1, mutation_test_2, identity)

#######################
# f(::Matrix)::Matrix #
#######################

const MATRIX_TO_MATRIX_FUNCS = (inv,)


@testset "test_discrete" begin

C_111 = ss([-5], [2], [3], [0])
C_212 = ss([-5 -3; 2 -9], [1; 2], [1 0; 0 1], [0; 0])
C_221 = ss([-5 -3; 2 -9], [1 0; 0 2], [1 0], [0 0])
C_222_d = ss([-5 -3; 2 -9], [1 0; 0 2], [1 0; 0 1], [1 0; 0 1])

@test c2d_x0map(ss(4*[1 0; 0 1]), 0.5, :zoh) == (ss(4*[1 0; 0 1], 0.5), zeros(0, 2))
@test c2d_x0map(ss(4*[1 0; 0 1]), 0.5, :foh) == (ss(4*[1 0; 0 1], 0.5), zeros(0, 2))
@test_tupleapprox(c2d_x0map(C_111, 0.01, :zoh),
    ss([0.951229424500714], [0.019508230199714396], [3], [0], 0.01), [1 0])
@test_tupleapprox(c2d_x0map(C_111, 0.01, :foh),
    ss([0.951229424500714], [0.01902855227625244], [3], [0.029506188017136226], 0.01),
    [1 -0.009835396005712075])
@test_tupleapprox(c2d_x0map(C_212, 0.01, :zoh),
    ss([0.9509478368863918 -0.027970882212682433; 0.018647254808454958 0.9136533272694819],
    [0.009466805409666932; 0.019219966830212765], [1 0; 0 1], [0; 0], 0.01),
    [1 0 0; 0 1 0])
@test_tupleapprox(c2d_x0map(C_212, 0.01, :foh),
    ss([0.9509478368863921 -0.027970882212682433; 0.018647254808454954 0.913653327269482],
    [0.008957940478201584; 0.018468989584974498], [1 0; 0 1], [0.004820885889482196;
    0.009738343195298675], 0.01), [1 0 -0.004820885889482196; 0 1 -0.009738343195298675])
@test_tupleapprox(c2d_x0map(C_221, 0.01, :zoh),
    ss([0.9509478368863918 -0.027970882212682433; 0.018647254808454958 0.9136533272694819],
    [0.009753161420545834 -0.0002863560108789034; 9.54520036263011e-5 0.019124514826586465],
    [1.0 0.0], [0.0 0.0], 0.01), [1 0 0 0; 0 1 0 0])
@test_tupleapprox(c2d_x0map(C_221, 0.01, :foh),
    ss([0.9509478368863921 -0.027970882212682433; 0.018647254808454954 0.913653327269482],
    [0.009511049106772921 -0.0005531086285713394; 0.00018436954285711309 0.018284620042117387],
    [1 0], [0.004917457305816479 -9.657141633428213e-5], 0.01),
    [1 0 -0.004917457305816479 9.657141633428213e-5;
    0 1 -3.219047211142736e-5 -0.009706152723187249])
@test_tupleapprox(c2d_x0map(C_222_d, 0.01, :zoh),
    ss([0.9509478368863918 -0.027970882212682433; 0.018647254808454958 0.9136533272694819],
    [0.009753161420545834 -0.0002863560108789034; 9.54520036263011e-5 0.019124514826586465],
    [1 0; 0 1], [1 0; 0 1], 0.01), [1 0 0 0; 0 1 0 0])
@test_tupleapprox(c2d_x0map(C_222_d, 0.01, :foh),
    ss([0.9509478368863921 -0.027970882212682433; 0.018647254808454954 0.913653327269482],
    [0.009511049106772921 -0.0005531086285713394; 0.00018436954285711309 0.018284620042117387],
    [1 0; 0 1], [1.0049174573058164 -9.657141633428213e-5; 3.219047211142736e-5 1.0097061527231872], 0.01),
    [1 0 -0.004917457305816479 9.657141633428213e-5; 0 1 -3.219047211142736e-5 -0.009706152723187249])

# Test c2d for transfer functions
G = tf([1, 1], [1, 3, 1])
Gd = c2d(G, 0.2)
@test Gd ≈ tf([0, 0.165883310712090, -0.135903621603238], [1.0, -1.518831946985175, 0.548811636094027], 0.2) rtol=1e-14

# Issue #391
@test c2d(tf(C_111), 0.01, :zoh) ≈ tf(c2d(C_111, 0.01, :zoh))
@test c2d(tf(C_111), 0.01, :foh) ≈ tf(c2d(C_111, 0.01, :foh))

# Issue #471 discussion, test c2d for zero row C
C_210 = ss(C_212.A, C_212.B, zeros(0, 2), zeros(0, 1))
@test c2d(C_210, 0.01).A ≈ c2d(C_212, 0.01).A

# c2d on a zpk model should arguably return a zpk model
@test_broken typeof(c2d(zpk(G), 1)) <: TransferFunction{<:ControlSystems.SisoZpk}

# ERRORS
@test_throws MethodError c2d(ss([1], [2], [3], [4], 0.01), 0.01)   # Already discrete
@test_throws MethodError c2d(ss([1], [2], [3], [4], -1), 0.01)     # Already discrete

# d2c
@static if VERSION > v"1.4" # log(matrix) is buggy on previous versions, should be fixed in 1.4 and back-ported to 1.0.6
    @test d2c(c2d(C_111, 0.01)) ≈ C_111
    @test d2c(c2d(C_212, 0.01)) ≈ C_212
    @test d2c(c2d(C_221, 0.01)) ≈ C_221
    @test d2c(c2d(C_222_d, 0.01)) ≈ C_222_d
    @test d2c(Gd) ≈ G

    sys = ss([0 1; 0 0], [0;1], [1 0], 0)
    sysd = c2d(sys, 1)
    @test d2c(sysd) ≈ sys
end

# forward euler
@test c2d(C_111, 1, :fwdeuler).A == I + C_111.A
@test d2c(c2d(C_111, 0.01, :fwdeuler), :fwdeuler) ≈ C_111
@test d2c(c2d(C_212, 0.01, :fwdeuler), :fwdeuler) ≈ C_212
@test d2c(c2d(C_221, 0.01, :fwdeuler), :fwdeuler) ≈ C_221
@test d2c(c2d(C_222_d, 0.01, :fwdeuler), :fwdeuler) ≈ C_222_d
@test d2c(c2d(G, 0.01, :fwdeuler), :fwdeuler) ≈ G

Cd = c2d(C_111, 0.001, :fwdeuler)
t = 0:0.001:2
y,_ = step(C_111, t)
yd,_ = step(Cd, t)
@test norm(yd-y) / norm(y) < 1e-3

# dab
import DSP: conv
ζ = 0.2
ω = 1
B = [1]
A   = [1, 2ζ*ω, ω^2]
P  = tf(B,A)
# Control design
ζ0 = 0.7
ω0 = 2
Am = [1, 2ζ0*ω0, ω0^2]
Ao = conv(2Am, [1/2, 1]) # Observer polynomial, add extra pole due to the integrator
AR = [1,0] # Force the controller to contain an integrator ( 1/(s+0) )

B⁺  = [1] # The process numerator polynomial can be facored as B = B⁺B⁻ where B⁻ contains the zeros we do not want to cancel (non-minimum phase and poorly damped zeros)
B⁻  = [1]
Bm  = conv(B⁺, B⁻) # In this case, keep the entire numerator polynomial of the process

R,S,T = rstc(B⁺,B⁻,A,Bm,Am,Ao,AR) # Calculate the 2-DOF controller polynomials

Gcl = tf(conv(B,T),zpconv(A,R,B,S)) # Form the closed loop polynomial from reference to output, the closed-loop characteristic polynomial is AR + BS, the function zpconv takes care of the polynomial multiplication and makes sure the coefficient vectores are of equal length

@test ControlSystems.isstable(Gcl)

p = pole(Gcl)
# Test that all desired poles are in the closed-loop system
@test norm(minimum(abs.((pole(tf(Bm,Am)) .- sort(p, by=imag)')), dims=2)) < 1e-6
# Test that the observer poles are in the closed-loop system
@test norm(minimum(abs.((pole(tf(1,Ao)) .- sort(p, by=imag)')), dims=2)) < 1e-6

end


# https://github.com/cvxgrp/scs
"""
   unvec_symm(x, dim)
Returns a dim-by-dim symmetric matrix corresponding to `x`.
`x` is a vector of length dim*(dim + 1)/2, corresponding to a symmetric matrix
```
X = [ X11     X12/√2 ... X1k/√2
      X21/√2  X22    ... X2k/√2
      ...
      Xk1/√2  Xk2/√2 ... Xkk ],
```
where
`vec(X) = (X11, X12, X22, X13, X23, ..., Xkk)`

Note that the factor √2 preserves inner products:
`x'*c = Tr(unvec_symm(c, dim) * unvec_symm(x, dim))`
"""
function unvec_symm_scs(x, dim)
    X = zeros(eltype(x), dim, dim)
    idx = 1
    for i in 1:dim
        for j in 1:i
            if i == j
                X[i,j] = x[idx]
            else
                X[j,i] = X[i,j] = x[idx] / sqrt(2)
            end
            idx += 1
        end
    end
    return X
end

"""
   vec_symm(X)
Returns a vectorized representation of a symmetric matrix `X`.
`vec(X) = (X11, √2*X12, X22, √2*X13, X23, ..., Xkk)`

Note that the factor √2 preserves inner products:
`x'*c = Tr(unvec_symm(c, dim) * unvec_symm(x, dim))`
"""
function vec_symm_scs(X)
    x_vec = sqrt(2).*X[LinearAlgebra.tril(trues(size(X)))']
    idx = 1
    for i in 1:size(X)[1]
        x_vec[idx] =  x_vec[idx]/sqrt(2)
        idx += i + 1
    end
    return x_vec
end

# Write and read CP from file
function cp_to_file(file, params_dict; dense=true)
    A, b, c = params_dict[:A], params_dict[:b], params_dict[:c]
    if isnothing(params_dict[:x_star])
         _cp_to_file(file, (A, b, c), dense=dense)
    else
        x_star = params_dict[:x_star]
        y_star = params_dict[:y_star]
        s_star = params_dict[:s_star]
        _cp_to_file(file, (A, b, c), opt_vals=(x_star, y_star, s_star), dense=dense)
    end
end

function _cp_to_file(file, params; opt_vals=(), dense=true)
    if dense
        writedlm(file, (params..., opt_vals...))
    else
        writedlm(file, (findnz(params[1])..., params[2:3]..., opt_vals...))
    end
end

function cp_from_file(file; dense=true)
    file_vec = readlines(file)
    nvars = length(file_vec)

    if dense
        ~(nvars in [3, 6]) && throw(ArgumentError("Invalid file"))
        offset = 0
        A_flat = parse.(Float64, split(file_vec[1], '\t'))
        b = parse.(Float64, split(file_vec[2+offset], '\t'))
        c = parse.(Float64, split(file_vec[3+offset], '\t'))
        m,n = length(b), length(c)
        A = reshape(A_flat, (m,n))
    else
        ~(nvars in [5, 8]) && throw(ArgumentError("Invalid file"))
        offset = 2
        A_row_inds = parse.(Int, split(file_vec[1], '\t'))
        A_col_inds = parse.(Int, split(file_vec[2], '\t'))
        A_vals = parse.(Float64, split(file_vec[3], '\t'))
        A = sparse(A_row_inds, A_col_inds, A_vals)
        b = parse.(Float64, split(file_vec[2+offset], '\t'))
        c = parse.(Float64, split(file_vec[3+offset], '\t'))
    end

    if nvars in [6, 8]
        x_star = parse.(Float64, split(file_vec[4+offset], '\t'))
        y_star = parse.(Float64, split(file_vec[5+offset], '\t'))
        s_star = parse.(Float64, split(file_vec[6+offset], '\t'))
    else
        x_star, y_star, s_star = nothing, nothing, nothing
    end
    return Dict(:A => A, :b => b, :c => c, :x_star => x_star,
                :y_star => y_star, :s_star => s_star)
end

function derivatives_to_file(file, deriv, adjoint)
    packed_deriv = (deriv[:dA], deriv[:db], deriv[:dc], deriv[:dx], deriv[:dy], deriv[:ds])
    packed_adjoint = (adjoint[:dA], adjoint[:db], adjoint[:dc], adjoint[:dx], adjoint[:dy], adjoint[:ds])
    writedlm(file, (packed_deriv..., packed_adjoint...))
end

function derivatives_from_file(file)
    file_vec = readlines(file)

    function from_file(file_vec, offset)
        ret = Dict()
        ret[:db] = parse.(Float64, split(file_vec[2+offset], '\t'))
        ret[:dc] = parse.(Float64, split(file_vec[3+offset], '\t'))
        m, n = length(ret[:db]), length(ret[:dc])
        ret[:dA] = reshape(parse.(Float64, split(file_vec[1+offset], '\t')), (m,n))

        ret[:dx] = parse.(Float64, split(file_vec[4+offset], '\t'))
        ret[:dy] = parse.(Float64, split(file_vec[5+offset], '\t'))
        ret[:ds] = parse.(Float64, split(file_vec[6+offset], '\t'))
        return ret
    end

    # derivative, adjoint
    return from_file(file_vec, 0), from_file(file_vec, 6)
end

function _vectorized_index_row_major(i, j)
    if i <= j
        k = div((j-1)*j, 2) + i
    else
        k = div((i-1)*i, 2) + j
    end
    return k
end

function index_map_to_row_major(dim)
    return sortperm(index_map_to_col_major(dim))
end

function index_map_to_col_major(dim)
    index_map = []
    for i=1:dim
        for j=i:dim
            k = _vectorized_index_row_major(i, j)
            push!(index_map, k)
        end
    end
    index_map = sortperm(index_map)
    return index_map
end

function rewrite_sdps_to_row_major!(A, b, cones)
    i = 1
    for cone in cones
        if typeof(cone) <: MOI.PositiveSemidefiniteConeTriangle
            d = MOI.dimension(cone)
            s = cone.side_dimension
            index_map = index_map_to_row_major(cone.side_dimension)
            A[i:(i+d-1), :] .= A[i:(i+d-1), :][index_map, :]
            b[i:(i+d-1)] .= b[i:(i+d-1)][index_map]
        end
        i += MOI.dimension(cone)
    end
end

function rewrite_sdps_to_row_major(A, b, cones)
    i = 1
    A_ = copy(A)
    b_ = copy(b)
    for cone in cones
        if typeof(cone) <: MOI.PositiveSemidefiniteConeTriangle
            d = MOI.dimension(cone)
            s = cone.side_dimension
            index_map = index_map_to_row_major(cone.side_dimension)
            A_[i:(i+d-1), :] .= A[i:(i+d-1), :][index_map, :]
            b_[i:(i+d-1)] .= b[i:(i+d-1)][index_map]
        end
        i += MOI.dimension(cone)
    end
    return A_, b_
end

function rewrite_sdps_to_col_major!(A, b, cones)
    i = 1
    for cone in cones
        if typeof(cone) <: MOI.PositiveSemidefiniteConeTriangle
            d = MOI.dimension(cone)
            s = cone.side_dimension
            index_map = index_map_to_col_major(cone.side_dimension)
            A[i:(i+d-1), :] .= A[i:(i+d-1), :][index_map, :]
            b[i:(i+d-1)] .= b[i:(i+d-1)][index_map]
        end
        i += MOI.dimension(cone)
    end
end

function rewrite_sdps_to_col_major(A, b, cones)
    i = 1
    A_ = copy(A)
    b_ = copy(b)
    for cone in cones
        if typeof(cone) <: MOI.PositiveSemidefiniteConeTriangle
            d = MOI.dimension(cone)
            s = cone.side_dimension
            index_map = index_map_to_col_major(cone.side_dimension)
            A_ .= A[i:(i+d-1), :][index_map, :]
            b_ .= b[i:(i+d-1)][index_map]
        end
        i += MOI.dimension(cone)
    end
    return A_, b_
end


using DataDrivenDiffEq
using ModelingToolkit
using LinearAlgebra
using OrdinaryDiffEq
using Test

@testset "Basis" begin
    @variables u[1:3]
    @parameters w[1:2]
    h = [u[1]; u[2]; cos(w[1]*u[2]+w[2]*u[3]); u[3]+u[2]]
    h_not_unique = [1u[1]; u[1]; 1u[1]^1; h]
    basis = Basis(h_not_unique, u, parameters = w)

    @test isequal(variables(basis), u)
    @test isequal(parameters(basis), w)
    @test free_parameters(basis) == 6
    @test free_parameters(basis, operations = [+, cos]) == 7
    @test DataDrivenDiffEq.count_operation((ModelingToolkit.Constant(1) + cos(u[2])*sin(u[1]))^3, [+, cos, ^, *]) == 4

    basis_2 = unique(basis)
    @test isequal(basis, basis_2)
    @test size(basis) == size(h)
    @test basis([1.0; 2.0; π], p = [0. 1.]) ≈ [1.0; 2.0; -1.0; π+2.0]
    @test size(basis) == size(basis_2)
    push!(basis_2, sin(u[2]))
    @test size(basis_2)[1] == length(h)+1

    basis_3 = merge(basis, basis_2)
    @test size(basis_3) == size(basis_2)
    @test isequal(variables(basis_3), variables(basis_2))
    @test isequal(parameters(basis_3), parameters(basis_2))

    merge!(basis_3, basis)
    @test basis_3 == basis_2

    push!(basis, 1u[3]+1u[2])
    unique!(basis)
    @test size(basis) == size(h)

    @variables a
    g = [u[1]; u[3]; a]
    basis = Basis(g, [u; a])
    @test basis([1; 2; 3; 4]) == [1; 3; 4]
    g = [1.0*u[1]; 1.0*u[3]; 1.0*u[2]]
    basis = Basis(g, u, parameters = [])
    X = ones(Float64, 3, 10)
    X[1, :] .= 3*X[1, :]
    X[3, :] .= 5*X[3, :]
    # Check the array evaluation
    @test basis(X) ≈ [1.0 0.0 0.0; 0.0 0.0 1.0; 0.0 1.0 0.0] * X
    f = jacobian(basis)
    @test f([1;1;1], [0.0 0.0]) ≈ [1.0 0.0 0.0; 0.0 0.0 1.0; 0.0 1.0 0.0]
    @test_nowarn sys = ODESystem(basis)
    @test_nowarn [xi for xi in basis]
    @test_nowarn basis[2:end]; basis[2]; first(basis); last(basis); basis[:]

    @variables u[1:2] t

    g = [u[2]; -sin(u[1])*exp(-t)]
    basis = Basis(g, [u...; t])
    @test_nowarn ODESystem(basis, t)
end

@testset "DMD" begin
    # Create some linear data
    A = [0.9 -0.2; 0.0 0.2]
    y = [[10.; -10.]]
    for i in 1:10
        push!(y, A*y[end])
    end
    X = hcat(y...)
    @test_throws AssertionError ExactDMD(X[:, 1:end-2], dt = -1.0)
    estimator = ExactDMD(X[:,1:end-2])
    @test isstable(estimator)
    @test estimator.Ã ≈ A
    @test eigvals(estimator) ≈ eigvals(A)
    @test eigvecs(estimator) ≈ eigvecs(A)
    @test_nowarn dynamics(estimator)
    @test_throws AssertionError dynamics(estimator, discrete = false)
    @test_nowarn update!(estimator, X[:, end-1], X[:,end])
end

@testset "EDMD" begin
    # Test for linear system
    function linear_sys(u, p, t)
        x = -0.9*u[1]
        y = -0.3*u[2]
        return [x;y]
    end

    u0 = [π; 1.0]
    tspan = (0.0, 20.0)
    prob = DiscreteProblem(linear_sys, u0, tspan)
    sol = solve(prob,FunctionMap())

    @variables u[1:2]
    h = [1u[1]; 1u[2]; sin(u[1]); cos(u[1]); u[1]*u[2]]
    basis = Basis(h, u)

    @test_throws AssertionError ExtendedDMD(sol[:,:], basis, dt = -1.0)
    estimator = ExtendedDMD(sol[:,:], basis)
    @test basis == estimator.basis
    basis_2 = reduce_basis(estimator, threshold = 1e-5)
    @test size(basis_2)[1] < size(basis)[1]
    estimator_2 = ExtendedDMD(sol[:,:], basis_2)
    p1 = DiscreteProblem(dynamics(estimator), u0, tspan, [])
    s1 = solve(p1,FunctionMap())
    p2 = DiscreteProblem(dynamics(estimator_2), u0, tspan, [])
    s2 = solve(p2,FunctionMap())
    p3 = DiscreteProblem(linear_dynamics(estimator_2), estimator_2(u0), tspan, [])
    s3 = solve(p3,FunctionMap())
    @test sol[:,:] ≈ s1[:,:]
    @test sol[:,:] ≈ s2[:,:]
    @test estimator_2.basis(sol[:,:])≈ s3[:,:]
    @test eigvals(estimator_2) ≈ [-0.9; -0.3]

    # Test for nonlinear system
    function nonlinear_sys(du, u, p, t)
        du[1] = sin(u[1])
        du[2] = -0.3*u[2] -0.9*u[1]
    end

    prob = DiscreteProblem(nonlinear_sys, u0, tspan)
    sol = solve(prob,FunctionMap())
    estimator = ExtendedDMD(sol[:,:], basis)
    p4 = DiscreteProblem(dynamics(estimator), u0, tspan, [])
    s4 = solve(p4,FunctionMap())
    @test sol[:,:] ≈ s4[:,:]
end

@testset "DMDc" begin
    # Define measurements from unstable system with known control input
    X = [4 2 1 0.5 0.25; 7 0.7 0.07 0.007 0.0007]
    U = [-4 -2 -1 -0.5]
    B = Float32[1; 0]

    # But with a little more knowledge
    sys = DMDc(X, U, B = B)
    @test isa(get_dynamics(sys), ExactDMD)
    @test sys.koopman.Ã ≈[1.5 0; 0 0.1]
    @test get_input_map(sys) ≈ [1.0; 0.0]
    @test !isstable(sys)
    @test_nowarn eigen(sys)

    # Check the solution of an unforced and forced system against each other
    dudt_ = dynamics(sys)
    prob = DiscreteProblem(dudt_, X[:, 1], (0., 10.))
    sol_unforced = solve(prob,  FunctionMap())

    dudt_ = dynamics(sys, control = (u, p, t) -> -0.5u[1])
    prob = DiscreteProblem(dudt_, X[:, 1], (0., 10.))
    sol = solve(prob, FunctionMap())

    @test all(abs.(diff(sol[1,:])) .< 1e-5)
    @test sol[2,:] ≈ sol_unforced[2,:]
end

@testset "SInDy" begin

    # Create a nonlinear pendulum
    function pendulum(u, p, t)
        x = u[2]
        y = -9.81sin(u[1]) - 0.1u[2]^3 -0.2*cos(u[1])
        return [x;y]
    end

    u0 = [0.99π; -1.0]
    tspan = (0.0, 20.0)
    dt = 0.3
    prob = ODEProblem(pendulum, u0, tspan)
    sol = solve(prob, Tsit5(), saveat = dt)

    # Create the differential data
    DX = similar(sol[:,:])
    for (i, xi) in enumerate(eachcol(sol[:,:]))
        DX[:,i] = pendulum(xi, [], 0.0)
    end
    # Create a basis
    @variables u[1:2]

    # Lots of polynomials
    polys = Operation[1]
    for i ∈ 1:5
        push!(polys, u.^i...)
        for j ∈ 1:i-1
            push!(polys, u[1]^i*u[2]^j)
        end
    end

    # And some other stuff
    h = [cos(u[1]); sin(u[1]); u[1]*u[2]; u[1]*sin(u[2]); u[2]*cos(u[2]); polys...]

    basis = Basis(h, u)

    opt = STRRidge(1e-2)
    basis = Basis(h, u, parameters = [])
    Ψ = SInDy(sol[:,:], DX, basis, opt = opt, maxiter = 2000)
    @test_nowarn set_threshold!(opt, 1e-2)
    @test size(Ψ)[1] == 2

    # Simulate
    estimator = ODEProblem(dynamics(Ψ), u0, tspan, [])
    sol_ = solve(estimator,Tsit5(), saveat = dt)
    @test sol[:,:] ≈ sol_[:,:]

    opt = ADMM(1e-2, 0.7)
    Ψ = SInDy(sol[:,:], DX, basis, maxiter = 5000, opt = opt)
    @test_nowarn set_threshold!(opt, 1e-2)

    # Simulate
    estimator = ODEProblem(dynamics(Ψ), u0, tspan, [])
    sol_2 = solve(estimator,Tsit5(), saveat = dt)
    @test norm(sol[:,:] - sol_2[:,:], 2) < 2e-1

    opt = SR3(1e-2, 1.0)
    Ψ = SInDy(sol[:,:], DX, basis, maxiter = 5000, opt = opt)
    @test_nowarn set_threshold!(opt, 0.1)

    # Simulate
    estimator = ODEProblem(dynamics(Ψ), u0, tspan, [])
    sol_3 = solve(estimator,Tsit5(), saveat = dt)
    @test norm(sol[:,:] - sol_3[:,:], 2) < 1e-1

    # Now use the threshold adaptation
    opt = SR3(1e-2, 20.0)
    λs = exp10.(-5:0.1:-1)
    Ψ = SInDy(sol[:,:], DX[:, :], basis, λs,  maxiter = 20, opt = opt)
    estimator = ODEProblem(dynamics(Ψ), u0, tspan, [])
    sol_4 = solve(estimator,Tsit5(), saveat = dt)
    @test norm(sol[:,:] - sol_4[:,:], 2) < 1e-1

    # Check for errors
    @test_nowarn SInDy(sol[:,:], DX[1,:], basis, λs, maxiter = 1, opt = opt)
    @test_nowarn SInDy(sol[:, :], DX[1, :], basis, λs, maxiter = 1, opt = opt, denoise = true, normalize = true)

    # Check with noise
    X = sol[:, :] + 1e-3*randn(size(sol[:,:])...)
    Ψ = SInDy(X, DX, basis, λs, maxiter = 10000, opt = opt, denoise = true, normalize = true)

    estimator = ODEProblem(dynamics(Ψ), u0, tspan, [])
    sol_4 = solve(estimator,Tsit5(), saveat = dt)
    @test norm(sol[:,:] - sol_4[:,:], 2) < 5e-1

    # Check sparse_regression
    X .= Array(sol)
    opt = SR3(1e-1, 1.0)
    maxiter = 5000
    θ = basis(X)
    Ξ1 = sparse_regression(X, DX, basis, parameters(basis), maxiter, opt, true, true)
    Ξ2 = similar(Ξ1)
    Ξ3 = similar(Ξ1)
    sparse_regression!(Ξ2, X, DX, basis, parameters(basis), maxiter, opt, true, true)
    sparse_regression!(Ξ3, θ, DX, maxiter, opt, true, true)

    @test Ξ1 ≈ Ξ2 ≈ Ξ3
    @test isapprox(norm(Ξ1'*θ - DX,2), 25.18; atol = 1e-1)

end

@testset "ISInDy" begin

    # Create a test problem
    function simple(u, p, t)
        return [(2.0u[2]^2 - 3.0)/(1.0 + u[1]^2); -u[1]^2/(2.0 + u[2]^2); (1-u[2])/(1+u[3]^2)]
    end

    u0 = [2.37; 1.58; -3.10]
    tspan = (0.0, 10.0)
    prob = ODEProblem(simple, u0, tspan)
    sol = solve(prob, Tsit5(), saveat = 0.1)

    # Create the differential data
    X = sol[:,:]
    DX = similar(X)
    for (i, xi) in enumerate(eachcol(X))
        DX[:, i] = simple(xi, [], 0.0)
    end

    # Create a basis
    @variables u[1:3]
    polys = ModelingToolkit.Operation[]
    # Lots of basis functions
    for i ∈ 0:5
        if i == 0
            push!(polys, u[1]^0)
        end
        for ui in u
            if i > 0
                push!(polys, ui^i)
            end
        end
    end

    basis= Basis(polys, u)

    opt = ADM(1e-3)
    Ψ = ISInDy(X, DX, basis, opt = opt, maxiter = 100, rtol = 0.1)

    # Simulate
    estimator = ODEProblem(dynamics(Ψ), u0, tspan)
    sol_ = solve(estimator, Tsit5(), saveat = 0.1)
    @test sol[:,:] ≈ sol_[:,:]
end

@testset "Utilities" begin
    t = collect(-2:0.01:2)
    U = [cos.(t).*exp.(-t.^2) sin.(2*t)]
    S = Diagonal([2.; 3.])
    V = [sin.(t).*exp.(-t) cos.(t)]
    A = U*S*V'
    σ = 0.5
    Â = A + σ*randn(401, 401)
    n_1 = norm(A-Â)
    B = optimal_shrinkage(Â)
    optimal_shrinkage!(Â)
    @test norm(A-Â) < n_1
    @test norm(A-B) == norm(A-Â)

    X = randn(3, 100)
    Y = randn(3, 100)
    k = 3

    @test AIC(k, X, Y) == 2*k-2*log(sum(abs2, X- Y))
    @test AICC(k, X, Y) == AIC(k, X, Y)+ 2*(k+1)*(k+2)/(size(X)[2]-k-2)
    @test BIC(k, X, Y) == -2*log(sum(abs2, X -Y)) + k*log(size(X)[2])
    @test AICC(k, X, Y, likelyhood = (X,Y)->sum(abs, X-Y)) == AIC(k, X, Y, likelyhood = (X,Y)->sum(abs, X-Y))+ 2*(k+1)*(k+2)/(size(X)[2]-k-2)

    # Numerical derivatives
    function lorenz(u,p,t)
        x, y, z = u
        ẋ = 10.0*(y - x)
        ẏ = x*(28.0-z) - y
        ż = x*y - (8/3)*z
        return [ẋ, ẏ, ż]
    end
    u0 = [1.0;0.0;0.0]
    tspan = (0.0,50.0)
    dt = 0.005
    prob = ODEProblem(lorenz,u0,tspan)
    sol = solve(prob, Tsit5(), saveat = dt)

    X = Array(sol)
    DX = similar(X)
    for (i, xi) in enumerate(eachcol(X))
        DX[:,i] = lorenz(xi, [], 0.0)
    end

    windowSize, polyOrder = 9, 4
    halfWindow = Int(ceil((windowSize+1)/2))
    DX_sg = similar(X)
    DX_sg_cropped = similar(X[:,halfWindow+1:end-halfWindow])
    X_cropped = similar(X[:,halfWindow+1:end-halfWindow])
    for i =1:size(X,1)
        DX_sg[i,:] = savitzky_golay(X[i,:], windowSize, polyOrder, deriv=1, dt=dt, crop=false)
        X_cropped[i,:], DX_sg_cropped[i,:] = savitzky_golay(X[i,:], windowSize, polyOrder, deriv=1, dt=dt)
    end
    DX_sg2 = savitzky_golay(X, windowSize, polyOrder, deriv=1, dt=dt, crop=false)
    X_cropped2, DX_sg_cropped2 =  savitzky_golay(X, windowSize, polyOrder, deriv=1, dt=dt)
    @test(DX_sg2 == DX_sg)
    DX_sg = DX_sg[:,halfWindow+1:end-halfWindow]
    @test X_cropped == X[:,halfWindow+1:end-halfWindow]
    @test DX_sg_cropped == DX_sg
    @test X_cropped2 == X[:,halfWindow+1:end-halfWindow]
    @test DX_sg_cropped2 == DX_sg

    DX = DX[:,halfWindow+1:end-halfWindow]
    @test isapprox(DX_sg, DX, rtol=1e-2)

    # Sampling
    X = randn(Float64, 2, 100)
    t = collect(0:0.1:9.99)
    Y = randn(size(X))
    xt = burst_sampling(X, 5, 10)
    @test 10 <= size(xt)[end] <= 60
    @test all([any(xi .≈ X) for xi in eachcol(xt)])
    xt, tt = burst_sampling(X, t, 5, 10)
    @test all(diff(tt) .> 0.0)
    @test size(xt)[end] == size(tt)[end]
    @test all([any(xi .≈ X) for xi in eachcol(xt)])
    @test !all([any(xi .≈ Y) for xi in eachcol(xt)])
    xs, ts = burst_sampling(X, t, 2.0, 1)
    @test all([any(xi .≈ X) for xi in eachcol(xs)])
    @test size(xs)[end] == size(ts)[end]
    @test ts[end]-ts[1] ≈ 2.0
    X2n = subsample(X, 2)
    t2n = subsample(t, 2)
    @test size(X2n)[end] == size(t2n)[end]
    @test size(X2n)[end] == Int(round(size(X)[end]/2))
    @test X2n[:, 1] == X[:, 1]
    @test X2n[:, end] == X[:, end-1]
    @test all([any(xi .≈ X) for xi in eachcol(X2n)])
    xs, ts = subsample(X, t, 0.5)
    @test size(xs)[end] == size(ts)[end]
    @test size(xs)[1] == size(X)[1]
    @test all(diff(ts) .≈ 0.5)
    # Loop this a few times to be sure its right
    @test_nowarn for i in 1:20
        xs, ts = burst_sampling(X, t, 2.0, 1)
        xs, ts = subsample(X, t, 0.5)
    end
  end


abstract type AbstractDiscreteFunction end
const ADFun = AbstractDiscreteFunction
abstract type CompositeFunction <: ADFun end
abstract type BasicDiscreteFunction <: ADFun end

# INTERFACES

# iterator for domain
domain(m :: ADFun) = error("not implemented")
# iterator for codomain
codomain(m :: ADFun) = error("not implemented")
# basic constructor from domain and codomain
(adf :: ADFun)(dom :: FinSet, cod :: FinSet) = error("not implemented")

"""
return factors (f1, ..., fp) for composite function,
"""
factors(fp :: CompositeFunction) = error("not implemented")
proj(f :: CompositeFunction, i) = factors(f)[i]
Base.getindex(f :: CompositeFunction, i) = proj(f, i)
Base.setindex!(ft :: CompositeFunction, v :: NTuple, k...) = ft[tuple(k...)] = v

rng(m :: AbstractDiscreteFunction) = codomain(m)
# number of functions
size(comp :: CompositeFunction) = length(comp.factors)

include("dfuns/generic.jl")
include("dfuns/residue_fun.jl")
include("dfuns/fun_product.jl")
include("dfuns/fun_tupling.jl")

function Base.show(io :: IO, f :: AbstractDiscreteFunction)
    d = domain(f)
    r = codomain(f)
    print(io, "Fun f : ")
    print(io, d)
    print(io, " -> ")
    print(io, r)
end

function Base.summary(f :: AbstractDiscreteFunction)
    d = domain(f)
    r = codomain(f)
    println("Domain : ", d)
    println("Codomain : ", r)
    for x in tablegen(f)
        println(x)
    end
end

# returns generator of type x => f(x)
function tablegen(f :: AbstractDiscreteFunction)
    d = domain(f)
    res = (x => f(x) for x in d)
    return res
end


# THE BASIC DIFFERENTIATION RULES
# https://www.mathbootcamps.com/power-rule-derivatives/ = power-rule-derivatives
# https://www.dummies.com/education/math/calculus/the-basic-differentiation-rules/ for dummies

# The constant rule
 f(x) = 5 # 5 is a horizontal line with a slope of zero, and thus its derivative is also zero

# The power rule
 f(x) = f^5
# To find its derivative take the 5 power, pull it to the front and reduce the power by 1 thus
  f(x) = 5f^4 # derivative

# Negative power
  f(x) = x^-2
# negative power derivative
 f(x) = 2x^-3 # pull power in front and reduce power by 1 , this ^-2 becomes ^-3

# find the derivative of the function
 y = 2x^4 - 5x^2 + 1
# enclose in brackets
 y' = (2x^4)' - (5x^2)' + (1)'
# factor out coefficients
 y' = 2(x^4)' - 5(x^2)' + (1)'
# apply the power rule
y' = 2(4x^3)' - 5(2x^1)' + (0) # note the constant rule of 1, 1 is a line with a slope of 0 and derivative also 0
# answer is to now simplify
= 8x^3 - 10x

# Find the derivative of the function
y = 5x^3 - 3x^2 + 10x - 8
# enclose in brackets
y' = (5x^3)' - (3x^2)' + (10x)' - (8)'
# factor out the coefficients
y' = 5(x^3)' - 3(x^2)' + 10(x^0)' - (8)' # note - since x is byitself we can write 1x^0 anything > 0 to the power of 0 is 1
# apply the power rule
y' = 5(3x^2)' - 3(2x^1)' + 10(x^0)' - (0)' # note the constant rule of 8, 8 is a line with a slope of 0 and derivative also 0
# now simplify
= 15x^2 - 6x + 10x

# find the derivative of the function
y = 2/x^4 - 1/x^2

# apply rule of negative exponents
y = 2x^-4 - x^-2

# enclose in brackets
y = (2x^-4) - (x^-2)

# factor out the coefficients
y = 2(x^-4) - (x^-2)

# power rule
y = 2(4x^-4-1) - (2x^-2-1) # OR reduce by 1 = 2(4x^-5) - (2x^-3)

# simplify
 -8x^-5 + (2x^-3)
 = 8 / x^5 + 2 / x^3 # only negative terms go to the bottom

 # Derivatives of functions with radicals (square roots and other roots)
 # You can differentiate radical functions by rewriting them as power functions and then using the power rule.
 f(x) = 3 sqrt(x^2)
 # rewrite as
 f(x) = x^2/3

 # find the derivative of the function
 y = 4 sqrt(x) -6 3 sqrt(x^2)
# rewrite function using algebra
y = 4x^1/2 - 6x^2/3
# enclose in brackets
y = (4x^1/2) - (6x^2/3)
# factor out the coefficients
y = 4(x^1/2) - 6(x^2/3)
# power rule
y = 4(1/2x^1/2-1) - 6(2/3x^2/3-1) # note ^2/3 becomes 1/3 as subtract from numerator
y = 2x^-1.5 - 4x^-1/3
# convert to fraction pulling negative exponents to the bottom
y = 2 / x^1.5 - 4 / x^1/3

= 2 / sqrt(x) - 4 / 3 sqrt(x)

# power rule is then used in the chain rule and quotient rule etc...

# https://www.mathbootcamps.com/the-product-rule/ - Product rule

# tomorrow 5.14.2019

# https://www.mathbootcamps.com/the-chain-rule/ - chain rule
# chain rule
f'(x) = ( g(h(x)))' = g'(h(x)) h'(x)

# g = outside function
# h = inside function
""" to find the derivative of f(x) then you can do this by taking the derivative of outside function g and then multiplying it by its derivative of the function h.
In other words you are finding the derivative of f(x) by finding the derivative of its pieces
"""

# Examples of applying the chain rule
""" as we apply the chain rule we will always focus on figuring out what the outside and inside functions are
"""
# find the derivative of the function
f'(x) = (3x +1) ^5
# inside function = 3x + 1
# outside function = ^5

""" you know by the power rule that the derivative of x^5 = 5x^4
So, cover up that 3x+1, and pretend it is an x for a minute. The only deal is, you will have to pay a penalty. Since it was actually not just an x, you will have to multiply by the derivative of the 3x+1
"""

# Step 1 = keep the inside
# Step 2 = take the derivative of the outside
# step 3 = multiply inside by derivative of the outside
f'(x) = 5(3x +1)^4 (3x +1)'
f'(x) = 5(3x +1)^4 (3)

# simplyfy
f'(x) = 15(3x + 4)^4  # when multiply the inside by the outside = 5 * 3 = 15 then that leaves the x and x * 3 = 3x and 1 * 3 = 1

#=============================================
# final = https://medium.com/@pdquant/all-the-backpropagation-derivatives-d5275f727f60
#=============================================