using BinningAnalysis
using Test, Statistics, Random

@testset "All Tests" begin

    @testset "Logarithmic Binning" begin
        include("logbinning.jl")
    end

    @testset "Full Binning" begin
        include("fullbinning.jl")
    end

    @testset "Error Propagator" begin
        include("errorpropagator.jl")
    end

    @testset "Incremental Binning" begin
        include("incrementbinner.jl")
    end

    @testset "Generic functions" begin
        x = [0.0561823, 0.846613, 0.813439, 0.357134, 0.157445, 0.103298, 0.948842, 0.629425, 0.290206, 0.00695332, 0.869828, 0.949165, 0.897995, 0.916239, 0.457564, 0.349827, 0.398683, 0.264218, 0.72754, 0.934315, 0.666448, 0.134813, 0.364933, 0.829088, 0.256443, 0.595029, 0.172097, 0.241686, 0.489935, 0.239663, 0.391291, 0.00751015, 0.138935, 0.0569876, 0.571786, 0.694996, 0.798602, 0.923308, 0.73978, 0.414774, 0.835145, 0.731303, 0.271647, 0.707796, 0.00348624, 0.0905812, 0.316176, 0.921054, 0.131037, 0.599667, 0.805071, 0.440813, 0.086516, 0.363658, 0.476161, 0.931257, 0.28974, 0.78717, 0.60822, 0.144024, 0.214432, 0.061922, 0.626495, 0.512072, 0.758078, 0.840485, 0.242576, 0.147441, 0.599222, 0.993569, 0.0365044, 0.0983033, 0.713144, 0.422394, 0.480044, 0.968745, 0.518475, 0.431319, 0.4432, 0.526007, 0.612975, 0.468387, 0.262145, 0.888011, 0.105744, 0.325821, 0.769525, 0.289073, 0.336083, 0.443037, 0.489698, 0.141654, 0.915284, 0.319068, 0.341001, 0.704346, 0.0794996, 0.0412352, 0.70016, 0.0195158]
        @test std_error(x) ≈ std_error(x; method=:log)
        @test std_error(x; method=:log) ≈ 0.03384198827323159
        @test std_error(x; method=:full) ≈ 0.030761063167290947
        @test std_error(x; method=:jackknife) ≈ 0.029551526503551463
        @test std_error(x; method=:error_propagator) ≈ 0.03384198827323159
        @test tau(x) ≈ tau(x; method=:log)
        @test tau(x; method=:log) ≈ 0.15572524869061377
        @test tau(x; method=:full) ≈ 0.04176737474771308

        x = x .+ 1im
        @test std_error(x)≈ std_error(x; method=:log)
        @test std_error(x; method=:log) ≈ 0.0338419882732316
        @test std_error(x; method=:full) ≈ 0.03076106316729094
        @test std_error(x; method=:jackknife) ≈ 0.029551526503551445
        @test std_error(x; method=:error_propagator) ≈ 0.03384198827323173
        @test tau(x) ≈ tau(x; method=:log)
        @test tau(x; method=:log) ≈ 0.15572524869061377
        @test tau(x; method=:full) ≈ 0.04176737474771308

        x = Array{Float64,2}[[0.311954 0.476706 0.314101; 0.420978 0.478085 0.0194284], [0.491386 0.493583 0.295477; 0.136896 0.1634 0.84641], [0.318536 0.705903 0.121377; 0.764174 0.240484 0.25894], [0.286923 0.172458 0.392881; 0.124348 0.140628 0.730131], [0.287451 0.221914 0.382938; 0.29568 0.249575 0.87685]]
        @test std_error(x) ≈ std_error(x; method=:log)
        @test std_error(x, method=:log) ≈ [0.03856212859659066 0.09765048982846934 0.04879645809318544; 0.11744109653881814 0.05978167652935139 0.1722342286233907]
        @test std_error(x, method=:full) ≈ [0.03856212859659072 0.09765048982846936 0.048796458093185405; 0.11744109653881814 0.05978167652935137 0.17223422862339066]
        @test std_error(x, method=:error_propagator) ≈ [0.03856212859659066 0.09765048982846934 0.04879645809318544; 0.11744109653881814 0.05978167652935139 0.1722342286233907]
        @test tau(x) ≈ tau(x; method=:log)
        @test tau(x; method=:log) ≈ [0.0 0.0 0.0; 0.0 0.0 0.0]
        @test isapprox(tau(x; method=:full), [0.0 -5.55112e-17 -5.55112e-17; -1.11022e-16 0.0 0.0], atol=1e-16)

        x = ["this", "should", "error"]
        @test_throws ErrorException std_error(x, method=:jackknife)
        @test_throws ArgumentError std_error(x, method=:whatever)
    end

    @testset "Jackknife" begin
        Random.seed!(123)
        g(x1, x2) = x1^2 - x2

        # Real
        ts = rand(Float64, 1000)
        ts2 = rand(Float64, 1000)

        @test isapprox.(
            jackknife_full(identity, ts),
            (0.5032466005013134, 1.1091128016005314e-13, 0.008897060002353763),
            atol = 1e-12
        ) |> all
        @test isapprox.(
            jackknife_full(identity, 1 ./ ts),
            (7.97707906607684, -5.323741447682551e-12, 2.522127468106169),
            atol = 1e-12
        ) |> all
        @test isapprox.(
            jackknife_full(g, ts, ts2.^2),
            (-0.05969375347674344, 7.915767673482427e-5, 0.013117185693279606),
            atol = 1e-12
        ) |> all

        # check consistency with Julia's Statistics
        @test jackknife(identity, ts)[2] ≈ std(ts)/sqrt(length(ts))

        # Complex
        ts = rand(ComplexF64, 1000)
        ts2 = rand(ComplexF64, 1000)

        @test isapprox.(
            jackknife_full(identity, ts),
            (0.5026931517594448 + 0.49854565281725627im, 4.4364512064021255e-13 - 3.327338404801594e-13im, 0.012946143510730868),
            atol = 1e-12
        ) |> all
        @test isapprox.(
            jackknife_full(identity, 1 ./ ts),
            (1.1496877962692906 - 1.0838709685240246im, 1.3309353619206377e-12 - 6.654676809603188e-13im, 0.05676635162628594),
            atol = 1e-12
        ) |> all
        @test isapprox.(
            jackknife_full(g, ts, ts2.^2),
            (0.02004836872405491 - 0.01825289014938747im, -3.1784715169222433e-6 + 5.949834723031899e-7im, 0.026446578773003968),
            atol = 1e-12
        ) |> all

        # check consistency with Julia's Statistics
        @test jackknife(identity, ts)[2] ≈ std(ts)/sqrt(length(ts))
    end
end


using Playground
using Test
using LinearAlgebra

using Distances
using IterTools
using LightGraphs
using SimpleWeightedGraphs
using NearestNeighbors
using StaticArrays
using Plots
using RecipesBase

n = 1000
# Datasets:
# Two perpendicular lines at 0.5 distance apart.
const two_lines = (vcat([@SVector[0.0, rand()] for _ in 1:n÷2],
                        [@SVector[rand(), 0.5] for _ in 1:n÷2]),
                   Euclidean(),
                   0.4)
# 2-Torus
const torus = let θ = rand(n), φ = rand(n), R = 3, r = 1
    ([@SVector[(R+r*cospi(2θ[i]))cospi(2φ[i]),
               (R+r*cospi(2θ[i]))sinpi(2φ[i]),
               r*sinpi(2θ[i])] for i in 1:n],
     Euclidean(),
     1.0)
end
# 3-sphere
const sphere3 = ([normalize(@SVector randn(4)) for _ in 1:n],
                 Euclidean(),
                 1.0)
# Integer points on a grid, manhattan distance.
const grid2 = ([@SVector[i, j, k] for i in 1:10 for j in 1:10 for k in 1:10],
               Cityblock(),
               5)

const datasets = (two_lines, torus, sphere3, grid2)

include("equilateraliterator.jl")
include("geodesiccomplex.jl")


# This file is a part of JuliaFEM.
# License is MIT: see https://github.com/JuliaFEM/ModelReduction.jl/blob/master/LICENSE

using FEMBase

"""
    craig_bampton(K, M, r, l, n)

Reduce the stiffness and mass matrices with the Craig-Bampton method.
K = original stiffness matrix, M = original mass matrix,
r = retained DOF:s, l = internal DOF:s, n = the number of modes to keep.

"""
function craig_bampton(K, M, r, l, n)
    Krr = K[r,r]; Krl = K[r,l]
    Klr = K[l,r]; Kll = K[l,l]
    Mrr = M[r,r]; Mrl = M[r,l]
    Mlr = M[l,r]; Mll = M[l,l]
    X = zeros(size(Kll,1), n)
    if issparse(K) && issparse(M)
        SparseArrays.dropzeros!(Kll)
        nz = get_nonzero_rows(Kll)
        w, X[nz,:] = eigs(Kll[nz,nz], Mll[nz,nz]; nev=n, which=:SM)
        kll = (Kll + Kll')/2
    else
        w2 = eigvals(Kll,Mll)
        w = w2[1:n]
        X1 = eigvecs(Kll,Mll)
        X = X1[:,1:n]
    end
    Kmm = X'*Kll*X
    P = Krl*X
    Q = Mrl*X
    invw = diagm(inv.(w))
    Kbb = Krr - P*invw*P'
    Kmb = zeros(n, length(r))
    Kbm = zeros(length(r), n)
    MU = X'*Mll*X
    Mbb = Mrr - Q*invw*P' - P*invw*Q' + P*invw*MU*invw*P'
    Mmb = Q' - MU*invw*P'
    Mbm = Q - P*invw*MU
    Mmm = MU
    # Final system:
    Mred = [Mbb Mbm; Mmb Mmm]
    Kred = [Kbb Kbm; Kmb Kmm]
    Z = 10e-10
    Mred[abs.(Mred) .< Z] = 0.0
    Kred[abs.(Kred) .< Z] = 0.0
    return Mred, Kred
end

"""
    craig_bampton(K, M, c, r, l, n)

Reduce the stiffness, damping and mass matrices with the Craig-Bampton method.
K = original stiffness matrix, M = original mass matrix, c = damping ratio,
r = retained DOF:s, l = internal DOF:s, n = the number of modes to keep.

"""
function craig_bampton(K, M, c, r, l, n)
    Krr = K[r,r]; Krl = K[r,l]
    Klr = K[l,r]; Kll = K[l,l]
    Mrr = M[r,r]; Mrl = M[r,l]
    Mlr = M[l,r]; Mll = M[l,l]
    if issparse(K) && issparse(M)
        w2, X1 = eigs(Kll, Mll)
    else
        w2 = eigvals(Kll,Mll)
        X1 = eigvecs(Kll,Mll)
    end
    X = X1[:,1:n]
    V = X1'*Kll*X1; Z = 10e-6
    Kmm = X'*Kll*X
    b = -X*inv(Kmm)*X'*Klr
    B = -X1*inv(V)*X1'*Klr
    Kbb = Krr + Krl*B
    Kbm = (Krl + b'*Kll)*X
    Kmb = X'*(Klr + Kll*b)
    Mbb = Mrr + Mrl*B + B'*Mlr + B'*Mll*B
    Mmm = X'*Mll*X
    Mbm = (Mrl + b'*Mll)*X
    Mmb = X'*(Mlr + Mll*b)
    Cbb = zeros(Kbb)
    Cbm = zeros(Kbm)
    Cmb = zeros(Kmb)
    Kmm[abs.(Kmm) .< Z] = 0.0
    Cmm = 2*c*sqrt.(Kmm)
    Mred = [Mbb Mbm; Mmb Mmm]
    Mred[abs.(Mred) .< Z] = 0.0
    Kred = [Kbb Kbm; Kmb Kmm]
    Kred[abs.(Kred) .< Z] = 0.0
    Cred = [Cbb Cbm; Cmb Cmm]
    Cred[abs.(Cred) .< Z] = 0.0
    return Mred, Cred, Kred
end


function Likelyhood_Ratio_test(simple,full)
    degrees = dof(full) - dof(simple)
    ccdf(Distributions.Chisq(degrees), deviance(simple) - deviance(full))
end

function AIC_test(candidate, simpler)
    exp((aic(candidate) - aic(simpler))/2)
end
function AICc(model)
    aic(model) + ((2*(dof(model)^2) + 2*dof(model))/(nobs(model) - dof(model) - 1))
end
function AICc_test(candidate, simpler)
    exp((AICc(candidate) - AICc(simpler))/2)
end


# Add tests to the quote for functions with  FastMath varients.
function jacobian_via_frule(f,z)
    du_dx, dv_dx = reim(frule((Zero(), 1),f,z)[2])
    du_dy, dv_dy = reim(frule((Zero(),im),f,z)[2])
    return [
        du_dx  du_dy
        dv_dx  dv_dy
    ]
end
function jacobian_via_rrule(f,z)
    _, pullback = rrule(f,z)
    du_dx, du_dy = reim(pullback( 1)[2])
    dv_dx, dv_dy = reim(pullback(im)[2])
    return [
        du_dx  du_dy
        dv_dx  dv_dy
    ]
end

function jacobian_via_fdm(f, z::Union{Real, Complex})
    fR2((x, y)) = (collect ∘ reim ∘ f)(x + im*y)
    v = float([real(z)
               imag(z)])
    j = jacobian(central_fdm(5,1), fR2, v)[1]
    if size(j) == (2,2)
        j
    elseif size(j) == (1, 2)
        [j
         false false]
    else
        error("Invalid Jacobian size $(size(j))")
    end
end

function complex_jacobian_test(f, z)
    @test jacobian_via_fdm(f, z) ≈ jacobian_via_frule(f, z)
    @test jacobian_via_fdm(f, z) ≈ jacobian_via_rrule(f, z)
end

const FASTABLE_AST = quote
    @testset "Trig" begin
        @testset "Basics" for x = (Float64(π)-0.01, Complex(π, π/2))
            test_scalar(sin, x)
            test_scalar(cos, x)
            test_scalar(tan, x)
        end
        @testset "Hyperbolic" for x = (Float64(π)-0.01, Complex(π-0.01, π/2))
            test_scalar(sinh, x)
            test_scalar(cosh, x)
            test_scalar(tanh, x)
        end
        @testset "Inverses" for x = (0.5, Complex(0.5, 0.25))
            test_scalar(asin, x)
            test_scalar(acos, x)
            test_scalar(atan, x)
        end
        @testset "Multivariate" begin
            @testset "sincos" begin
                x, Δx, x̄ = randn(3)
                Δz = (randn(), randn())

                frule_test(sincos, (x, Δx))
                rrule_test(sincos, Δz, (x, x̄))
            end
        end
    end

    @testset "exponents" begin
        for x in (-0.1, 6.4)
            test_scalar(inv, x)

            test_scalar(exp, x)
            test_scalar(exp2, x)
            test_scalar(exp10, x)
            test_scalar(expm1, x)

            test_scalar(cbrt, x)

            if x >= 0
                test_scalar(sqrt, x)
                test_scalar(log, x)
                test_scalar(log2, x)
                test_scalar(log10, x)
                test_scalar(log1p, x)
            end
        end
    end

    @testset "Unary complex functions" begin
        for f ∈ (abs, abs2, conj), z ∈ (-4.1-0.02im, 6.4, 3 + im)
            @testset "Unary complex functions f = $f, z = $z" begin
                complex_jacobian_test(f, z)
            end
        end
        # As per PR #196, angle gives a Zero() pullback for Real z and ΔΩ, rather than
        # the one you'd get from considering the reals as embedded in the complex plane
        # so we need to special case it's tests
        for z ∈ (-4.1-0.02im, 6.4 + 0im, 3 + im)
            complex_jacobian_test(angle, z)
        end
        @test frule((Zero(), randn()), angle, randn())[2] === Zero()
        @test rrule(angle, randn())[2](randn())[2]        === Zero()
    end

    @testset "Unary functions" begin
        for x in (-4.1, 6.4)
            test_scalar(+, x)
            test_scalar(-, x)
        end
    end

    @testset "binary function ($f)" for f in (/, +, -, hypot, atan, rem, ^, max, min)
        x, Δx, x̄ = 10rand(3)
        y, Δy, ȳ = rand(3)
        Δz = rand()

        frule_test(f, (x, Δx), (y, Δy))
        rrule_test(f, Δz, (x, x̄), (y, ȳ))
    end

    @testset "sign" begin
        @testset "real" begin
            @testset "at $x" for x in (-1.1, -1.1, 0.5, 100.0)
                test_scalar(sign, x)
            end

            @testset "Zero over the point discontinuity" begin
                # Can't do finite differencing because we are lying
                # following the subgradient convention.

                _, pb = rrule(sign, 0.0)
                _, x̄ = pb(10.5)
                @test extern(x̄) == 0

                _, ẏ = frule((Zero(), 10.5), sign, 0.0)
                @test extern(ẏ) == 0
            end
        end
        @testset "complex" begin
            @testset "at $z" for z in (-1.1 + randn() * im, 0.5 + randn() * im)
                test_scalar(sign, z)

                # test that complex (co)tangents with real primal gives same result as
                # complex primal with zero imaginary part

                ż, ΔΩ = randn(ComplexF64, 2)
                Ω, ∂Ω = frule((Zero(), ż), sign, real(z))
                @test Ω == sign(real(z))
                @test ∂Ω ≈ frule((Zero(), ż), sign, real(z) + 0im)[2]

                Ω, pb = rrule(sign, real(z))
                @test Ω == sign(real(z))
                @test pb(ΔΩ)[2] ≈ rrule(sign, real(z) + 0im)[2](ΔΩ)[2]
            end

            @testset "zero over the point discontinuity" begin
                # Can't do finite differencing because we are lying
                # following the subgradient convention.

                _, pb = rrule(sign, 0.0 + 0.0im)
                _, z̄ = pb(randn(ComplexF64))
                @test extern(z̄) == 0.0 + 0.0im

                _, Ω̇ = frule((Zero(), randn(ComplexF64)), sign, 0.0 + 0.0im)
                @test extern(Ω̇) == 0.0 + 0.0im
            end
        end
    end
end

# Now we generate tests for fast and nonfast versions
@eval @testset "fastmath_able Base functions" begin
    $FASTABLE_AST
end

@eval @testset "fastmath_able FastMath functions" begin
    $(Base.FastMath.make_fastmath(FASTABLE_AST))
end


import Winston
import DSP

#==============================================================================#
#                 Plot Filter's Impulse & Frequency Response                   #
#==============================================================================#
# x = vector containing a filter's impulse response
#
# Example:
#    plot_response( firdes(0.5, kaiser(37, 5.653) ))
# See example 7.8 in DTSP

function freqz( coefficients::Vector )
    if !method_exists( Winston.plot, ())
        error( "To use plot_response, you must load the Winston package")
    end
    x  = coefficients
    xx = [x, zeros(1024 - length(x))]
    M  = [0:length(x) - 1]
    n  = int(ceil(length( x )/2))
    X  = fft(xx)[1:512]
    X  = abs(X)
    X  = 20*log10( X )
    f  = linspace( 0, 1, 512 )

    impulse = Winston.stem( M,
                            x,
                            title  = "Impulse Response",
                            xlabel = "Tap Index",
                            ylabel = "Amplitude"
                          )
    Winston.setattr(impulse.frame1, draw_grid=true)

    freq = Winston.FramedPlot(
                            title  = "Frequency Response",
                            xlabel = "f/f_{Nyquist}",
                            ylabel = "dB"
                        )
    Winston.add(freq, Winston.Curve(f, X))
    Winston.setattr(freq.frame1, draw_grid=true)

    t = Winston.Table(2, 1)
    t[1,1] = impulse
    t[2,1] = freq

    return t
end

#==============================================================================#
#                               Plot Constellation                             #
#==============================================================================#

function plot_constellation( symbols::Vector )
    p = Winston.scatter( symbols,
                         ".",
                         xlabel = "In Phase",
                         ylabel = "Quadrature"
                       )

    return p
end

#==============================================================================#
#                                Plot Spectrum                                 #
#==============================================================================#

function plot_spectrum( signal::Vector, sample_rate::Real = 1.0 )
    spectrum = DSP.periodogram( signal )
    spectrum = 10*log10( spectrum.^2 )
    spectrum = fftshift( spectrum )

    frequencies = linspace( -sample_rate/2, sample_rate/2, length(spectrum) )

    spectrum_plot = Winston.FramedPlot(
                            title  = "Spectrum",
                            xlabel = "frequency",
                            ylabel = "dB"
                        )
    Winston.add(spectrum_plot, Winston.Curve( frequencies, spectrum ))

    return spectrum_plot
end


function gen_initialstate(N,kets)

    ketCoeff = zeros(Complex128,length(kets),1)
    parameterArray = zeros(Int,length(kets),2)

    for (i,str) in enumerate(kets)
        # First we find where the ket starts
        ketstart = search(str,'|')
        # Grabbing the coefficient in front of ket
        coeff = str[1:ketstart-1]
        if isempty(coeff)
            ketCoeff[i] = 1
        elseif coeff == '-'
            ketCoeff[i] = -1
        else
            ketCoeff[i] = eval(parse(coeff))
        end
        # Grabbig qubit state
        if str[ketstart+1] == 'g'
            parameterArray[i,1] = 0
        elseif str[ketstart+1] == 'e'
            parameterArray[i,1] = 1
        else
            error("Error, qubit state should be 'g' or 'e'")
        end
        # Grabbing resonator state
        parameterArray[i,2] = parse(Int,str[ketstart+3:end-1])
    end

    # Making sure that N is large enough
    if N <=  maximum(parameterArray[:,2])
        error("The size of the resonator space (N) must be at least the size of the larger number state plus one (due to zero state)!")
    end

    # Assembling the state density matrix
    state = zeros(Complex128,2N,2N)

    for i = 1:length(kets), j = 1:length(kets)
        # Defining the ket
        qubit_ket = zeros(Complex128,2,1) # Initialising the qubit ket (2D)
        qubit_ket[parameterArray[i,1]+1] = 1 # Setting the vector as (1 0) for g or (0 1) for e
        resonator_ket = zeros(Complex128,N,1) # Initialising the resonator ket (ND)
        resonator_ket[parameterArray[i,2]+1] = 1 # Setting the vector
        ket = qubit_ket ⊗ resonator_ket # Tensor product
        # Defining the bra
        qubit_bra =zeros(Complex128,2,1)
        qubit_bra[parameterArray[j,1]+1] = 1
        resonator_bra = zeros(Complex128,N,1)
        resonator_bra[parameterArray[j,2]+1] = 1
        bra = (qubit_bra ⊗ resonator_bra)'
        # Multiplying the initial ket with its complex conjugate to make
        # the density matrix
        state = state + ketCoeff[i]*ketCoeff[j]'*ket*bra
    end

    # Return normalized state
    @compat return state/sum(abs.(ketCoeff).^2)
end

function gen_hamiltonian(N,ω_q,ω_r,g;rwa=true,noisy=false)
    # This function generates a times invariant matrix for the Jaynes-Cummings
    # Hamiltonian. The parameters ω_r, ω_q and g should be given in
    # rad/s. The N parameter is the highest energy level of the harmonic
    # oscillator that is considered. Higher levels are truncated.

    # The Jaynes-Cummings Hamiltonian is written with the following
    # conventions: ω_r is the resonator frequency, ω_q is the qubit
    # frequency and g is the coupling. The resonator Hilbert space is truncated
    # to n = N, and the ground state of the qubit is given by the vector
    # [1;0] (hence the minus sign before σ_z).

    H = ħ * ω_r * qeye(2) ⊗ (a_dagger(N)*a(N)) - # resonator
        0.5 * ħ * ω_q * σ_z ⊗ qeye(N) # qubit

    if rwa # interaction, with or without rotating wave approximation
        H += ħ * g * ( σ_plus ⊗ a(N) + σ_minus ⊗ a_dagger(N) )
    else
        H += ħ * g * (σ_plus + σ_minus) ⊗ (a(N) + a_dagger(N))
    end

    if noisy
        noise = rand(Complex128,2N,2N)
        H += 0.01 * ħ * g * (noise'+noise)
    end

    return H
end

function gen_drive_hamiltonian(ω_d,N,samples=16)
    Δt = (2π/ω_d)/samples # Calculate Δt based on the desired number of samples per period
    time_vec = linspace(0,2π/ω_d,samples+1)[1:end-1] # Calculate operator for a full period
    I_m = qeye(2)
    adag_m = a_dagger(N)
    a_m = a(N)
    H_d_array = map(time_vec) do t
        ħ * I_m ⊗ (adag_m*exp(-im * ω_d * t) + a_m*exp(im * ω_d * t))
    end
    return Δt, H_d_array
end

function gen_timeevoarray(H,t_f,samples)
    # Generate the time evolution operators for a time-independent Hamiltonian
    times = linspace(0,t_f,samples+1)[1:end-1]
    U_array = map(times) do t
        expm(-1.0im * H * t / ħ)
    end
    return times, U_array
end

function gen_displacementop(α,N)
    # Generate the harmonic oscillator displacement operator
    expm( α * a_dagger(N) - α' * a(N) )
end

function qeye(d)
    eye(Complex128,d)
end

function a(N) # Truncated matrix for the lowering operator
    out = zeros(Complex128,N,N)
    for i = 1:N-1
        out[i,i+1] = sqrt(i)
    end
    return out
end

function a_dagger(N) # Truncated matrix for the raising operator
    out = zeros(Complex128,N,N)
    for i = 1:N-1
        out[i+1,i] = sqrt(i)
    end
    return out
end


using BasisFunctions: UnitInterval, PolynomialBasis
using BasisFunctions: hasderivative, hasantiderivative, ordering, support
using BasisFunctions: supremum, infimum
using BasisFunctions.Test: suitable_function

const PBSplineInterval = UnitInterval
const PBSplineIndex = NativeIndex{:pbspline}

# standard grid generation
get_pbspline_nodes(T,n) = collect(LinRange{T}(0.0, 1.0, n))

function check_equispaced(ξ::Vector{T}) where {T}
    Δ = ξ[2] - ξ[1]
    for i in 1:(length(ξ)-1)
        abs(ξ[i+1] - ξ[i] - Δ) > 1/length(ξ) * 1e-2 ? (return false) : nothing
    end
    true
end

# constructors
struct PBSpline{T} <: PolynomialBasis{T}
    p :: Int                    # spline degree
    nodes :: ScatteredGrid{T}   # knot points
    # n :: Int                    # number of splines
    # L :: T                      # domain length
    # h :: T                      # L/n

    function PBSpline(p::Int, nodes::ScatteredGrid{T}) where {T}
        @assert length(nodes) > p
        @assert check_equispaced(nodes.points)

        new{T}(p,nodes)
    end
end

# constructors for unit interval grid
PBSpline(p::Int, n::Int) = PBSpline(p, get_pbspline_nodes(Float64,n))
PBSpline(p::Int, ξ::Vector{T}) where {T} = PBSpline(p, ScatteredGrid(ξ, PBSplineInterval{T}()))

# n splines of degree p=n-1
PBSpline(n::Int) = PBSpline(n-1, get_pbspline_nodes(Float64,n))
PBSpline{T}(n::Int) where {T} = PBSpline(n-1, get_pbspline_nodes(T,n))
PBSpline(ξ::Vector{T}) where {T} = PBSpline(length(ξ)-1, ξ)

# constructor for interval [a,b)
PBSpline(p, ξ, a::Number, b::Number) = rescale(PBSpline(p, ξ), a, b)

# basis properties
nodes(b::PBSpline)  = b.nodes.points
nnodes(b::PBSpline) = length(b.nodes.points)
degree(b::PBSpline) = b.p
Base.size(b::PBSpline) = (nnodes(b),)

# convenience
L_domain(b::PBSpline) = nodes(b)[end] - nodes(b)[1]
invh_elements(b::PBSpline) = nnodes(b)/L_domain(b)

BasisFunctions.native_index(b::PBSpline, idx) = PBSplineIndex(idx)
BasisFunctions.ordering(b::PBSpline) = Base.OneTo(nnodes(b))
BasisFunctions.period(b::PBSpline{T}) where {T} = one(T)
BasisFunctions.support(b::PBSpline{T}) where {T} = PBSplineInterval{T}()

BasisFunctions.interpolation_grid(b::PBSpline{T}) where {T} = PeriodicEquispacedGrid(length(b), PBSplineInterval{T}())
BasisFunctions.hasinterpolationgrid(b::PBSpline) = true

BasisFunctions.similar(::PBSpline, ::Type{T}, n::Int) where {T} = PBSpline{T}(n)
BasisFunctions.similar(::PBSpline, ::Type{T}, ξ::Vector{T}) where {T} = PBSpline(ξ)

# for testing purposes
BasisFunctions.Test.suitable_function(s::PBSpline) = x->exp(cos(2π*x/(supremum(support(s)) - infimum(support(s)))))

# evaluation at point x
function _pbspline(b::PBSpline{T}, i::PBSplineIndex, x::T,
                    n::Int = nnodes(b), invh::T = invh_elements(b)) where {T}
    x -= nodes(b)[1]; x *= invh; x -= (i-1)
    # periodicity
    while x < zero(T)  x += n end
    while x > n        x -= n end
    return _unitpbspline(b.p,x)
end

_pbspline(b::PBSpline, i, x) = _pbspline(b, native_index(b,i), x)

# evaluation of "unit" spline with support (0,n+1)
function _unitpbspline(p::Int, x::T) where {T}
    if p == 0
        return zero(T) <= x < one(T) ? one(T) : zero(T)
    elseif x > p+1
        return zero(T)
    else
        return x/p * _unitpbspline(p-1,x) + (p+1-x)/p * _unitpbspline(p-1,x-1)
    end
end

function BasisFunctions.unsafe_eval_element(b::PBSpline, i, x)
    _pbspline(b, i, x)
end

function _pbspline_derivative(b::PBSpline{T}, i::PBSplineIndex, x::T,
                    n::Int = nnodes(b), invh::T = invh_elements(b)) where {T}
    x -= nodes(b)[1]; x *= invh; x -= (i-1)
    # periodicity
    while x < zero(T)  x += n end
    while x > n        x -= n end
    invh * (_unitpbspline(b.p-1, x) - _unitpbspline(b.p-1, x-1 < 0 ? x-1+n : x-1) )
end

function BasisFunctions.unsafe_eval_element_derivative(b::PBSpline, i, x, nᵢ::Int = nnodes(b))
    _pbspline_derivative(b, i, x)
end


LONG_SCALE = float(0xFFFFFFFFFFFFFFF)

maybeAppendUnit(unit::Any, appended_unit) = maybeAppendUnit([unit], appended_unit)
function maybeAppendUnit(unit::AbstractArray, appended_unit)
  if length(appended_unit) > 0
    push!(unit, string(appended_unit))
  end
  return unit
end

function getUnit(op::AbstractPlanOutOpRandom, appended_unit="")
  unit = getArgMixed(op, "unit")
  maybeAppendUnit(unit, appended_unit)
end

function getHash(op::AbstractPlanOutOpRandom, i::AbstractPlanOutMapper, appended_unit="")
  if hasArg(op, "full_salt")
    full_salt = getArg(op, "full_salt")
  else
    salt = getArgString(op, "salt")
    experiment_salt = i.salt
    full_salt = "$experiment_salt.$salt"
  end
  unit_str = join(getUnit(op, appended_unit), ".")
  hash_str = "$full_salt.$unit_str"::ASCIIString
  hex = hexdigest("sha1", hash_str)[1:15]
  parse(Int, "0x$hex")
end

function getUniform(op::AbstractPlanOutOpRandom, i::AbstractPlanOutMapper, min_val=0.0, max_val=1.0, appended_unit="")
  zero_to_one = getHash(op, i, appended_unit) / LONG_SCALE
  min_val + (max_val - min_val) * zero_to_one
end

type RandomFloat <: AbstractPlanOutOpRandom
  args::Dict{AbstractString, Any}
end
randomfloat(args::Dict) = RandomFloat(args)

randExecute(op::RandomFloat, i::AbstractPlanOutMapper) = getUniform(op, i, getArgNumeric(op, "min"), getArgNumeric(op, "max"))

type RandomInteger <: AbstractPlanOutOpRandom
  args::Dict{AbstractString, Any}
end
randominteger(args::Dict) = RandomInteger(args)

function randExecute(op::RandomInteger, i::AbstractPlanOutMapper)
  min_val = getArgInt(op, "min")
  max_val = getArgInt(op, "max")

  min_val + getHash(op, i) % (max_val - min_val + 1)
end

type BernoulliTrial <: AbstractPlanOutOpRandom
  args::Dict{AbstractString, Any}
end
bernoullitrial(args::Dict) = BernoulliTrial(args)

function randExecute(op::BernoulliTrial, i::AbstractPlanOutMapper)
  p = getArgNumeric(op, "p")
  rand_val = getUniform(op, i, 0.0, 1.0)
  return Int(rand_val <= p)
end

type BernoulliFilter <: AbstractPlanOutOpRandom
  args::Dict{AbstractString, Any}
end
bernoullifilter(args::Dict) = BernoulliFilter(args)

function randExecute(op::BernoulliFilter, i::AbstractPlanOutMapper)
  p = getArgNumeric(op, "p")
  choices = getArgList(op, "choices")
  ret = []
  for i in choices
    if getUniform(op, i, 0.0, 1.0) <= p
      push!(ret, i)
    end
  end
  return ret
end

type UniformChoice <: AbstractPlanOutOpRandom
  args::Dict{AbstractString, Any}
end
uniformchoice(args::Dict) = UniformChoice(args)

function randExecute(op::UniformChoice, i::AbstractPlanOutMapper)
  choices = getArgList(op, "choices")
  if length(choices) == 0
    return []
  end
  rand_index = (getHash(op, i) % length(choices)) + 1
  return getindex(choices, rand_index)
end

type WeightedChoice <: AbstractPlanOutOpRandom
  args::Dict{AbstractString, Any}
end
weightedchoice(args::Dict) = WeightedChoice(args)

function randExecute(op::WeightedChoice, m::AbstractPlanOutMapper)
  choices = getArgList(op, "choices")
  weights = getArgList(op, "weights")
  if length(choices) == 0
    return []
  end

  cum_weights = Dict(enumerate(weights))
  cum_sum = 0.0
  for (i, j) in cum_weights
    cum_sum += j
    cum_weights[i] = cum_sum
  end
  stop_val = getUniform(op, m, 0.0, cum_sum)
  for (i, j) in cum_weights
    if stop_val <= j
      return choices[i]
    end
  end
end

type Sample <: AbstractPlanOutOpRandom
  args::Dict{AbstractString, Any}
end
sample(args::Dict) = Sample(args)

function randExecute(op::Sample, m::AbstractPlanOutMapper)
  choices = copy(getArgList(op, "choices"))
  if hasArg(op, "draws")
    num_draws = getArgInt(op, "draws")
  else
    num_draws = length(choices)
  end
  for i in reverse(1:length(choices))
    j = (getHash(op, m, i) % (i + 1)) + 1
    tmp = copy(choices[i])
    choices[i] = choices[j]
    choices[j] = tmp
  end
  choices[1:num_draws]
end


# Generate EinCode From a lattice.
using OMEinsum, LightGraphs
export eincode
function eincode(graph::Graph)
    tensor_dim = [length(neighbors(graph, i)) for i in LightGraphs.vertices(graph)]
    code = [zeros(Int, tensor_dim[i]) for i = 1:length(tensor_dim)]
    t = ones(Int, length(tensor_dim))
    edges = [i for i in LightGraphs.edges(graph)]

    for i = 1:ne(graph)
        s, d = src(edges[i]), dst(edges[i])
        code[s][t[s]] = i
        code[d][t[d]] = i
        t[src(edges[i])] += 1
        t[dst(edges[i])] += 1
    end
    return EinCode(([Tuple(i) for i in code]...,), ())
end


"""
    RProp_params
"""
struct RProp_params{T<:Real}
    eta_plus::T
    eta_minus::T
    Delta_min::T
    Delta_max::T
    Delta_0::T
end

"""
    train_with_RProp(X::Array{Float64,2}, y::Array{Float64,1}, cov_func_parameters::gaussian_kernel_hyperparameters, MaxIter::Int, noise::Float64, params::RProp_params)
Trains kriging hyperparameters with RProp.
"""
function calibrate_by_ML_with_Rprop(X::AbstractArray{T,2}, y::AbstractArray{R,1}, cov_func_parameters::gaussian_kernel_hyperparameters, MaxIter::Integer, noise::Real, params::RProp_params) where T<:Real where R<:Real
    # An implementation of this paper https://pdfs.semanticscholar.org/aa65/042ae494455a14811927eb0574871d276454.pdf
    iter = 0
    w_0 = cov_func_parameters.w_0
    w_i = cov_func_parameters.w_i
    threshold = params.Delta_min * params.eta_plus + 10*eps()
    old_0_sign = 1
    old_i_signs = ones(length(w_i))
    while iter < MaxIter
        marginal_likelihood, K, invK = marginal_likelihood_gaussian_derivatives(X, y, gaussian_kernel_hyperparameters(w_0, w_i), noise)
        Delta0 = params.Delta_0
        Deltas = Array{typeof(w_0),1}(undef, length(w_i)) .= params.Delta_0
        new_0_sign = sign(marginal_likelihood[1])
        new_i_signs = sign.(marginal_likelihood[2:length(marginal_likelihood)])
        w_0 = w_0 + new_0_sign*Delta0
        w_i = w_i .+ new_i_signs .* Deltas
        if iter > 0
            Delta0 = new_0_sign * old_0_sign > 0 ? Delta0*params.eta_plus : Delta0*params.eta_minus
            Delta0 = min(max(Delta0, params.Delta_min ), params.Delta_max )
            for i in 1:length(w_i)
                Deltas[i] = new_i_signs[i]*old_i_signs[i] > 0 ? Deltas[i]*params.eta_plus : Deltas[i] *params.eta_minus
                Deltas[i] = min(max(Deltas[i], params.Delta_min ), params.Delta_max )
            end
        end
        if all(Deltas .< threshold)
            return gaussian_kernel_hyperparameters(w_0, w_i)
        end
        old_0_sign  = new_0_sign
        old_i_signs = new_i_signs
        iter = iter + 1
    end
    return gaussian_kernel_hyperparameters(w_0, w_i)
end

"""
    sample(dim::Int, batch_size::Int, replace::Bool)
This does sampling with or without replacement.
"""
function sample(dim::Integer, batch_size::Integer)
    return randperm(dim)[1:batch_size]
end

"""
    calibrate_by_ML_with_SGD(X::Array{Float64,2}, y::Array{Float64,1}, cov_func_parameters::gaussian_kernel_hyperparameters, steps::Int, batch_size::Int, step_multiple::Float64, noise::Float64, seed::Int = 1988)
This trains a kriging model by using maximum likelihood with stochastic gradient descent.
"""
function calibrate_by_ML_with_SGD(X::AbstractArray{T,2}, y::AbstractArray{R,1}, cov_func_parameters::gaussian_kernel_hyperparameters, steps::Integer, batch_size::Integer, step_multiple::Real, noise::Real, seed::Integer = 1988) where T<:Real where R<:Real
    Random.seed!(seed)
    ow_0 = cov_func_parameters.w_0
    ow_i = cov_func_parameters.w_i
    ndims = length(ow_i)
    nobs = length(y)
    for s in 1:steps
        samples = sample(nobs,batch_size)
        XSample = X[samples, :]
        ySample = y[samples]
        marginal_likelihood, K, invK = marginal_likelihood_gaussian_derivatives(XSample, ySample, gaussian_kernel_hyperparameters(ow_0, ow_i), noise)
        normalised_grad =  sign.(marginal_likelihood) .* ( abs.(marginal_likelihood) ./ (abs.(marginal_likelihood) .+ maximum(abs.(marginal_likelihood))) )
        ow_0 = ow_0 .* (1 .+ normalised_grad[1] .* step_multiple)
        ow_i = ow_i .* (1 .+ normalised_grad[2:(ndims+1)] .* step_multiple)
    end
    return gaussian_kernel_hyperparameters(ow_0, ow_i)
end


# include("kernel.jl")
# include("DCC.jl")
# From this line
# This function is: util(c,η,s) = (1+η) c^(1-σ)/(1-σ)
mutable struct Utility
    σ::Float64
    Utility(σ::Real) = new(Float64(σ))
    function (self::Utility)(c::Union{Real,RealVector})
        if (self.σ == 1)
            return(log.(c))
        else
            return(float.(c).^(1 - self.σ)./(1 - self.σ));
        end
    end

    function (self::Utility)(c::Union{Real,RealVector},η::Union{Real,RealVector})
        if (self.σ == 1)
            return(exp.(η).*log.(c))
        else
            return(exp.(η).* (float.(c).^(1 - self.σ))./(1 - self.σ));
        end
    end

end

mutable struct Transition
    α::Real
    A::Real
    Transition(α,A)=new(α,A);
    function (self::Transition)(c::Union{Real,RealVector},s::Union{Real,RealVector})
    # return(1.1 * s.^(self.α) .- c)
    # return(α*(s-c.+ 2.0))
    return (self.A * (s.^(self.α)) .- c)
    end
end

# function Transition(s::Real,c::Real)
    # return(1.05 * ( s - c ) + 3 )
# end

# function Transition(s::RealVector,c::RealVector)
    # return(1.05 * ( s - c ) .+ 3 )
# end

# How to use sub types?
function find_optim(xin::Real,ηin::Real, lb::Real,ub::Real, β::Float64, trans::Transition, util::Utility,ValueFn::ApproxFn)
    @suppress begin
        # η = 0.01;
        ff = c ->  - (util(c' * [1],ηin) + β * ValueFn(trans(c' * [1],xin)));
        # ub = trans(0,xin[1])-lb;
        @show init_val = (ub + lb) / 2;
        f_opt = optimize(ff,[lb],[ub],[lb],SAMIN(),Optim.Options(g_tol = 1e-12,
                         iterations = 1000,
                     store_trace = false,
                     show_trace = false));
        return(f_opt.minimizer[1])
    end
end

mutable struct State
    s::Union{Real,RealVector} #Observed
    η::Real #Unobserved
    State()=new(0,0);
    function State(s::Real, η::Real)
        new([s],η);
    end

    function State(s::Array, η::Real)
        new(s,η);
    end
end

function vectorize(s::State)
    return(vcat(s.s,s.η))
end

mutable struct DynamicDecisionProcess
    σ::Float64
    util::Utility
    trans::Transition
    PolicyFn::ApproxFn
    ValueFn::ApproxFn
    β::Float64
    dtrans::Function
    dutil::Function
    nSolve::Int
    function DynamicDecisionProcess(σ::Real,β::Float64,α::Real,A::Real)
        nSolve = 400;
        v_tol = 0.001;
        ϵ     = 0.05;
        # step_length= 2/nSolve;
        step_length=ϵ;
        util  = Utility(σ);
        trans = Transition(α,A);
        # x = hcat(range(2*ϵ,step= ϵ,length=nSolve)); #Convert it into 2dimension
        s = convert(Array{Float64,1},range(ϵ,step=step_length,length=nSolve));
        # TN=TruncatedNormal(0, 1, 0, 10);
        # s = rand(TN,nSolve);
        #Convert it into 2dimension
        η = randn(nSolve)/100; #Normal distributed error
        # η=zeros(nSolve);
        # v = (util(s)./(1 -β));
        sdat = hcat(s,η);

        s = vcat([1e-10*ϵ],s);
        c_opt = zeros(nSolve);
        v_first=zeros(nSolve+1);
        PolicyFn = ApproxFn(sdat,c_opt,:gaussian,2);
        ValueFn = ApproxFn(s,v_first,:gaussian,2);
        # PolicyFn.h =  0.5 * PolicyFn.h;
        # ValueFn.h =  0.5 * ValueFn.h ;
        # ValueFn.h = 1; #Try this
        dtrans = (c,s) -> Tracker.gradient(trans,c,s);
        dutil  = (c,η) -> Tracker.gradient(util,c,η)[1].data;
        ddc = new(float(σ),util,trans,PolicyFn,ValueFn,β,dtrans,dutil,nSolve);
        return(ddc);
    end
end

# dtrans = (c,s) -> Tracker.gradient(ddc.trans,c,s) #Transition derivatives, can be broadcasted
# Compute equilibrium(): similar to Paul Schrimpf's dynamic choice problem

# While normV + normS2 + normV2 > tol (What is tole)
    # UpateValue, updateStrategy     : Update the solved value for policy and value function
    # gridUpdate && abs2(strategy.val,stratGrid.val)>strategy.val.size()*0.2
    # # UpdateSolvedGrid: To make the distribution stationary
    # stratGrid = strategy

function computeDistance(ddc::DynamicDecisionProcess,old_policy::ApproxFn,old_value::ApproxFn)
     normPolicy = abs2(old_policy.y - ddc.PolicyFn.y)./ddc.PolicyFn.n;
     normValue  = abs2(old_value.y  - ddc.ValueFn.y)./ddc.ValueFn.n;
     normData   = mean(abs2(old_value.xdata - ddc.ValueFn.xdata));
     return(normPolicy+normValue+normData);
end

function UpdateSolvedGrid!(ddc::DynamicDecisionProcess,T)
    # Update the grade for solving the points
    # ddc.policy
    η_new = randn(ddc.nSolve)/100; #Generate η
    # η_new = zeros(ddc.nSolve); #Generate η
    s_new = ddc.ValueFn.xdata;
    x_new = hcat(s_new,η_new);
    s_old = s_new;
    val_new = val_old = ddc.ValueFn.y;
    t = 0;
    while t < T
        # @show t;
        c_new = ddc.PolicyFn(x_new);
        s_new = ddc.trans(c_new,s_old);
        UpdateData(ddc.ValueFn,s_old,val_old);

        val_new = ddc.util(c_new,x_new[:,2]) + ddc.β * ddc.ValueFn(s_new);
        val_old = deepcopy(val_new);
        s_new[s_new.<0].=minimum(s_old);
        η_new = randn(ddc.nSolve); #Generate η
        x_new = hcat(s_new,η_new);
        s_diff= abs2(s_old - s_new)/ddc.nSolve;
        s_old = deepcopy(s_new);
        t += 1;
    end

end

function UpdateVal!(ddc::DynamicDecisionProcess,T::Int)
    c_opt = ddc.PolicyFn.y;
    s = ddc.PolicyFn.xdata[:,1];
    η = ddc.PolicyFn.xdata[:,2];
    iter = 0
    tol = 1e-8
    v_diff = Inf
    while (iter < T && v_diff > tol)
        ϵ = minimum(s)*0.9;
        lb = ϵ;
        for n = 1:ddc.nSolve
            ub = max(0.9 * ddc.trans(0,s[n]),ddc.trans(0,s[n])-2*ϵ);
            c_opt[n] = find_optim(s[n],η[n],lb,ub,ddc.β,ddc.trans,ddc.util,ddc.ValueFn);
        end

        y = ddc.util(c_opt) + ddc.β * ddc.ValueFn(ddc.trans(c_opt,s));
        # println("Iteration:",iter);
        # @show iter;
        # @show v_diff = mean(abs.(y - ddc.ValueFn.y));
        UpdateVal(ddc.ValueFn,vcat([ddc.ValueFn.y[1]],y));
        @show iter += 1;
    end
    UpdateVal(ddc.PolicyFn,c_opt);
end

function computeEquilibrium(ddc::DynamicDecisionProcess)
    i = 0;
    diff_v=Inf;
    diff_v_old = 0;
    diff_r=Inf;
    # tol = 1/ddc.nSolve;
    tol_r = 0.1;
    tol_v = 1e-5;
    # while (i < 30) & (diff_v > tol)
    while (i < 30) & (diff_r > tol_r) & (diff_v > tol_v)
        old_value=deepcopy(ddc.ValueFn); old_policy=deepcopy(ddc.PolicyFn);
        UpdateVal!(ddc,10);
        @show diff_v=computeDistance(ddc,old_policy,old_value);
        @show diff_r = abs(diff_v-diff_v_old) / diff_v;
        diff_v_old = diff_v;
        # UpdateSolvedGrid!(ddc::DynamicDecisionProcess,2);
        i+=1;
    end
end

# simulate dynamic discrete choice problem from the solved problem
function simulate_ddc(nM,nT,ddc::DynamicDecisionProcess)
    # lb = minimum(ddc.PolicyFn.xdata,dims=1);
    # ub = maximum(ddc.PolicyFn.xdata,dims=1);
    # if lb[2]==ub[2]
    #     ub[2] = lb[2]+1e-6
    # end
    # x0 = zeros(nM,ddc.PolicyFn.q); #Initial states
    # for i = 1:ddc.PolicyFn.q
    #     du = Uniform(lb[i],ub[i]);
    #     x0[:,i] = rand(du,nM);
    # end
    # du=DiscreteUniform(1,ddc.nSolve); #Draw from the nSolved states
    # s = zeros(nM,nT+1);
    # a = zeros(nM,nT);
    # s[:,1] = x0[:,1];
    # du = Uniform(lb[2],ub[2]);

    # du=DiscreteUniform(1,ddc.nSolve); #Draw from the nSolved states
    du=Uniform(2,20);
    s = [ State() for i=zeros(nM,nT)]; #nM x nT state
    a = [ 0.0 for i=zeros(nM,nT)]; #nM x nT action
    for n = 1:nM
        ind_1=rand(du);
        # s[n,1] = State(ddc.PolicyFn.xdata[ind_1,1:end-1],randn()/100);
        s[n,1] = State(rand(du),randn()/100);
        for t = 1:(nT-1)
        # x = hcat(s[:,t],randn(nM)/3);
            x = vectorize(s[n,t]); #This is a bit problematic
            a[n,t] = ddc.PolicyFn(x);
            s_1 = ddc.trans(a[n,t],s[n,t].s);
            if s_1[1] < 0
                @show n,t
                break
            end
            s[n,t+1] = State(s_1,randn()/100);

        end
        x = vectorize(s[n,nT]); #T
        a[n,nT] = ddc.PolicyFn(x);
    end

    return(state=s,action=a);
end

function check_ee(ddc::DynamicDecisionProcess)
    s0=rand(Uniform(3,15));
    ϵ0=0;
    a0=ddc.PolicyFn([s0,ϵ0])[1];
    s1=ddc.trans(a0,s0);
    a1=ddc.PolicyFn([s1,0])[1];
    ds=ddc.dtrans(a1,s1)[2].data;
    return(ddc.dutil(a0,0)- ds *ddc.β  * ddc.dutil(a1,0));
end

mutable struct EulerEquation
    σ::Float64
    util::Utility
    trans::Transition
    PolicyFn::ApproxFn
    β::Float64
    dtrans::Function
    dutil::Function
    nSolve::Int
    function EulerEquation(σ::Real,β::Float64,α::Real)
        v_tol = 0.001;
        ϵ     = 0.005;
        util  = Utility(σ);
        trans = Transition(α);
        nSolve = 500;
        # x = hcat(range(2*ϵ,step= ϵ,length=nSolve)); #Convert it into 2dimension
        s = convert(Array{Float64,1},range(2*ϵ+1,step=ϵ,length=nSolve)); #Convert it into 2dimension
        η = randn(nSolve)/100; #Normal distributed error
        # η=zeros(nSolve);
        # v = (util(s)./(1 -β));
        sdat = hcat(s,η);
        c_opt = zeros(nSolve).+ϵ;
        tol=1e-3; diff=Inf;max_iter=10;iter=0;
        dtrans = (c,s) -> Tracker.gradient(trans,c,s);
        dutil  = (c) -> Tracker.gradient(util,c,0)[1].data;

        PolicyFn = ApproxFn(sdat,c_opt,:gaussian,2);
        dtrans_s = (c,s) -> dtrans(c,s)[2].data;

        function f_obj(c)
            diff_val =
            dutil.(c) - β.* dtrans_s.(c,PolicyFn.xdata[:,1]) .* PolicyFn( hcat(trans.(c,PolicyFn.xdata[:,1]),zeros(nSolve)) );
            return(sum(diff_val.^2))
        end

        while (diff > tol) & (iter<max_iter)

            result = optimize(f_obj,c_opt,PolicyFn.xdata[:,1],zeros(nSolve).+ϵ,SAMIN());
            # c_opt=result.minimizer;;
            @show diff = sum( (result.minimizer-PolicyFn.y).^2);
            UpdateVal(PolicyFn,result.minimizer);
            iter+=1;
        end
        ee = new(float(σ),util,trans,PolicyFn,β,dtrans,dutil,nSolve);
        return(ee);
    end
end

function check_ee(ee::EulerEquation)
    s0=rand();
    ϵ0=0;
    a0=ee.PolicyFn([s0,ϵ0])[1];
    s1=ee.trans(a0,s0);
    a1=ee.PolicyFn([s1,0])[1];
    return(ee.dutil(a0)- 1.05*ee.β * ee.dutil(a1));
end

function simulate_ee(nM,nT,ee::EulerEquation)
    lb = minimum(ee.PolicyFn.xdata,dims=1);
    ub = maximum(ee.PolicyFn.xdata,dims=1);
    if lb[2]==ub[2]
        ub[2] = lb[2]+1e-6
    end
    x0 = zeros(nM,ee.PolicyFn.q); #Initial states
    for i = 1:ee.PolicyFn.q
        du = Uniform(lb[i],ub[i]);
        x0[:,i] = rand(du,nM);
    end
    s = zeros(nM,nT+1);
    a = zeros(nM,nT);
    s[:,1] = x0[:,1];
    for t = 1:nT
        # x = hcat(s[:,t],randn(nM)/3);
        x = hcat(s[:,t],zeros(nM));
        a[:,t] = ee.PolicyFn(x);
        s[:,t+1] = ee.trans(a[:,t],s[:,t]);
    end
    return(state=s,action=a);
end


input = readlines("./src/Day3/input.txt")

function count_trees(input, slope, down)
    pattern = copy(input)
    numtrees = 0
    pos = 1
    width = length(pattern[1])
    for row in 1:down:length(pattern)
        pos = mod(pos - 1, width) + 1
        val = pattern[row][pos]
        letters = string.(split(pattern[row],""))
        if val == '#'
            numtrees += 1
            letters[pos] = "X"
        else
            # print(val)
            letters[pos] = "O"
        end
        pattern[row] = join(letters)
        pos += slope
    end
    return numtrees, pattern
end

hill = copy(input)
num11, pat = count_trees(hill, 1, 1)
num31, pat = count_trees(hill, 3, 1)
num51, pat = count_trees(hill, 5, 1)
num71, pat = count_trees(hill, 7, 1)
num12, pat = count_trees(hill, 1, 2)

num11*num31*num51*num71*num12


module IfPartialBase

include("./../../constraints/geometric/constants.jl")
using JuMP
using .Constants

export if_pb

function if_pb(m)

    #if (flbz_front_left_bot_z[i] == flbz_front_left_bot_z[j] + hob_height_of_box[j] &&
    #    flbz_front_left_bot_z[i] - hob_height_of_box[j] >= 0 &&
    #    flbx_front_left_bot_x[j] > flbx_front_left_bot_x[i] - box_i_x_axis_cover &&
    #    flbx_front_left_bot_x[j] < flbx_front_left_bot_x[i] + box_i_x_axis_cover &&
    #    flby_front_left_bot_y[j] > flby_front_left_bot_y[i] - box_i_y_axis_cover &&
    #    flby_front_left_bot_y[j] < flby_front_left_bot_y[i] + box_i_y_axis_cover)

    # IF 1
    @variable(m, if11_pbs[i=1:length(boxes), j=1:length(boxes); i!=j], Bin)
    @variable(m, if12_pbs[i=1:length(boxes), j=1:length(boxes); i!=j], Bin)
    @variable(m, if1_pbs[i=1:length(boxes), j=1:length(boxes); i!=j] >= 0, Bin)

    #Hz1 >= x-b+e
    #H(1-z1) >= b-x-e
    @constraint(m, c1_if11_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        H * m[:if11_pbs][i, j] >=
        m[:flbz_front_left_bot_z][i] - (m[:flbz_front_left_bot_z][j] + m[:hob_height_of_box][j]) + 0.001
    )
    @constraint(m, c2_if11_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        H * (1 - m[:if11_pbs][i, j]) >=
        (m[:flbz_front_left_bot_z][j] + m[:hob_height_of_box][j]) - m[:flbz_front_left_bot_z][i] - 0.001
    )

    #Hz2 >= b-x+e
    #H(1-z2) >= x-b-e
    @constraint(m, c1_if12_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        H * m[:if12_pbs][i, j] >=
        (m[:flbz_front_left_bot_z][j] + m[:hob_height_of_box][j]) - m[:flbz_front_left_bot_z][i] + 0.001
    )
    @constraint(m, c2_if12_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        H * (1 - m[:if12_pbs][i, j]) >=
        m[:flbz_front_left_bot_z][i] - (m[:flbz_front_left_bot_z][j] + m[:hob_height_of_box][j]) - 0.001
    )

    #If if11, if12 = 1, then box i and j have z-bot/top value within interval -e <= z <= +e, hence, expression = 1
    @constraint(m, c_if1_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        m[:if11_pbs][i, j] + m[:if12_pbs][i, j] >=
        2 * m[:if1_pbs][i, j]
    )

    #Xj < Xi + Xcoveri
    @variable(m, if2_pbs[i=1:length(boxes), j=1:length(boxes); i!=j], Bin)
    @constraint(m, c1_if2_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        L * (1 - m[:if2_pbs][i, j]) >=
        m[:flbx_front_left_bot_x][j] + 0.001 - (m[:flbx_front_left_bot_x][i] + m[:box_x_axis_cover][i])
    )

    @constraint(m, c2_if2_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        L * m[:if2_pbs][i, j] >=
        (m[:flbx_front_left_bot_x][i] + m[:box_x_axis_cover][i]) - m[:flbx_front_left_bot_x][j] - 0.001
    )

    # Xj + Xcoverj > Xi
    @variable(m, if3_pbs[i=1:length(boxes), j=1:length(boxes); i!=j], Bin)
    @constraint(m, c1_if3_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        L * (1 - m[:if3_pbs][i, j]) >=
        m[:flbx_front_left_bot_x][i] + 0.001 - (m[:flbx_front_left_bot_x][j] + m[:box_x_axis_cover][j])
    )

    @constraint(m, c2_if3_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        L * m[:if3_pbs][i, j] >=
        (m[:flbx_front_left_bot_x][j] + m[:box_x_axis_cover][j]) - m[:flbx_front_left_bot_x][i] - 0.001
    )

    # Yj < Yi + Ycoveri
    @variable(m, if4_pbs[i=1:length(boxes), j=1:length(boxes); i!=j], Bin)
    @constraint(m, c1_if4_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        W * (1 - m[:if4_pbs][i, j]) >=
        m[:flby_front_left_bot_y][j] + 0.001 - (m[:flby_front_left_bot_y][i] + m[:box_y_axis_cover][i])
    )

    @constraint(m, c2_if4_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        W * m[:if4_pbs][i, j] >=
        (m[:flby_front_left_bot_y][i] + m[:box_y_axis_cover][i]) - m[:flby_front_left_bot_y][j] - 0.001
    )

    # Yj + Ycoverj > Yi
    @variable(m, if5_pbs[i=1:length(boxes), j=1:length(boxes); i!=j], Bin)
    @constraint(m, c1_if5_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        W * (1 - m[:if5_pbs][i, j]) >=
        m[:flby_front_left_bot_y][i] + 0.001 - (m[:flby_front_left_bot_y][j] + m[:box_y_axis_cover][j])
    )

    @constraint(m, c2_if5_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        W * m[:if5_pbs][i, j] >=
        (m[:flby_front_left_bot_y][j] + m[:box_y_axis_cover][j]) - m[:flby_front_left_bot_y][i] - 0.001
    )

    @variable(m, if_on_top[i=1:length(boxes), j=1:length(boxes); i!=j], Bin)
    @constraint(m, c_if_pbs[i=1:length(boxes), j=1:length(boxes); i!=j],
        m[:if1_pbs][i, j] +
        m[:if2_pbs][i, j] +
        m[:if3_pbs][i, j] +
        m[:if4_pbs][i, j] +
        m[:if5_pbs][i, j] >=
        5 * m[:if_on_top][i, j]
    )

    return m
end

end


# EP simulations with i.i.d. and correlated Gaussian patterns and noisy labels

using EPDilutedClassifier

N=128; ρ=0.25; α=6.0;
epsconv=1e-4; maxiter=10000; η=0.95;
damp=0.99;
λ0=1e4
w=generate_weights(N,ρ);
report=Reporter(w)

# iid patterns
println("Test with i.i.d. Gaussian patterns")
run_instance_gaussian_iid(w,ρ,α,η,callback=report,ρ0=ρ,λ0=λ0,δρ=0.0,η0=η,δη=0.0,
        damp=damp,epsconv=epsconv,maxiter=maxiter)

# gaussian correlated patterns
println("Test with correlated Gaussian patterns")
run_instance_gaussian_cor(w,ρ,α,η,callback=report,ρ0=ρ,λ0=λ0,δρ=0.0,η0=η,δη=0.0,
        damp=damp,epsconv=epsconv,maxiter=maxiter)


export Arc

mobius(a,b,c,d,z) = (a*z+b)/(c*z+d)
mobius(a,b,c,d,z::Number) = isinf(z) ? a/c : (a*z+b)/(c*z+d)
mobiusinv(a,b,c,d,z) = mobius(d,-b,-c,a,z)

mobiusD(a,b,c,d,z) = (a*d-b*c)/(d+c*z)^2
mobiusinvD(a,b,c,d,z) = mobiusD(d,-b,-c,a,z)
"""
    Arc(c,r,(θ₁,θ₂))

represents the arc centred at `c` with radius `r` from angle `θ₁` to `θ₂`.
"""
struct Arc{T,V<:Real,TT} <: SegmentDomain{TT}
    center::T
    radius::V
    angles::Tuple{V,V}
    Arc{T,V,TT}(c,r,a) where {T,V,TT} = new{T,V,TT}(convert(T,c),convert(V,r),Tuple{V,V}(a))
end

Arc(c::T,r::V,t::Tuple{V1,V2}) where {T<:Number,V<:Real,V1<:Real,V2<:Real} =
    Arc{promote_type(T,V,V1,V2),
        promote_type(real(T),V,V1,V2),
        Complex{promote_type(real(T),V,V1,V2)}}(c,r,t)
Arc(c::Vec{2,T},r::V,t::Tuple{V,V}) where {T<:Number,V<:Real} =
    Arc{Vec{2,promote_type(T,V)},
        promote_type(real(T),V),
        Vec{2,promote_type(real(T),V)}}(c,r,t)
Arc(c::Vec{2,T},r::V,t::Tuple{V1,V2}) where {T<:Number,V<:Real,V1<:Real,V2<:Real} =
    Arc{Vec{2,promote_type(T,V,V1,V2)},
        promote_type(real(T),V,V1,V2),
        Vec{2,promote_type(T,V,V1,V2)}}(c,r,t)

Arc(c::Tuple,r,t) = Arc(Vec(c...),r,t)
Arc(c,r,t0,t1) = Arc(c,r,(t0,t1))

complex(a::Arc{V}) where {V<:Vec} = Arc(complex(a.center...),a.radius,a.angles)

isambiguous(d::Arc) =
    isnan(d.center) && isnan(d.radius) && isnan(d.angles[1]) && isnan(d.angles[2])
convert(::Type{Arc{T,V}},::AnyDomain) where {T<:Number,V<:Real} =
    Arc{T,V}(NaN,NaN,(NaN,NaN))
convert(::Type{IT},::AnyDomain) where {IT<:Arc} =
    Arc(NaN,NaN,(NaN,NaN))

isempty(d::Arc) = false

reverseorientation(a::Arc) = Arc(a.center,a.radius,reverse(a.angles))

arclength(d::Arc) = d.radius*(d.angles[2]-d.angles[1])

function mobiuspars(z0,r,t0,t1)
    c=exp(im*t0/2)+exp(im*t1/2)
    f=exp(im*t0/2)-exp(im*t1/2)
    d=exp(im*t0/2+im*t1/2)*r
    -c,c*(z0+d),f,f*(d-z0)
end

mobiuspars(a::Arc) = mobiuspars(a.center,a.radius,a.angles...)

for OP in (:mobius,:mobiusinv,:mobiusD,:mobiusinvD)
    @eval $OP(a::Arc,z) = $OP(mobiuspars(a)...,z)
end

tocanonical(a::Arc{T},x) where {T<:Number} = real(mobius(a,x))
tocanonicalD(a::Arc{T},x) where {T<:Number} = mobiusD(a,x)
fromcanonical(a::Arc{T,V,TT},x) where {T<:Number,V<:Real,TT<:Complex} =
    mobiusinv(a,x)
fromcanonicalD(a::Arc{T},x) where {T<:Number} = mobiusinvD(a,x)

tocanonical(a::Arc{V},x::Vec) where {V<:Vec} =
    tocanonical(complex(a),complex(x...))
fromcanonical(a::Arc{V},x::Number) where {V<:Vec} =
    Vec(reim(fromcanonical(complex(a),x))...)
fromcanonicalD(a::Arc{V},x::Number) where {V<:Vec} =
    Vec(reim(fromcanonicalD(complex(a),x))...)

## information

issubset(a::Arc,b::Arc) =
    a.center==b.center && a.radius==b.radius &&
                    (b.angles[1]≤a.angles[1]≤a.angles[2]≤b.angles[2] ||
                            b.angles[1]≥a.angles[1]≥a.angles[2]≥b.angles[2])

# Algebra

*(c::Real,d::Arc) =
    Arc(c*d.center,c*d.radius,(sign(c)*d.angles[1],sign(c)*d.angles[2]))
*(d::Arc,c::Real) = *(c,d)

for op in (:+,:-)
    @eval begin
        $op(c::Number,d::Arc) = Arc($op(c,d.center),d.radius,d.angles)
        $op(d::Arc,c::Number) = Arc($op(d.center,c),d.radius,d.angles)
    end
end

# allow exp(im*Segment(0,1)) for constructing arc
function exp(d::IntervalOrSegment{<:Complex})
    @assert isapprox(real(leftendpoint(d)),0) && isapprox(real(rightendpoint(d)),0)
    Arc(0,1,(imag(leftendpoint(d)),imag(rightendpoint(d))))
end


using FEniCS
using PyPlot

mu = 1
rho = 1
lambda = 1
g = 9.81

# Create mesh and define function space
mesh = UnitSquareMesh(20, 20) # TODO: use RectangleMesh(0.0, 0.0, 10.0, 4.0, 10, 10)
V = VectorFunctionSpace(mesh, "P", 1)
c = Constant((0, 0))
bc = DirichletBC(V, c, "on_boundary && x[1]<1E-14") # What BCs are bing set???
tol = 1E-14

# Define strain
function epsilon(u)
     0.5 * (nabla_grad(u) + Transpose(nabla_grad(u)))
end

# Define stress
function sigma(u)
     lambda * nabla_div(u) * Identity(d) + 2 * mu * epsilon(u)
end

u = TrialFunction(V)
d = geometric_dimension(u) # space dimension
v = TestFunction(V)
f = Constant((0, -rho*g)) # Vector of uniform body force
T = Constant((0, 0))
a = inner(sigma(u), epsilon(v))*dx
L = dot(f, v) * dx + dot(T, v) * ds
u = FeFunction(V)
lvsolve(a, L, u, bc)

# s = sigma(u) - (1.0/3.0) * tr(sigma(u)) * Identity(d)
V = FunctionSpace(mesh, "P", 1)
u_magnitude = sqrt(dot(u, u))
u_magnitude = project(u_magnitude, V)
plothandle = FEniCS.Plot(u_magnitude)
colorbar(plothandle)


"""
Compute spectral geopotential `ϕ` from spectral temperature `Tabs`
and spectral topography `ϕ0`.
"""
function geopotential!( ϕ::Array{Complex{NF},3},      # geopotential
                        ϕ0::Array{Complex{NF},2},     # surface geopotential
                        Tabs::Array{Complex{NF},3},   # absolute Temperature
                        G::GeoSpectral{NF}) where {NF<:AbstractFloat}

    mx,nx,nlev = size(ϕ)

    @boundscheck size(ϕ) == size(Tabs) || throw(BoundsError())
    @boundscheck (mx,nx) == size(ϕ0)   || throw(BoundsError())

    @unpack xgeop1, xgeop2, lapserate_correction = G.geometry

    # BOTTOM LAYER
    # is last index k=end, integration over half a layer
    for j in 1:nx
        for i in 1:mx
            ϕ[i,j,end] = ϕ0[i,j] + xgeop1[end]*Tabs[i,j,end]
        end
    end

    # OTHER LAYERS
    # integrate two half-layers from bottom to top
    for k in nlev-1:-1:1
        for j in 1:nx
            for i in 1:mx
                ϕ[i,j,k] = ϕ[i,j,k+1] + xgeop2[k+1]*Tabs[i,j,k+1] + xgeop1[k]*Tabs[i,j,k]
            end
        end
    end

    # LAPSERATE CORRECTION IN THE FREE TROPOSPHERE (>nlev)
    # TODO only for spectral coefficients 1,: ?
    for k in 2:nlev-1
        for j in 1:nx
            ϕ[1,j,k] = ϕ[1,j,k] +
                    lapserate_correction[k-1]*(Tabs[1,j,k+1] - Tabs[1,j,k-1])
        end
    end
end


@testset "Iterative Refinement for predictor (FourBar)" begin
    @var x a y b x_hat a_hat y_hat b_hat
    gamma = [Variable(Symbol(:gamma, i)) for i = 1:8]
    delta = [Variable(Symbol(:delta, i)) for i = 1:8]
    gamma_hat = [Variable(Symbol(:gamma_hat, i)) for i = 1:8]
    delta_hat = [Variable(Symbol(:delta_hat, i)) for i = 1:8]
    #system of polynomials
    D1 = map(1:8) do i
        (a_hat * x - delta_hat[i] * x) * gamma[i] +
        (a * x_hat - delta[i] * x_hat) * gamma_hat[i] +
        (a_hat - x_hat) * delta[i] +
        (a - x) * delta_hat[i] - delta[i] * delta_hat[i]
    end
    D2 = map(1:8) do i
        (b_hat * y - delta_hat[i] * y) * gamma[i] +
        (b * y_hat - delta[i] * y_hat) * gamma_hat[i] +
        (b_hat - y_hat) * delta[i] +
        (b - y) * delta_hat[i] - delta[i] * delta_hat[i]
    end
    D3 = [gamma[i] * gamma_hat[i] + gamma[i] + gamma_hat[i] for i = 1:8]
    F = System(
        [D1; D2; D3],
        [x; a; y; b; x_hat; a_hat; y_hat; b_hat; gamma; gamma_hat],
        [delta; delta_hat],
    )

    s = Complex{Float64}[
        -2.6370887+0.35329306im,
        -3.7223306+2.5267087im,
        -0.70272787-0.74162847im,
        -0.52112188-1.5249im,
        -0.29444549+0.2698171im,
        0.23877446+1.1593521im,
        -0.2226372+0.24523739im,
        -0.23666974+0.87507397im,
        2.5277031-6.3353057im,
        -0.88010494+1.3133313im,
        -0.73606124+0.15544107im,
        0.80744564-3.0613903im,
        -0.82122921+0.80545901im,
        0.68534751-2.5000172im,
        0.65872999-2.3719828im,
        -0.87767826-0.35435132im,
        -0.93290889+0.12048708im,
        -0.93106365-0.75512927im,
        1.8130785-1.6567022im,
        -0.85699423+0.24221833im,
        -0.73738109-1.1832401im,
        -0.81460307+0.2750148im,
        -0.80200623+0.28313097im,
        -0.12955283+2.5215805im,
    ]
    p = Complex{Float64}[
        7.297148065259444-3.7969299471707454im,
        0.06125202773860465+0.9251492821389162im,
        1.4586512320807385+0.641527879044937im,
        3.3162292880980537-2.979942348702623im,
        -0.6613217458488018+0.8321586519581641im,
        2.6212360706406703-2.6766427578876666im,
        2.4522388848243555-2.5938408784246976im,
        -1.7265529790073868-0.29337325907998707im,
        0.24011427062306998+1.375412535460542im,
        -0.19336068638852033+0.5669754468190339im,
        0.147943349839693-0.2784406507316879im,
        -0.01641513182688631+1.5963735195205522im,
        -0.33880136827427765+0.2772103200265199im,
        -0.19788506286822416+1.6632361622942196im,
        -0.26097545384785903+1.6764055030939693im,
        0.44012684131549085+1.0212717350758722im,
    ]
    q = Complex{Float64}[
        -0.35263257737537096+1.2598825875492277im,
        -0.5353479512983561-0.5963278468670689im,
        1.4096839876427854-1.2227632666172732im,
        -0.2550909822371234-0.641270846350799im,
        0.24180730168363096-0.46611650665985527im,
        0.29953332004980354-0.8469219548644632im,
        0.35386303195558433-0.3683866291866925im,
        1.2021910424897007+1.1148794002014533im,
        -0.35263257737537096-1.2598825875492277im,
        -0.5353479512983561+0.5963278468670689im,
        1.4096839876427854+1.2227632666172732im,
        -0.2550909822371234+0.641270846350799im,
        0.24180730168363096+0.46611650665985527im,
        0.29953332004980354+0.8469219548644632im,
        0.35386303195558433+0.3683866291866925im,
        1.2021910424897007-1.1148794002014533im,
    ]

    @testset "Fourbar - AD: $AD" for AD = 0:3
        tracker = Tracker(
            ParameterHomotopy(F, p, q),
            options = TrackerOptions(automatic_differentiation = AD),
        )
        res = track(tracker, s, 1, 0)
        @test is_success(res)
    end
end


using LightGraphs
import LightGraphs: induced_subgraph
using Base.Test

g = smallgraph(:bull)
n = 3
h = g[1:n]
@test nv(h) == n
@test ne(h) == 3

h = g[[1,2,4]]
@test nv(h) == n
@test ne(h) == 2

h = g[[1,5]]
@test nv(h) == 2
@test ne(h) == 0
@test typeof(h) == typeof(g)

g = DiGraph(100,200)
h = g[5:26]
@test nv(h) == 22
@test typeof(h) == typeof(g)

@test_throws ErrorException g[[1,1]]


using .StaticArrays

# import TensorCast: static_slice, static_glue # because Revise doesn't track these

"""
    static_slice(A, sizes)

Slice array `A` into an `AbstractArray{StaticArray}`,
with the slices always belonging to the first few indices, i.e. `code = (:,:,...,*,*)`.
Expects `sizes = Size(size(A)[1:ncolons])` for stability.

Typically produces `reshape(reinterpret(SArray...`; copy this to get `Array{SArray}`,
or call `static_slice(A, sizes, false)` to omit the reshape.
"""
@inline function static_slice(A::AbstractArray{T,N}, sizes::Size{tup}, finalshape::Bool=true) where {T,N,tup}
    IN = length(tup)
    IT = SArray{Tuple{tup...}, T, IN, prod(tup)}
    if N-IN>1 && finalshape
        finalsize = size(A)[1+IN:end]
        reshape(reinterpret(IT, vec(A)), finalsize)
    else
        reinterpret(IT, vec(A)) # always a vector
    end
end

"""
    static_glue(A)

Glues the output of `static_slice` back into one array, again with `code = (:,:,...,*,*)`.
"""
@inline function static_glue(A::AbstractArray{IT,ON}, finalshape::Bool=true) where {IT,ON}
    IT <: StaticArray || error("static_glue needs an array of StaticArrays")
    if finalshape
        finalsize = (size(IT)..., size(A)...)
        reshape(reinterpret(eltype(IT), A), finalsize)
    else
        reinterpret(eltype(eltype(A)), A)
    end
end

function auto_glue(A::AbstractArray{IT,ON}, code::Tuple) where {IT<:StaticArray,ON}
    if iscodesorted(code)
        static_glue(A)
    else
        copy_glue(A, code)
    end
end


"""

# basis_set

Determine basis_set

$(SIGNATURES)

Part of API, exported.
"""
function basis_set(dag::DAG; debug=false)
  as = transpose(topological_sort(dag.a))
  nod = dag.vars[topological_order(dag.a)]
  dv = 1:length(nod)

  # Correct if adj matixi is upper triangular
  @assert istriu(as) "Sorted adj matrix not upper triangular."

  ind = Vector{Vector{Symbol}}()
  for r in dv
    for s in r:length(nod)
      ((as[r, s] !== 0) || (s == r)) && continue
      ed = nod[[r, s]]
      pa_r = filter(i -> as[i, r] == 1, dv)
      pa_s = filter(i -> as[i, s] == 1, dv)
      dsep = union(nod[pa_r], nod[pa_s])
      debug && println("r = $(nod[r]), s= $(nod[s]), pa_r = $(nod[pa_r]), pa_s = $(nod[pa_s]), dsep = $dsep")
      dsep = setdiff(dsep, ed)
      append!(ed, dsep)
      append!(ind, [ed])
    end
  end
  BasisSet(dag, ind)
end

export
  basis_set


using Xpress
Solver = Xpress

using BenchmarkTools

BenchmarkTools.DEFAULT_PARAMETERS.samples = 100

const nvars = 1000
const ncons = 1000
const ncoefs = 100

const inds = collect(1:div(nvars,ncoefs):nvars)
const coefs = collect(1.0:Float64(ncoefs))
@assert length(inds) == length(coefs)

const At = sparse(repmat(inds, ncons), repmat(collect(1:ncoefs),1,ncons)'[:], repmat(coefs, ncons), nvars, ncons)
const rhs = fill(24.1,ncons)

# env = Solver.Env()
# m1 = Solver.Model(env,"model")
m1 = Solver.Model()
Solver.add_cvars!(m1, zeros(nvars))
function add100cons1(m)
    for i in 1:ncons
        Solver.add_constr!(m, inds, coefs, '<', 24.1)
    end
end

m2 = Solver.Model()
Solver.add_cvars!(m2, zeros(nvars))
function add100cons2(m)

    add_constrs_t!(m, At, '<', rhs)
    nothing
end

# @benchmark add100cons1(m1)
# @benchmark add100cons2(m2)