# This package redefines Complex operations so that there is a single
# complex infinity and a single complex NaN.

import Base.inv, Base.Complex, Base.show, Base.showcompact
import Base.+, Base.-, Base.*, Base./, Base.==

function Complex(x::Real, y::Real)
    if isnan(x) || isnan(y)
        return ComplexNaN
    end

    if isinf(x) || isinf(y)
        return ComplexInf
    end

    if x==0
        x = zero(x)
    end
    if y==0
        y = zero(y)
    end

    return Complex(promote(x,y)...)
end

module RiemannComplexNumbers
"""
Complex not-a-number in the `RiemannComplexNumbers` module.
"""
const ComplexNaN = Complex(NaN,NaN)

"""
Single complex infinity in the `RiemannComplexNumbers` module.
"""
const ComplexInf = Complex(Inf,Inf)

export ComplexNaN, ComplexInf

function my_inv(z::Complex)
    if isnan(z)
        return ComplexNaN
    end
    if isinf(z)
        return zero(z)
    end
    if z == 0
        return ComplexInf
    end
    return my_inv_work(z)
end

my_inv_work(z::Complex)  = conj(z)/abs2(z)
my_inv_work{T<:Integer}(z::Complex{T}) = inv(float(z))

function my_inv_work(w::Complex128)
    c, d = reim(w)
    half = 0.5
    two = 2.0
    cd = max(abs(c), abs(d))
    ov = realmax(c)
    un = realmin(c)
    ep = eps(Float64)
    bs = two/(ep*ep)
    s = 1.0
    cd >= half*ov  && (c=half*c; d=half*d; s=s*half) # scale down c,d
    cd <= un*two/ep && (c=c*bs; d=d*bs; s=s*bs     ) # scale up c,d
    if abs(d)<=abs(c)
        r = d/c
        t = 1.0/(c+d*r)
        p = t
        q = -r * t
    else
        c, d = d, c
        r = d/c
        t = 1.0/(c+d*r)
        p = r * t
        q = -t
    end
    return Complex128(p*s,q*s) # undo scaling
end

# z-->b w-->a
function my_div(a::Complex, b::Complex)
    a,b = promote(a,b)
    if isnan(a) || isnan(b)
        return ComplexNaN
    end
    if isinf(b)
        if isinf(a)
            return ComplexNaN
        end
        return zero(typeof(z=a))
    end
    if b==0
        if a==0
            return ComplexNaN
        end
        return ComplexInf
    end
    return my_div_work(a,b)
end

# Special cases are already handled before this is called. Code taken
# from Julia's complex.jl
function my_div_work(a::Complex, b::Complex)
    are = real(a); aim = imag(a); bre = real(b); bim = imag(b)
    if abs(bre) <= abs(bim)
        r = bre / bim
        den = bim + r*bre
        Complex((are*r + aim)/den, (aim*r - are)/den)
    else
        r = bim / bre
        den = bre + r*bim
        Complex((are + aim*r)/den, (aim - are*r)/den)
    end
end

# Also taken from complex.jl
function my_div_work(z::Complex128, w::Complex128)
    a, b = reim(z); c, d = reim(w)
    half = 0.5
    two = 2.0
    ab = max(abs(a), abs(b))
    cd = max(abs(c), abs(d))
    ov = realmax(a)
    un = realmin(a)
    ep = eps(Float64)
    bs = two/(ep*ep)
    s = 1.0
    ab >= half*ov  && (a=half*a; b=half*b; s=two*s ) # scale down a,b
    cd >= half*ov  && (c=half*c; d=half*d; s=s*half) # scale down c,d
    ab <= un*two/ep && (a=a*bs; b=b*bs; s=s/bs     ) # scale up a,b
    cd <= un*two/ep && (c=c*bs; d=d*bs; s=s*bs     ) # scale up c,d
    abs(d)<=abs(c) ? ((p,q)=robust_cdiv1(a,b,c,d)  ) : ((p,q)=robust_cdiv1(b,a,d,c); q=-q)
    return Complex128(p*s,q*s) # undo scaling
end

end #end of module

show(io::IO, z::Complex)        = print(io, string(z,false))
showcompact(io::IO, z::Complex) = print(io, string(z,true))

#### BASIC FOUR OPERATIONS ####

# Addition

function +(w::Complex, z::Complex)
    w,z = promote(w,z)
    if isnan(w) || isnan(z)
        return ComplexNaN
    end
    if isinf(w)
        if isinf(z)
            return ComplexNaN
        end
        return ComplexInf
    end
    if isinf(z)
        return ComplexInf
    end
    return Complex(w.re+z.re, w.im+z.im)
end

+(w::Complex, x::Real) = w + Complex(x)
+(x::Real, w::Complex) = Complex(x) + w

# Binary minus

function -(w::Complex, z::Complex)
    w,z = promote(w,z)
    if isnan(w) || isnan(z)
        return ComplexNaN
    end
    if isinf(w)
        if isinf(z)
            return ComplexNaN
        end
        return ComplexInf
    end
    if isinf(z)
        return ComplexInf
    end
    return Complex(w.re-z.re, w.im-z.im)
end

-(w::Complex, x::Real) = w - Complex(x)
-(x::Real, w::Complex) = Complex(x) - w

# Unary minus

function -(w::Complex)
    if isnan(w)
        return ComplexNan
    end
    if isinf(w)
        return ComplexInf
    end
    return Complex(-w.re, -w.im)
end

# Multiplication

function *(w::Complex, z::Complex)
    w,z = promote(w,z)
    if isnan(w) || isnan(z)
        return ComplexNaN
    end
    if isinf(w)
        if z==0
            return ComplexNaN
        end
        return ComplexInf
    end
    if isinf(z)
        if w==0
            return ComplexNaN
        end
        return ComplexInf
    end
    return Complex(w.re*z.re - w.im*z.im , w.re*z.im + w.im*z.re)
end

*(w::Complex, x::Real) = w * Complex(x)
*(x::Real, w::Complex) = Complex(x) * w

# Division

/(w::Complex, x::Real) = w * RiemannComplexNumbers.my_inv(Complex(x))
/(x::Real, z::Complex) = Complex(x)*RiemannComplexNumbers.my_inv(z)

/(w::Complex, z::Complex) = RiemannComplexNumbers.my_div(w,z)
/(w::Complex128, z::Complex128) = RiemannComplexNumbers.my_div(w,z)

# These cover the cases in complex.jl (Julia 0.3.10)
inv(w::Complex{Float64}) = RiemannComplexNumbers.my_inv(w)
inv{T<:Integer}(w::Complex{T}) = RiemannComplexNumbers.my_inv(w)
inv{T<:AbstractFloat}(w::Complex{T}) = RiemannComplexNumbers.my_inv(w)
inv(w::Complex) = RiemannComplexNumbers.my_inv(w)

# Equality

function ==(w::Complex, z::Complex)
    if isnan(w) || isnan(z)
        return false
    end
    if isinf(w) && isinf(z)
        return true
    end
    return w.re == z.re && w.im == z.im
end

==(w::Complex, x::Real) = w == Complex(x)
==(x::Real, z::Complex) = Complex(x) == z

import Base.string

function string(z::Complex,compact::Bool=true)
    if isnan(z)
        if compact
            return "C_NaN"
        else
            return "ComplexNaN"
        end
    end

    if isinf(z)
        if compact
            return "C_Inf"
        else
            return "ComplexInf"
        end
    end

    a,b = reim(z)

    # This is to hide -0.0
    if a==0
        a = zero(a)
    end

    if b==0
        b = zero(b)
    end

    sp = " "
    if compact
        sp = ""
    end

    op = "+"
    if b<0
        op = "-"
    end

    return string(a) * sp * op * sp * string(abs(b)) * "im"
end


## Computes a direct plug-in selector of the
## bandwidth for local linear regression as
## described in the 1996 J. Amer. Statist. Assoc.
## paper by Ruppert, Sheather and Wand.

function dpill(x::Vector{Float64}, y::Vector{Float64};
               blockmax::Int = 5,
               divisor::Int = 20,
               trim::Float64 = 0.01,
               proptrun::Float64 = 0.05,
               gridsize::Int = 401,
               range_x::Vector{Float64} = Float64[],
               truncate = true)
    ## Trim the 100(trim)% of the data from each end (in the x-direction).
    sp = sortperm(x)
    x = x[sp] #don't sort in place so original x and y are not muted
    y = y[sp]
    indlow = ifloor(trim*length(x)) + 1
    indupp = length(x) - ifloor(trim*length(x))

    x = x[indlow:indupp]
    y = y[indlow:indupp]

    ## Rename common parameters
    n = length(x)
    M = gridsize
    if range_x == Float64[]
        range_x = [minimum(x), maximum(x)]
    end

    a = range_x[1]
    b = range_x[2]

    ## Bin the data

    gpoints = linspace(a, b, M)
    xcounts, ycounts = rlbin(x, y, gpoints, truncate)

    ## Choose the value of N using Mallow's C_p
    Nmax = max(min(ifloor(n/divisor), blockmax), 1)
    Nval = cpblock(x, y, Nmax, 4)

    ## Estimate sig^2, theta_22 and theta_24 using quartic fits
    ## on "Nval" blocks.

    (sigsqQ, th22Q, th24Q) = blkest(x, y, Nval, 4)

    ## Estimate theta_22 using a local cubic fit
    ## with a "rule-of-thumb" bandwidth: "gamseh"

    gamseh = (sigsqQ*(b-a)/(abs(th24Q)*n))

    if th24Q < 0.0
        gamseh = (3.0 * gamseh / (8.0 * sqrt(pi)))^(1.0/7.0)
    end
    if th24Q > 0.0
        gamseh = (15.0 * gamseh / (16.0 * sqrt(pi)))^(1.0/7.0)
    end

    mddest = locpoly(xcounts, ycounts, gamseh, drv=2, range_x=range_x, binned=true)[2]

    llow = ifloor(proptrun*M) + 1
    lupp = M - ifloor(proptrun*M)
    th22kn = sum((mddest[llow:lupp].^2) .* xcounts[llow:lupp])/n

    ## Estimate sigma^2 using a local linear fit
    ## with a "direct plug-in" bandwidth: "lamseh"
    C3K = (1.0/2.0) + 2.0*sqrt(2.0) - (4.0/3.0)*sqrt(3.0)
    C3K = (4.0*C3K/(sqrt(2.0*pi)))^(1.0/9.0)
    lamseh = C3K*(((sigsqQ^2)*(b-a)/((th22kn*n)^2))^(1.0/9.0))

    ## Now compute a local linear kernel estimate of
    ## the variance.
    mest = locpoly(xcounts, ycounts, lamseh, range_x=range_x, binned=true)[2]
    Sdg = sdiag(xcounts, lamseh, range_x=range_x, binned=true)[2]
    SSTdg = sstdiag(xcounts, lamseh, range_x=range_x, binned=true)[2]
    sigsqn = sum(y.^2) .- 2*sum(mest.*ycounts) + sum((mest.^2).*xcounts)
    sigsqd = n - 2*sum(Sdg.*xcounts) + sum(SSTdg.*xcounts)
    sigsqkn = sigsqn/sigsqd

    ## Combine to obtain final answer.
    (sigsqkn*(b-a)/(2.0*sqrt(pi)*th22kn*n))^(1.0/5.0)
end


function hiv_sim!(x, ϵ1, ϵ2, params, dt)
    T1, T2, T1i, T2i, V, E = x
    toti = T1i + T2i
    λ1, λ2, λE, d1, d2, dE, k1, k2, Kb, Kd, δ, δE, ρ1, ρ2, m1, m2, bE, NT, c, f = params

    dT1 = λ1 - (d1 * T1) - (1-ϵ1) * k1 * V * T1
    dT2 = λ2 - (d2 * T2) - (1-f*ϵ1) * k2 * V * T2
    dT1i = (1-ϵ1) * k1 * V * T1 - (δ * T1i) - m1 * E * T1i
    dT2i = (1-f*ϵ1) * k2 * V * T2 - (δ * T2i) - m2 * E * T2i
    dV = (1 - ϵ2) * NT * δ * toti - (c * V) - ((1-ϵ1) * ρ1 * k1 * T1 + (1 - f*ϵ1) * ρ2 * k2 * T2) * V
    dE = λE + bE * toti * E / (toti + Kb) - dE * toti * E / (toti + Kd) - δE * E
    x[1] += dt * dT1
    x[2] += dt * dT2
    x[3] += dt * dT1i
    x[4] += dt * dT2i
    x[5] += dt * dV
    x[6] += dt * dE
    return nothing
end

function hiv_simulate!(x, ϵ, params, dt, sim_steps)
    Δt = dt / sim_steps
    for i in 1:sim_steps
        hiv_sim!(x, ϵ[1], ϵ[2], params, Δt)
    end
    return nothing
end

function hiv_params()
    λ1 = 10000.0
    λ2 = 31.98
    λE = 1.0
    d1 = 0.01
    d2 = 0.01
    dE = 0.25
    k1 = 8.0 * 1e-7
    k2 = 1.0 * 1e-4
    Kb = 100.0
    Kd = 500.0
    δ = 0.7
    δE = 0.1
    ρ1 = 1.0
    ρ2 = 1.0
    m1 = 1.0 * 1e-5
    m2 = 1.0 * 1e-5
    bE = 0.3
    NT = 100.0
    c = 13.0
    f = 0.34
    return λ1, λ2, λE, d1, d2, dE, k1, k2, Kb, Kd, δ, δE, ρ1, ρ2, m1, m2, bE, NT, c, f
end

function hiv_dt(x, p, t)
    T1, T2, T1i, T2i, V, E = x
    toti = T1i + T2i
    ϵ1, ϵ2 = p
    dT1 = 10000.0 - (0.01 * T1) - (1.0 - ϵ1) * (8.0 * 1e-7) * V * T1
    dT2 = 31.98 - (0.01 * T2) - (1.0 - 0.34*ϵ1) * (1.0 * 1e-4) * V * T2
    dT1i = (1-ϵ1) * (8.0 * 1e-7) * V * T1 - (0.7 * T1i) - (1.0 * 1e-5) * E * T1i
    dT2i = (1-0.34*ϵ1) * (1.0 * 1e-4) * V * T2 - (0.7 * T2i) - (1.0 * 1e-5) * E * T2i
    dV = (1 - ϵ2) * 100.0 * 0.7 * (T1i + T2i) - (13.0 * V) - ((1-ϵ1) * 1.0 * (8.0 * 1e-7) * T1 + (1 - 0.34*ϵ1) * 1.0 * (1.0 * 1e-4) * T2) * V
    dE = 1.0 + 0.3 * (T1i + T2i) * E / (T1i + T2i + 100.0) - 0.25 * (T1i + T2i) * E / (T1i + T2i + 500.0) - 0.1 * E
    return @SVector [dT1, dT2, dT1i, dT2i, dV, dE]
end

function hiv_simulate!(prob, x, ϵ, dt)
    tspan = (0.0, dt)  # simulate 5 days at a time
    prob = remake(prob, u0=x, p=ϵ)
    sol = DiffEqBase.solve(prob, BS3(), reltol=1e-1, abstol=1e-1, save_everystep=false)
    return sol.u[2]
end

function hiv_step!(prob::ODEProblem, state, action, dt, actions)
    ϵ = actions[action]
    t, s, x = state
    reward = hiv_reward(s, ϵ)
    s = hiv_simulate!(prob, s, ϵ, dt)
    t += dt
    @. x = log10(s)
    γ = 0.98
    if t ≥ 1000
        γ = 0.0
    end

    return (t, s, x), x, reward, γ

end

function hiv_step!(state, action, dt, actions, params)
    ϵ = actions[action]
    t, s, x = state
    reward = hiv_reward(s, ϵ)
    sim_steps = 5*65
    hiv_simulate!(s, ϵ, params, dt, sim_steps)
    # hiv_simulate!(s, ϵ, dt)
    t += dt
    @. x = log10(s)
    γ = 0.98
    if t ≥ 1000
        γ = 0.0
    end

    return (t, s, x), x, reward, γ

end

function hiv_reward(s, ϵ)
    V, E = s[5], s[6]
    ϵ1, ϵ2 = ϵ[1], ϵ[2]
    return 0.1 * V  + 20000.0 * ϵ1^2 + 2000.0 * ϵ2^2 - 1000.0 * E
end

function hiv_initial()
    t = 0.0
    # s = @SVector [163573.0, 5.0, 11945.0, 46.0, 63919.0, 24.0]
    s = [163573.0, 5.0, 11945.0, 46.0, 63919.0, 24.0]
    x = zeros(6)
    @. x = log10(s)
    return (t, s, x), x
end

function hiv()
    return create_hiv()
end

function create_hiv()
    [(3.0, 8.0), (-1, 4.0), (-2.5, 8.0), (-2.0, 4.0), (-1.0, 8.0), (0.0, 7.0)]
    X = Array{Float64,2}([
         3.0 8
        -1.0 4
        -2.5 8
        -2.0 4
        -1.0 8
         0.0 7
    ])
    S = ([0.0, 1000.0],
        exp10.(X),
        X
    )
    A = 1:4
    # u0 = @SVector [163573.0, 5.0, 11945.0, 46.0, 63919.0, 24.0]
    dt = 5.0
    # tspan = (0.0, dt)
    # ps = (0.0, 0.0)
    # prob = ODEProblem(hiv_dt,u0,tspan,ps)
    params = hiv_params()
    actions = ((0.0, 0.0), (0.7, 0.0), (0.0, 0.3), (0.7, 0.3))
    # p = (s,a)->hiv_step!(prob, s, a, dt, actions)
    p = (s,a)->hiv_step!(s, a, dt, actions, params)
    d0 = hiv_initial

    meta = Dict{Symbol,Any}()
    # meta[:minreward] = -5.0
    # meta[:maxreward] = 5
    # meta[:minreturn] = -5 * ceil(maxT / (dt * 20))
    # meta[:maxreturn] =  5 * ceil(maxT / (dt * 20))
    meta[:stochastic] = false
    meta[:minhorizon] = 200
    meta[:maxhorizon] = 200
    meta[:discounted] = true
    meta[:episodes] = 300

	bplot = HivPlotData()
    render = (state,clearplot=false)->hivplot(bplot,state,clearplot)
    m = SequentialProblem(S,X,A,p,d0,meta,render)
end

struct HivPlotData <: Any
    ts::Vector{Float64}
    xs::Vector{Vector{Float64}}

	function HivPlotData()
		new(Vector{Float64}(),Vector{Vector{Float64}}())
	end
end

@userplot HivPlot
@recipe function f(ap::HivPlot)
	data, state, clearplot = ap.args
	t, state, x = state
	if clearplot
        empty!(data.ts)
        empty!(data.xs)
	end

    push!(data.ts, t)
	push!(data.xs, deepcopy(x))
	legend := false
	xlims := (0, 1000)
	# grid := true
	# ticks := true
	layout := (3,2)
	# foreground_color := :white
	# aspect_ratio := 1.
    xs = hcat(data.xs...)
    names = ["T1 Healthy", "T2 Healthy", "T1 Infected", "T2 Infected", "Free Virus Particles", "Immune Effectors"]
    labels = ["log10 T1", "log10 T2", "log10 T1i", "log10 T2i", "log10 V", "log10 E"]
    ylimits = [(3.0, 8.0), (-1, 4.0), (-2.5, 8.0), (-2.0, 4.0), (-1.0, 8.0), (0.0, 7.0)]
    for i in 1:6
        @series begin
            title --> string(names[i])
            ylims := ylimits[i]
            yguide := labels[i]
            seriestype := :path
            subplot := i
            data.ts, xs[i, :]
        end
    end

end


using MatrixProfile
using Test, Statistics, LinearAlgebra, Plots

using SlidingDistancesBase
using MatrixProfile: Profile

normdist = (x,y)->norm(znorm(x)-znorm(y))

function naive_matrix_profile(A,B,m)
    res = map(1:length(B)-m+1) do i
        findmin(distance_profile(ZEuclidean(), getwindow(B,m,i), A))
    end
    Profile(B,getindex.(res, 1), getindex.(res, 2), m, A)
end

function profeq(p1,p2)
    cond = p1.P ≈ p2.P
    cond && return true
    @show norm(p1.P - p2.P)
    false
end

@testset "MatrixProfile.jl" begin

   t = range(0, stop=1, step=1/10)
   y0 = sin.(2pi .* t)
   T = [randn(50); y0; randn(50); y0; randn(50)]
   A = sign.([randn(50); y0; randn(50); y0; randn(50)])[1:end÷2] .+ 0.01.*randn.()

   profile = @inferred matrix_profile(T, length(y0))
   P,I = profile.P, profile.I
   @test_nowarn plot(profile)
   # plot(T, layout=2)
   # plot!(P, sp=2)

   m = findmin(P)
   @test m[1] < 1e-6
   @test m[2] == 51 || m[2] == 112

   # Test Euclidean between two series
   profile3 = @inferred matrix_profile(T, T, length(y0))
   @test all(profile3.P .< 1e-6)

   profile5 = matrix_profile(T[1:end÷2], T, length(y0))
   @test sum(profile5.P[1:length(T)÷2-length(y0)]) < 1e-5

   profile6 = matrix_profile(T, T[1:end÷2], length(y0))
   @test sum(profile6.P) < 1e-5

   profile7n = naive_matrix_profile(T, A, length(y0))
   profile7n2 = @inferred matrix_profile(T, A, length(y0), normdist)
   profile7 = matrix_profile(T, A, length(y0))
   @test profeq(profile7, profile7n)
   @test profeq(profile7n, profile7n2)

   profile8n = naive_matrix_profile(A, T, length(y0))
   profile8n2 = @inferred matrix_profile(A, T, length(y0), normdist)
   profile8 = @inferred matrix_profile(A, T, length(y0))
   @test profeq(profile8, profile8n)
   @test profeq(profile8n, profile8n2)

   @testset "Generic matrix profile" begin
       @info "Testing Generic matrix profile"

       # Test generic one serie
       profile2 = @inferred matrix_profile(T, length(y0), normdist)
       @test profile2.P ≈  profile.P
       @test all(eachindex(profile.I)) do i
           profile2.I[i] ∈ (51,112) || profile2.I[i] == profile.I[i]
       end

       # Test generic between two series
       profile4 = @inferred matrix_profile(T, T, length(y0), normdist)
       @test all(profile4.P .< 1e-6)
   end

   @testset "Motifs" begin
       @info "Testing Motifs"
       t = range(0, stop=1, step=1/10)
       y0 = sin.(2pi .* t)
       T = [randn(50); y0; randn(50); y0; randn(50)]
       A = sign.([randn(50); y0; randn(50); y0; randn(50)])[1:end÷2] .+ 0.01.*randn.()
       profile = matrix_profile(T, length(y0))

       @time mot = motifs(profile, 2, r=2, th=5)
       @test MatrixProfile.subseqtype(mot) == "Motif"
       @test onsets(mot[1]) == [51, 112]
       @test_nowarn plot(mot)
       @test_nowarn plot(profile, mot)

       an = anomalies(profile, 3)
       @test length(an) == 3
       @test length(seqs(an)) == 3
       @test MatrixProfile.subseqtype(an) == "Anomaly"
       @test onsets(an)[1] == argmax(profile.P)
       @test_nowarn plot(an)
       @test MatrixProfile.seqlength(an) == length(y0)

   end

   @time matrix_profile(randn(Float32, 2^15), 256)

   @testset "mpdist" begin
       @info "Testing mpdist"

       t = 1:0.01:3
       A = @. sin(2pi * t) + 0.1 * randn()
       B = @. sign(sin(2pi * t)) + 0.1 * randn()
       m = 4
       # p1 = matrix_profile(A,B,m)
       # p2 = matrix_profile(B,A,m)
       # partialsort([p1.P; p2.P], 20)
       # plot(p1)
       # plot!(p2)
       @test @inferred(mpdist(A,B, m)) > 0
       @test mpdist(A,A, m) < 10sqrt(eps())
       @test mpdist(B,B, m) < 10sqrt(eps())
       T = [A; B]
       p = mpdist_profile(T, 50, 5)
       @test_nowarn plot(p)

       snips = snippets(T, 2, 50, m=5)
       @test_nowarn plot(snips)

   end

 end

# Benchmark
# @btime matrix_profile($(randn(Float32, 10_000)), 20)
#
# @time matrix_profile((randn(Float32, 100_000)),  20)
#
# @time matrix_profile((randn(Float32, 10_000)), (randn(Float32, 1_000)), 20, (x,y)->norm(x-y))
#
# @profiler matrix_profile(randn(10_000), 20)
# Q = randn(5)
# T = randn(10)
# d1 = DynamicAxisWarping.window_dot(Q,T)
# d2 = DynamicAxisWarping.window_dot2(Q,T)
# d3 = DynamicAxisWarping.window_dot3(Q,T)
# [d1 d2 d3]

# Eval precision
# a = randn(Float64, 50_000)
# p1 = matrix_profile(a,  20)
# p2 = matrix_profile(Float32.(a),  20)
# @show mean(abs.(p1.P - p2.P)./p1.P)
# plot(abs.(p1.P - p2.P)./p1.P)

# path = "/tmp/MixedBag/01911m_02019m_III_7680_200.txt"
# T = parse.(Int, split(join(Char.(read(path))), ','))
# profile, snips, Cfracs = snippets(T, 3, 20, 10)
# plot(plot(snips, layout = (1, 3), size = (800, 200)), plot(profile, snips, legend = false), layout=(2,1))


#using Compat.Serialization
#using Compat: @info

# Sets up a variety of different Gauss-Legendre data lengths
# for resolving different regimes of oscillatory function
#const GLH_NODES_20, GLH_WEIGHTS_20 = gausslegendre(20)
const GLH_NODES_100, GLH_WEIGHTS_100 = gausslegendre(100)
const GLH_NODES_200, GLH_WEIGHTS_200 = gausslegendre(200)
const GLH_NODES_500, GLH_WEIGHTS_500 = gausslegendre(500)
const GLH_NODES_1000, GLH_WEIGHTS_1000 = gausslegendre(1000)
const GLH_NODES_2000, GLH_WEIGHTS_2000 = gausslegendre(2000)
const GLH_NODES_2500, GLH_WEIGHTS_2500 = gausslegendre(2500)
const GLH_NODES_3500, GLH_WEIGHTS_3500 = gausslegendre(3500)
const GLH_NODES_10000, GLH_WEIGHTS_10000 = gausslegendre(10000)
const GLH_N = [100,200,500,1000,2000,2500,3500,10000]

const LGFH_DIR  = joinpath(pwd(), "cache")

using ProgressMeter

alpha_to_string(α::Float64) = string(100000+α*10000)[2:6]

lgfh_file(α) = joinpath(LGFH_DIR,"lgfhtable_alpha"*alpha_to_string(α)*".dat")

function _get_gl_data(n;i=0)
  # Set the number of Gauss points so that it will resolve the Nyquist frequency
  # on the oscillatory part of the Fourier integral
  while 2*n > GLH_N[i += 1] end
  return getfield(Fields,Symbol("GLH_NODES_",string(GLH_N[i]))), getfield(Fields,Symbol("GLH_WEIGHTS_",string(GLH_N[i])))
end

function load_lgf_helmholtz(N,α)
    if isfile(lgfh_file(α))
        G = deserialize(open(lgfh_file(α), "r"))
        if size(G,1) ≥ N
            return G
        end
    end
    build_lgf_helmholtz(N,α)
end

function build_lgf_helmholtz(N,α)
    @info "Building and caching Helmholtz LGF table for α = "*string(α)

    asymptotic_radius = _get_lgf_switch(α)
    @info "Switch to asymptotic formula at i = "*string(asymptotic_radius)

    lgf_support = _get_lgf_support(α)
    @info "Support radius = "*string(lgf_support)

    g = zeros(ComplexF64,N, N)
    @showprogress for y in 0:N-1, x in 0:y
      ir = _index_radius(x,y)
      if ir < asymptotic_radius
        g[x+1,y+1] = lgf_helmholtz(x,y,α)
      elseif ir < lgf_support
        g[x+1,y+1] = lgf_helmholtz_asymptotic(x,y,α)
      end
    end

    G = Symmetric(g)
    mkpath(LGFH_DIR)
    serialize(open(lgfh_file(α),"w"),G)
    G
end

#quadgauss_helmholtz(f::Function) = dot(GLH_WEIGHTS, f(GLH_NODES))

function lgf_helmholtz(i :: Integer, j :: Integer, α::Real)

  nodes_i, weights_i = _get_gl_data(abs(i))
  nodes_j, weights_j = _get_gl_data(abs(j))

  quadgauss_j(f::Function) = dot(weights_j, f(nodes_j))

  if i ≥ j
      v = ComplexF64(0.0)
      for (idex,y) in enumerate(nodes_i)
          xi1 = 0.5*π*(y+1)
          expixi1 = cos(i*xi1)
          cosxi1 = cos(xi1)
          v += weights_i[idex]*quadgauss_j() do x
              xi2 = 0.5*π*(x .+ 1)
              expixi2 = cos.(j*xi2)
              return expixi2.*expixi1./(im*α .+ 4 .- 2*cos.(xi2) .- 2*cosxi1)
          end
      end
      return v/4
  else
      return lgf_helmholtz(j,i,α)
  end

end

lgf_helmholtz_asymptotic(i :: Integer, j :: Integer, α::Real) =
      conj(im*0.25*hankelh1(0,exp(im*π/4)*sqrt(α)*sqrt(i^2+j^2)))

function _get_lgf_support(α::Number;tol=eps(Float64))
  i = 0
  while abs(lgf_helmholtz(i,0,α)) > tol
     i += 1
  end
  return i
end

function _get_lgf_switch(α::Number;tol=1e-9)
  i = 1
  while abs(lgf_helmholtz(i,0,α)-lgf_helmholtz_asymptotic(i,0,α)) > tol
     i += 1
  end
  return i
end

_index_radius(i::Int,j::Int) = ceil(Int,sqrt(i^2+j^2))


export normalize!

"""
    normalize!(S)

Locally normalize the weights of a SPN using Algorithm 1 from Peharz et al.
"""
function normalize!(spn::SumProductNetwork; ϵ = 1e-10)
    αp = ones(length(spn))

    for (nid, node) in enumerate(values(spn))

        if isa(node, Leaf)
            continue
        end

        α = 0.0

        if isa(node, SumNode)
            α = Float64(sum(weights(node)))
            if α < ϵ
                α = ϵ
            end
            T = eltype(node)
            logweights(node)[:] .-= T(log(α))
            logweights(node)[weights(node) .< ϵ] .= T(ϵ)
        elseif isa(node, ProductNode)
            α = αp[nid]
            αp[nid] = 1.
        end

        for fnode in parents(node)
            if isa(fnode, SumNode)
                id = findfirst(children(fnode) .== node)
                @assert id > 0
                T = eltype(fnode)
                logweights(fnode)[id] = logweights(fnode)[id] + T(log(α))
            elseif isa(fnode, ProductNode)
                id = findfirst(nodes .== fnode)
                if id == 0
                    @error("Parent of the following node not found! ", nid)
                end
                @assert id > 0
                αp[id] = α * αp[id]
            end
        end
    end
end


struct BinaryFuseFilter{T <: Union{UInt8, UInt16, UInt32}}
    seed::UInt64
    segment_length::UInt32
    segment_length_mask::UInt32
    segment_count::UInt32
    segment_count_length::UInt32
	array_length::UInt32
    fingerprints::Vector{T}
    n_keys::UInt32
end

function calculate_segment_length(arity::UInt32, n_keys::UInt32)
	# These parameters are very sensitive. Replacing 'floor' by 'round' can
    # substantially affect the construction time.
	if arity == 3
		return UInt32(1) << Int(floor(log(Float64(n_keys)) / log(3.33) + 2.25))
	elseif arity == UInt32(4)
		return UInt32(1) << Int(floor(log(Float64(n_keys)) / log(2.91) - 0.5))
	else
		return UInt32(65536)
    end
end

function calculate_size_factor(arity::UInt32, n_keys::UInt32)
	if arity == 3
		return Float64(max(1.125, 0.875 + 0.25 * log(1000000)/log(Float64(n_keys))))
	elseif arity == 4
		return Float64(max(1.075, 0.77 + 0.305 * log(600000)/log(Float64(n_keys))))
	else
		return Float64(2.0)
	end
end

function get_hash_from_hash(
	hash::UInt64,
	segment_count_length::UInt32,
	segment_length::UInt32,
	segment_length_mask::UInt32)
	hi = binary_fuse_mulhi(hash, UInt64(segment_count_length))
	h0 = UInt32(hi & 0xFFFFFFFF)
	h1 = h0 + segment_length
	h2 = h1 + segment_length
	h1 ⊻= UInt32((hash >> 18) & segment_length_mask)
	h2 ⊻= UInt32(hash & segment_length_mask)
	return h0, h1, h2
end

"""
`BinaryFuseFilter{T}(
	keys::Vector{UInt64};
	seed = UInt64(0x726b2b9d438b9d4d),
	max_iterations = 10) where T <: Union{UInt8, UInt16, UInt32}`

Build a binary fuse filter with T type fingerprints from
keys in `keys`.

## Arguments

`seed` is used as initial seed value,
which is iteratively changed during structure build step.

`max_iterations` restricts how many rounds of build step to try
before termination.

# Examples
```julia-repl
julia> filter = BinaryFuseFilter{UInt8}(UInt64.[1:10])
BinaryFuseFilter with 10 keys and seed 1198702242888554850.

julia> UInt64(1) in filter
true

julia> UInt64(11) in filter
false
```

"""
function BinaryFuseFilter{T}(
	keys::Vector{UInt64};
	seed = UInt64(0x726b2b9d438b9d4d),
	max_iterations = 10) where T <: Union{UInt8, UInt16, UInt32}

	n_keys = UInt32(length(keys))
	if n_keys == 0
		throw("No keys for construction.")
	end

	arity = UInt32(3)
	segment_length = calculate_segment_length(arity, n_keys)
	if (segment_length > 262144)
    	segment_length = UInt32(262144)
    end

	segment_length_mask = UInt32(segment_length - 1)
	size_factor = calculate_size_factor(arity, n_keys)
	capacity = UInt32(round(Float64(n_keys) * size_factor))
	segment_count = UInt32(ceil((capacity + segment_length - 1) / segment_length - (arity - 1)))
	array_length = (segment_count + arity - 1) * segment_length
	segment_count = UInt32(ceil((array_length + segment_length - 1)/segment_length))
	if (segment_count <= (arity - 1))
		segment_count = 1
	else
		segment_count = segment_count - (arity - 1)
    end
	array_length = (segment_count + arity - 1) * segment_length

	segment_count_length = UInt32(segment_count * segment_length)
	fingerprints = zeros(T, array_length)

	# Adding elements
	rngcounter = seed
	seed, rngcounter = splitmix64(rngcounter)
	capacity = UInt32(array_length)

	alone = zeros(UInt32, capacity)
	# the lowest 2 bits are the h index (0, 1, or 2)
	# so we only have 6 bits for counting;
	# but that's sufficient
	t2count = zeros(UInt8, capacity)
	reverseH = zeros(UInt8, n_keys)
	t2hash = zeros(UInt64, capacity)
	reverseOrder = zeros(UInt64, n_keys + 1)
	reverseOrder[n_keys + 1] = 1 # uneccessary item serving as a blocker

	# the array h0, h1, h2, h0, h1, h2
	h012 = zeros(UInt32, 5)
	iterations = 0

	blockBits = 1
	while UInt32(1 << blockBits) < segment_count
		blockBits += 1
	end
	block = 1 << blockBits

	while true
		iterations += 1
		if iterations > max_iterations
			throw("Too many iterations, you probably have duplicate keys.")
		end

		startPos = zeros(UInt32, block)
		for i in 1:length(startPos)
			# important: we do not want i * n_keys to overflow!!!
			# in java we have (int) ((long) i * size / block)
			startPos[i] = UInt32(((UInt64(i - 1) * UInt64(n_keys)) >> blockBits))
		end

		# counting sort to the h0 region
		maskblock = block - 1
		for key in keys
			hash = mixsplit(key, seed)
			# this is java >>> shifted values are zeros
			# java (int) (hash >>> (64 - blockBits))
			segment_index = hash >> (64 - blockBits)
			while reverseOrder[startPos[segment_index + 1] + 1] != 0
				segment_index += 1
				segment_index &= maskblock
			end
			reverseOrder[startPos[segment_index + 1] + 1] = hash
			startPos[segment_index + 1] += 1
		end

		# We can then scan through the partially sorted keys and update a temporary array of counters—
        # such an array tells us how many keys map to a given location. Because the keys are sorted
        # by region, a forward pass through the keys tends to access the counters in a forward manner,
        # thus reducing the number of cache misses compared to a random approach.
		error = 0
		for i in UInt32.(1:n_keys)
			hash = reverseOrder[i]
			index1, index2, index3 = get_hash_from_hash(
				hash, segment_count_length, segment_length, segment_length_mask)
			t2count[index1 + 1] += 4
			t2hash[index1 + 1] ⊻= hash
			t2count[index2 + 1] += 4
			t2count[index2 + 1] ⊻= 1
			t2hash[index2 + 1] ⊻= hash
			t2count[index3 + 1] += 4
			t2count[index3 + 1] ⊻= 2
			t2hash[index3 + 1] ⊻= hash
			if t2count[index1 + 1] < 4 # no clue what is being checked here...
				error = 1
			end
			if t2count[index2 + 1] < 4
				error = 1
			end
			if t2count[index3 + 1] < 4
				error = 1
			end
		end
		if error == 1
			continue
		end

		Qsize = 0
		# We scan the array of counters to identify the locations corresponding to a single set entry.
        # The entries are added to a stack. Since we scan forward, the later entries in the stack tend to
        # correspond to later locations.
		for i in UInt32.(1:capacity)
			alone[Qsize + 1] = i - 1
			if (t2count[i] >> 2) == 1
				Qsize += 1
			end
		end
		stacksize = UInt32(0)
		while Qsize > 0
			Qsize -= 1
			index = alone[Qsize + 1]
			if (t2count[index + 1] >> 2) == 1
				hash = t2hash[index + 1]
				found = t2count[index + 1] & 3
				reverseH[stacksize + 1] = found
				reverseOrder[stacksize + 1] = hash
				stacksize += 1

				index1, index2, index3 = get_hash_from_hash(
					hash, segment_count_length, segment_length, segment_length_mask)

				h012[1] = index2
				h012[2] = index3
				h012[3] = index1
				h012[4] = h012[1]

				other_index1 = h012[found + 1]
				alone[Qsize + 1] = other_index1
				if (t2count[other_index1 + 1] >> 2) == 2
					Qsize += 1
				end
				t2count[other_index1 + 1] -= 4
				t2count[other_index1 + 1] ⊻= mod3(found + 1)
				t2hash[other_index1 + 1] ⊻= hash

				other_index2 = h012[found + 2]
				alone[Qsize + 1] = other_index2
				if (t2count[other_index2 + 1] >> 2) == 2
					Qsize += 1
				end
				t2count[other_index2 + 1] -= 4
				t2count[other_index2 + 1] ⊻= mod3(found + 2)
				t2hash[other_index2 + 1] ⊻= hash
			end
		end

		if stacksize == n_keys
			break
		end
		for i in 1:n_keys
			reverseOrder[i] = 0
		end
		for i in 1:capacity
			t2count[i] = 0
			t2hash[i] = 0
		end
		rngcounter, seed = splitmix64(rngcounter)
	end

	# Finally, starting from the latter stack, we can construct the binary fuse filter, by making sure
    # that the bitwise exclusive or of the three locations map to the fingerprint of the matched
    # entries. By construction, we tend to go from locations that are at the beginning of the array,
    # working toward the end of the array.
	for i in n_keys:-1:1
		# the hash of the key we insert next
		hash = reverseOrder[i]
		xor2 = fingerprint(T, hash)
		index1, index2, index3 = get_hash_from_hash(
			hash, segment_count_length, segment_length, segment_length_mask)
		found = reverseH[i]
		h012[1] = index1
		h012[2] = index2
		h012[3] = index3
		h012[4] = h012[1]
		h012[5] = h012[2]
		fingerprints[h012[found + 1] + 1] = xor2 ⊻ fingerprints[h012[found + 2] + 1] ⊻ fingerprints[h012[found + 3] + 1]
	end

	return BinaryFuseFilter{T}(
		seed,
		segment_length, segment_length_mask,
    	segment_count, segment_count_length,
		array_length,
    	fingerprints, n_keys)
end

function Base.summary(io::IO, filter::BinaryFuseFilter)
    print(io, "BinaryFuseFilter with $(filter.n_keys) keys and seed $(filter.seed).")
end
Base.show(io::IO, filter::BinaryFuseFilter) = summary(io, filter)

function Base.in(key, filter::BinaryFuseFilter)
    hash = mixsplit(key, filter.seed)
    f = fingerprint(typeof(filter.fingerprints[1]), hash)
	h0, h1, h2 = get_hash_from_hash(
		hash, filter.segment_count_length, filter.segment_length, filter.segment_length_mask)
	f ⊻= (filter.fingerprints[h0 + 1] ⊻ filter.fingerprints[h1 + 1] ⊻ filter.fingerprints[h2 + 1])
	return f == 0
end


export Heun

"""
    Heun()

The [Heun](https://en.wikipedia.org/wiki/List_of_Runge–Kutta_methods#Heun's_method)
predictor of order 2.
"""
struct Heun <: AbstractPredictor end
struct HeunCache{T} <: AbstractPredictorCache
    dt::Vector{T}
    mk₁::Vector{T}
    mk₂::Vector{T}
end

function cache(::Heun, H, x, ẋ, t)
    HeunCache(promote(dt(H, x, t), copy(ẋ), copy(ẋ))...)
end

function predict!(
    xnext,
    cache::HeunCache,
    H::HomotopyWithCache,
    x,
    t,
    Δt,
    ẋ,
    Jac::JacobianMonitor,
)
    dt, mk₁, mk₂ = cache.dt, cache.mk₁, cache.mk₂
    n = length(xnext)
    @inbounds for i = 1:n
        mk₁[i] = -ẋ[i]
        xnext[i] = x[i] + Δt * ẋ[i]
    end

    minus_ẋ!(mk₂, H, xnext, t + Δt, Jac, dt)

    @inbounds for i = 1:n
        xnext[i] = x[i] - 0.5 * Δt * (mk₁[i] + mk₂[i])
    end

    nothing
end

order(::HeunCache) = 3


export WSum

struct WSum{T}
    c::T
end
@functor WSum
(l::WSum)(x::AbstractVector) = sum(x.*l.c)
(l::WSum)(x::AbstractArray) = sum_autobatch(x.*l.c)

WSum(in::Integer) = WSum(Complex{STD_REAL_PREC}, in)
function WSum(T::Type, in::Integer;
              initb = glorot_uniform)
  return WSum(initb(T, in))
end

mutable struct WSumCache{Ta,Tb,Tc}
    σᵢₙ::Ta
    out::Tc
    δℒ::Tb
    valid::Bool
end

cache(l::WSum, arr_T, in_T, in_sz)  =
    WSumCache(similar(l.c, Complex{real(eltype(l.c))}),
              zero(Complex{real(eltype(l.c))}),
              similar(l.c, Complex{real(eltype(l.c))}, 1, length(l.c)),
              false)

function layer_out_type_size(l::WSum, in_T ,in_sz)
    out_T     = promote_type(in_T, eltype(l.c))
    return out_T, (1,)
end

function (l::WSum)(c::WSumCache, x)
    σ = copyto!(c.σᵢₙ, x)

    # TODO the dot product allocates
    c.out = sum(x.*l.c)
    return c.out
end

function backprop(∇, l::WSum, c::WSumCache, δℒ::Number)
    # compute the derivative
    ∇.c .= c.σᵢₙ

    # Backpropagate
    c.δℒ .= δℒ .* transpose(l.c)
    return c.δℒ
end


@testset "SOAP Descriptor" begin

@info("Testing SOAP Descriptor for DC Si with cutoff=6.5, n=4, l=4")
using JuLIP
using QUIP, Test

at = bulk(:Si, cubic=true)
desc = soap(at, 6.5, n_max=4, l_max=4)
# Reference is from the output of the QUIP command as follows
# quip atoms_filename=test.xyz descriptor_str="soap cutoff=6.5 l_max=4 n_max=4 atom_sigma=0.5 normalise=T n_Z=1 Z={14} n_species=1 species_Z={14}"
soap_ref = [ 0.23864044377742688E+000,  0.64340429976653843E-038,  0.92117979653018819E-038,
             0.48626301215687845E-005,  0.18876337954462125E-005,  0.29409765463423732E+000,
            -0.11518886345720650E-036,  0.11050218320476954E-035,  0.11763958794274931E-002,
             0.45589948982910106E-003,  0.18122123201805027E+000,  0.11109715229581757E-033,
             0.20740200612211439E-033,  0.14230028097299613E+000,  0.55054202072415959E-001,
             0.51605689877931338E+000, -0.13873220359922910E-035,  0.53089215594650063E-036,
            -0.50839942118860665E-003, -0.50804299476442307E-003,  0.44970747195745586E+000,
             0.56664079661428650E-034, -0.62628772367525336E-034, -0.86970568361713990E-001,
            -0.86763453783360533E-001,  0.55798321223812897E+000,  0.17093065575762176E-033,
             0.32536812908176952E-031,  0.26577177885526940E-001,  0.68368050294466742E-001,
             0.37916397221481701E-002,  0.29040731747973391E-037, -0.11050630810221818E-037,
             0.11135545482166505E-004,  0.46298939173893863E-005,  0.33041486666568242E-002,
            -0.88957883103660356E-036, -0.32224333298567884E-036,  0.19049288398824812E-002,
             0.79069210886284307E-003,  0.57978317308968498E-002, -0.67817971363747986E-035,
             0.24028731016100991E-034, -0.82324728991149839E-003, -0.88112752017744061E-003,
             0.30121742054713143E-004,  0.23815358022366506E-036,  0.18522830192896921E-037,
             0.12750339845444976E-004,  0.56779863069817660E-005]
soap_now = vcat(desc[1,:]...)
println("QUIP.jl SOAP:",soap_now)
println("Reference:",soap_ref)
println(@test soap_now  ≈  soap_ref)

end


using DynamicalSystemsBase
using LinearAlgebra:norm
using Test

println("\nTesting continuous system evolution...")

@testset "Lorenz63 oop" begin

    lo11 = Systems.lorenz() #with Jac
    ts1 = trajectory(lo11, 1.0, Δt = 0.01)
    @test size(ts1) == (101, 3)
    @test ts1[1, :] == SVector{3}(lo11.u0)
    @test !any(x -> abs(x) > 1e3, ts1[end])

    ts1 = trajectory(lo11, 1.0; Δt=0.1, diffeq = (abstol=1e-8, reltol=1e-8))
    @test size(ts1) == (11, 3)
    @test ts1[1, :] == SVector{3}(lo11.u0)

    data2 = trajectory(lo11, 100; save_idxs = 1:2)
    @test size(data2)[2] == 2
end

@testset "Lorenz96 iin" begin
    u = ones(5)
    lo11 = Systems.lorenz96(5, u)
    ts1 = trajectory(lo11, 2.0, Δt = 0.01)
    @test size(ts1) == (201, 5)
    @test ts1[1, :] == SVector{5}(u)

    ts1 = trajectory(lo11, 2.0; Δt=0.1, diffeq = (abstol=1e-8, reltol=1e-8))
    @test size(ts1) == (21, 5)
    @test ts1[1, :] == SVector{5}(u)
    @test !any(x -> abs(x) > 1e3, ts1[end])
end

println("\nTesting stroboscopic map...")

@testset "Duffing stroboscopic map" begin
    F = 0.27; ω = 0.1;  # smooth boundary
    ds = Systems.duffing(zeros(2), ω = ω, f = F, d = 0.15, β = -1)
    smap = stroboscopicmap(ds, 2*pi/ω)
    reinit!(smap,[1., 1.])
    for j in 1:100
      step!(smap)
    end
    u = get_state(smap)
    @test abs(sum(u - [1.11, 0])) < 0.01
end


struct HMC{TV, TGV, TF<:AbstractFloat, TM} <: SecondOrderMetropolisSampler
    V::TV
    ∇V!::TGV
    β::TF
    M::TM
    Δt::TF
    nΔt::Int
    sqrtM::TM
    sqrtβ::TF
end

"""
    HMC(V, ∇V!, β, M, Δt, nΔt)

Set up the HMC sampler for Boltzmann.

### Fields

* V     - Potential
* ∇V!   - In place gradient of the potential
* β     - Inverse temperature
* Δt    - Time step
* nΔt   - Number of time steps to use in each Verlet run
"""
function HMC(V::TV, ∇V!::TGV, β::TF, M::TM, Δt::TF, nΔt::Int) where {TV, TGV, TF<:AbstractFloat,TM}
    sqrtM = sqrt.(M);
    sqrtβ = sqrt(β);
    return HMC(V, ∇V!, β, M, Δt, nΔt, sqrtM,sqrtβ);
end

mutable struct HMCState{Tq, TF<:AbstractFloat} <:SecondOrderMetropolisSamplerState
    x::Tq
    x_proposal::Tq
    V::TF
    V_proposal::TF
    E::TF
    E_proposal::TF
    ∇V::Tq
    p::Tq
    p_mid::Tq
    invMp::Tq
    accept::Int
end

function InitState!(x₀, sampler::HMC)
    V = sampler.V(x₀);
    return HMCState(x₀, copy(x₀), V, V, V, V, similar(x₀), similar(x₀), similar(x₀),similar(x₀), Int(0));
end

function InitState(x₀, sampler::HMC)
    V = sampler.V(x₀);
    return HMCState(copy(x₀), copy(x₀), V, V, V, V, similar(x₀), similar(x₀), similar(x₀),similar(x₀), Int(0));
end

#
# function Verlet!(q, p, ∇V!, M, Δt, nΔt, p_mid, ∇V)
function Verlet!(state::HMCState, sampler::HMC)

    sampler.∇V!(state.∇V, state.x_proposal);
    for _ in 1:sampler.nΔt
        @. state.p_mid = state.p - 0.5 * sampler.Δt * state.∇V;
        @. state.x_proposal = state.x_proposal + sampler.Δt * state.p_mid/sampler.M;
        sampler.∇V!(state.∇V, state.x_proposal);
        @. state.p = state.p_mid - 0.5 * sampler.Δt * state.∇V;
    end
    state
end

function UpdateState!(state::HMCState, sampler::HMC)

    # prepare initial state
    @. state.x_proposal = state.x;
    @. state.p = (sampler.sqrtM/sampler.sqrtβ)  * randn();
    @. state.invMp = state.p / sampler.M;
    state.E = state.V + 0.5 * (state.p ⋅ state.invMp);
    # run Verlet integrator
    Verlet!(state, sampler);
    # compute energy
    state.V_proposal = sampler.V(state.x_proposal);
    @. state.invMp = state.p / sampler.M;
    state.E_proposal = state.V_proposal + 0.5 * (state.p ⋅ state.invMp);
    # accept/reject
    a = min(1, exp( sampler.β *(state.E - state.E_proposal)));
    if rand()<a
        @. state.x = state.x_proposal;
        state.V = state.V_proposal;
        state.accept = 1;
    else
        state.accept = 0;
    end
    state
end


@testset "dotproduct" begin
    A = rand(10,5)
    B = rand(20,5)
    d = KernelFunctions.DotProduct()
    @test diag(pairwise(d,A,dims=2)) == [dot(A[:,i],A[:,i]) for i in 1:size(A,2)]
    @test_throws DimensionMismatch d(rand(3),rand(4))
    @test d(3.0,2.0) == 6.0
end


export TraceTestPencil

"""
    TraceTestPencil(F, p₀, v) <: AbstractSystem

The system `H(x,t) := [F(x, p₀+x[end]v); l ⋅[x[1:end-1];1] * λ + τ*γ)`.
"""
struct TraceTestPencil{S<:AbstractSystem,T} <: AbstractSystem
    F::TraceTestSystem{S,T}
    l::Vector{T}
    γ::T
end

function TraceTestPencil(F::TraceTestSystem, l)
    TraceTestPencil(F, l, cis(2π * rand()))
end

struct TraceTestPencilCache{SC<:TraceTestSystemCache} <: AbstractSystemCache
    cache::SC
end

function cache(F::TraceTestPencil, x, τ)
    c = cache(F.F, x, F.l)
    TraceTestPencilCache(c)
end

Base.size(F::TraceTestPencil) = size(F.F)

function evaluate!(u, F::TraceTestPencil, x, τ, c::TraceTestPencilCache)
    λ = x[end]
    evaluate!(u, F.F, x, F.l, c.cache)
    u[end] = u[end] * λ + τ[1] * F.γ
    u
end
function evaluate(F::TraceTestPencil, x, τ, c::TraceTestPencilCache)
    evaluate!(evaluate(F.F, x, F.l, c.cache), F, x, τ, c)
end

function jacobian!(U, F::TraceTestPencil, x, τ, c::TraceTestPencilCache)
    λ = x[end]
    jacobian!(U, F.F, x, F.l, c.cache)
    for j = 1:length(x)-1
        U[end, j] *= λ
    end
    U[end, end] = F.l[end]
    for i = 1:length(x)-1
        U[end, end] += F.l[i] * x[i]
    end
    U
end
function jacobian(F::TraceTestPencil, x, τ, c::TraceTestPencilCache)
    jacobian!(jacobian(F.F, x, F.l, c.cache), F, x, τ, c)
end

function evaluate_and_jacobian!(u, U, F::TraceTestPencil, x, τ, c::TraceTestPencilCache)
    λ = x[end]
    evaluate_and_jacobian!(u, U, F.F, x, F.l, c.cache)
    u[end] = u[end] * λ + τ[1] * F.γ
    for j = 1:length(x)-1
        U[end, j] *= λ
    end
    U[end, end] = F.l[end]
    for i = 1:length(x)-1
        U[end, end] += F.l[i] * x[i]
    end
    nothing
end

function differentiate_parameters!(u, F::TraceTestPencil, x, τ, c::TraceTestPencilCache)
    u .= zero(eltype(u))
    u[end, 1] = F.γ
    u
end

function differentiate_parameters(F::TraceTestPencil, x, τ, c::TraceTestPencilCache)
    u = similar(c.cache.u, size(F)[1], 1)
    differentiate_parameters!(u, F, x, τ, c)
end


import TriangleMesh, UnicodePlots, Plots
using Printf, LoopVectorization, LinearAlgebra, SparseArrays

include("CreateMeshFCFV.jl")
include("VisuFCFV.jl")
include("DiscretisationFCFV_Stokes.jl")
include("SolversFCFV_Stokes.jl")

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

function SetUpProblem!(mesh, P, Vx, Vy, Sxx, Syy, Sxy, VxDir, VyDir, SxxNeu, SyyNeu, SxyNeu, SyxNeu, sx, sy)
    # Evaluate T analytic on cell faces
    @avx for in=1:mesh.nf
        eta      = 1
        x        = mesh.xf[in]
        y        = mesh.yf[in]
        VxDir[in] = x^2*(1 - x)^2*(4*y^3 - 6*y^2 + 2*y)
        VyDir[in] =-y^2*(1 - y)^2*(4*x^3 - 6*x^2 + 2*x)
        # Stress at faces
        p         =  x*(1-x)
        # Using pseudo-tractions
        dVxdx = x^2*(2*x - 2)*(4*y^3 - 6*y^2 + 2*y) + 2*x*(1 - x)^2*(4*y^3 - 6*y^2 + 2*y);
        dVxdy = x^2*(1 - x)^2*(12*y^2 - 12*y + 2);
        dVydx = -y^2*(1 - y)^2*(12*x^2 - 12*x + 2);
        dVydy = -y^2*(2*y - 2)*(4*x^3 - 6*x^2 + 2*x) - 2*y*(1 - y)^2*(4*x^3 - 6*x^2 + 2*x);
        SxxNeu[in] = - p + eta*dVxdx
        SyyNeu[in] = - p + eta*dVydy
        SxyNeu[in] =       eta*dVxdy
        SyxNeu[in] =       eta*dVydx
        # SxxNeu[in] = -8*p*y*(x - 1)*(2*y^2 - 3*y + 1) - p + 8*x^2*y*(x - 1)*(2*y^2 - 3*y + 1)
        # SyyNeu[in] = -p - 8*x*y^2*(y - 1)*(2*x^2 - 3*x + 1) - 8*x*y*(y - 1)^2*(2*x^2 - 3*x + 1)
        # SxyNeu[in] = p^2*(12*y^2 - 12*y + 2) + y^2*(y - 1)^2*(-12.0*x^2 + 12.0*x - 2.0)
        # SyxNeu[in] = SxyNeu[in]
    end
    # Evaluate T analytic on barycentres
    @avx for iel=1:mesh.nel
        x        = mesh.xc[iel]
        y        = mesh.yc[iel]
        p        =  x*(1-x)
        P[iel]   =  p
        Vx[iel]  =  x^2*(1 - x)^2*(4*y^3 - 6*y^2 + 2*y)
        Vy[iel]  = -y^2*(1 - y)^2*(4*x^3 - 6*x^2 + 2*x)
        Sxx[iel] = -8*p*y*(x - 1)*(2*y^2 - 3*y + 1) - p + 8*x^2*y*(x - 1)*(2*y^2 - 3*y + 1)
        Syy[iel] = -p - 8*x*y^2*(y - 1)*(2*x^2 - 3*x + 1) - 8*x*y*(y - 1)^2*(2*x^2 - 3*x + 1)
        Sxy[iel] = p^2*(12*y^2 - 12*y + 2) + y^2*(y - 1)^2*(-12.0*x^2 + 12.0*x - 2.0)
        sx[iel]  = -p^2*(24*y - 12) - 4*x^2*(4*y^3 - 6*y^2 + 2*y) - 8*x*(2*x - 2)*(4*y^3 - 6*y^2 + 2*y) - 2*x + 1.0*y^2*(2*y - 2)*(12*x^2 - 12*x + 2) + 2.0*y*(1 - y)^2*(12*x^2 - 12*x + 2) - 4*(1 - x)^2*(4*y^3 - 6*y^2 + 2*y) + 1
        sy[iel]  = -2*p*(1 - x)*(12*y^2 - 12*y + 2) - x^2*(2*x - 2)*(12*y^2 - 12*y + 2) + 1.0*y^2*(1 - y)^2*(24*x - 12) + 4*y^2*(4*x^3 - 6*x^2 + 2*x) + 8*y*(2*y - 2)*(4*x^3 - 6*x^2 + 2*x) + 4*(1 - y)^2*(4*x^3 - 6*x^2 + 2*x)
    end
    return
end

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

function ComputeError( mesh, Vxe, Vye, Txxe, Tyye, Txye, Pe )
    eVx  = zeros(mesh.nel)
    eVy  = zeros(mesh.nel)
    eTxx = zeros(mesh.nel)
    eTyy = zeros(mesh.nel)
    eTxy = zeros(mesh.nel)
    eP   = zeros(mesh.nel)
    Vxa  = zeros(mesh.nel)
    Vya  = zeros(mesh.nel)
    Txxa = zeros(mesh.nel)
    Tyya = zeros(mesh.nel)
    Txya = zeros(mesh.nel)
    Pa   = zeros(mesh.nel)
    @avx for iel=1:mesh.nel
        x         = mesh.xc[iel]
        y         = mesh.yc[iel]
        Pa[iel]   =  x*(1-x)
        Vxa[iel]  =  x^2*(1 - x)^2*(4*y^3 - 6*y^2 + 2*y)
        Vya[iel]  = -y^2*(1 - y)^2*(4*x^3 - 6*x^2 + 2*x)
        Txxa[iel] = -8*Pa[iel]*y*(x - 1)*(2*y^2 - 3*y + 1) + 8*x^2*y*(x - 1)*(2*y^2 - 3*y + 1)
        Tyya[iel] = -8*x*y^2*(y - 1)*(2*x^2 - 3*x + 1) - 8*x*y*(y - 1)^2*(2*x^2 - 3*x + 1)
        Txya[iel] = Pa[iel]^2*(12*y^2 - 12*y + 2) + y^2*(y - 1)^2*(-12.0*x^2 + 12.0*x - 2.0)
        eVx[iel]  = Vxe[iel]  - Vxa[iel]
        eVy[iel]  = Vye[iel]  - Vya[iel]
        eTxx[iel] = Txxe[iel] - Txxa[iel]
        eTyy[iel] = Tyye[iel] - Tyya[iel]
        eTxy[iel] = Txye[iel] - Txya[iel]
        eP[iel]   = Pe[iel]   - Pa[iel]
    end
    errVx  = norm(eVx) /norm(Vxa)
    errVy  = norm(eVy) /norm(Vya)
    errTxx = norm(eTxx)/norm(Txxa)
    errTyy = norm(eTyy)/norm(Tyya)
    errTxy = norm(eTxy)/norm(Txya)
    errP   = norm(eP)  /norm(Pa)
    return errVx, errVy, errTxx, errTyy, errTxy, errP, Txxa, Tyya, Txya
end

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

function StabParam(tau, dA, Vol, mesh_type, coeff)
    if mesh_type=="Quadrangles";        taui = tau;    end
    # if mesh_type=="UnstructTriangles";  taui = tau*dA; end
    if mesh_type=="UnstructTriangles";  taui = tau end
    return taui
end

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

@views function main()

    println("\n******** FCFV STOKES ********")

    # Create sides of mesh
    xmin, xmax = 0, 1
    ymin, ymax = 0, 1
    n          = 4
    nx, ny     = 8*n, 8*n
    solver     = 1
    R          = 0.5
    inclusion  = 0
    # mesh_type  = "Quadrangles"
    mesh_type  = "UnstructTriangles"

    # Generate mesh
    if mesh_type=="Quadrangles"
        tau  = 20
        mesh = MakeQuadMesh( nx, ny, xmin, xmax, ymin, ymax, inclusion, R )
    elseif mesh_type=="UnstructTriangles"
        tau  = 20
        mesh = MakeTriangleMesh( nx, ny, xmin, xmax, ymin, ymax, inclusion, R )
    end
    println("Number of elements: ", mesh.nel)

    # Source term and BCs etc...
    Pa     = zeros(mesh.nel)
    Vxa    = zeros(mesh.nel)
    Vya    = zeros(mesh.nel)
    Sxxa   = zeros(mesh.nel)
    Syya   = zeros(mesh.nel)
    Sxya   = zeros(mesh.nel)
    sex    = zeros(mesh.nel)
    sey    = zeros(mesh.nel)
    VxDir  = zeros(mesh.nf)
    VyDir  = zeros(mesh.nf)
    SxxNeu = zeros(mesh.nf)
    SyyNeu = zeros(mesh.nf)
    SxyNeu = zeros(mesh.nf)
    SyxNeu = zeros(mesh.nf)
    gbar   = zeros(mesh.nel,mesh.nf_el,2)
    println("Model configuration :")
    @time SetUpProblem!(mesh, Pa, Vxa, Vya, Sxxa, Syya, Sxya, VxDir, VyDir, SxxNeu, SyyNeu, SxyNeu, SyxNeu, sex, sey)

    # Compute some mesh vectors
    println("Compute FCFV vectors:")
    @time ae, be, ze = ComputeFCFV(mesh, sex, sey, VxDir, VyDir, SxxNeu, SyyNeu, SxyNeu, SyxNeu, tau)

    # Assemble element matrices and RHS
    println("Compute element matrices:")
    @time Kuu_v, fu_v, Kup_v, fp = ElementAssemblyLoop(mesh, ae, be, ze, VxDir, VyDir, SxxNeu, SyyNeu, SxyNeu, SyxNeu, gbar, tau)

    # Assemble triplets and sparse
    println("Assemble triplets and sparse:")
    @time Kuu, fu, Kup = CreateTripletsSparse(mesh, Kuu_v, fu_v, Kup_v)

    # Solve for hybrid variable
    println("Linear solve:")
    @time Vxh, Vyh, Pe = StokesSolvers(mesh, Kuu, Kup, fu, fp, solver)

    # # Reconstruct element values
    println("Compute element values:")
    @time Vxe, Vye, Txxe, Tyye, Txye = ComputeElementValues(mesh, Vxh, Vyh, Pe, ae, be, ze, VxDir, VyDir, tau)

    # # Compute discretisation errors
    err_Vx, err_Vy, err_Txx, err_Tyy, err_Txy, err_P, Txxa, Tyya, Txya = ComputeError( mesh, Vxe, Vye, Txxe, Tyye, Txye, Pe )
    @printf("Error in Vx : %2.2e\n", err_Vx )
    @printf("Error in Vy : %2.2e\n", err_Vy )
    @printf("Error in Txx: %2.2e\n", err_Txx)
    @printf("Error in Tyy: %2.2e\n", err_Tyy)
    @printf("Error in Txy: %2.2e\n", err_Txy)
    @printf("Error in P  : %2.2e\n", err_P  )

    println(minimum(Pa))
    println(minimum(Pe))
    println(maximum(Pa))
    println(maximum(Pe))

    # Visualise
    println("Visualisation:")
    # @time PlotMakie( mesh, sex )
    @time PlotMakie( mesh, Pe, xmin, xmax, ymin, ymax, :viridis )
    # PlotElements( mesh )

end

main()


"""
    parallactic_angles(filelist; latitude=19.82636)

Calculate the parallactic angles (in degrees) for the given FITS files in the middle of the exposure. Uses the given
`latitude` which defaults to the Keck II site.
"""
parallactic_angles(filelist; kwargs...) = map(f -> parallactic_angle(read_header(f); kwargs...), filelist)

parallactic_angles(filelist::DataFrame; kwargs...) = map(r -> parallactic_angle(r; kwargs...), eachrow(filelist))

function parallactic_angle(row; latitude=19.82636)
    # compute the hour angle at the middle of the frame
    exp_start = Time(row["EXPSTART"])
    exp_stop = Time(row["EXPSTOP"])
    exp_mid = Dates.value((exp_stop - exp_start) / 2)
    # convert exp_mid from nanoseconds of time to degrees
    Δha = exp_mid / 1e9 / 24 / 10
    ha_mid = row["HA"] + Δha

    # precess the star coordinates to the appropriate epoch
    ra = deg2rad(row["RA"])
    dec = deg2rad(row["DEC"])
    coord = FK5Coords{2000}(ra, dec)
    obs_epoch = Date(row["DATE-OBS"])
    yr = year(obs_epoch) + (dayofyear(obs_epoch) - 1) / daysinyear(obs_epoch)
    coord_curr = convert(FK5Coords{yr}, coord)

    # derive true PA at the middle of the frame
    ra = coord_curr.ra
    dec = coord_curr.dec
    PA = -rad2deg(atan(-sind(ha_mid), cos(dec) * tand(latitude) - sin(dec) * cosd(ha_mid)))
    # apply instrumental effects
    PA += row["ROTPOSN"] - row["INSTANGL"] + (row["PARANG"] - row["PARANTEL"])

    return PA
end


# Leap on every year that is evenly divisible by 4
# except every year that is evenly divisible by 100
# unless the year is also evenly divisible by 400
is_leap_year(year::Int) = (year % 100 != 0 && year % 4 == 0) || (year % 400 == 0)


#############################################################################
# MathProgBase.jl
#############################################################################
# test/conininterface.jl
# Test the MathProgBase.jl interface for a provided conic solver.
#############################################################################

using Compat.Test
using Compat.LinearAlgebra
using Compat.SparseArrays
using MathProgBase

function coniclineartest(solver::MathProgBase.AbstractMathProgSolver;duals=false, tol=1e-6)
    @testset "Testing linear problems through conic interface with $solver" begin
        # Problem LIN1 - all vars in nonneg cone
        # min -3x - 2y - 4z
        # st    x +  y +  z == 3 '4' -> z=2, x->2, obj -> -14
        #            y +  z == 2
        #       x>=0 y>=0 z>=0
        # Opt solution = -11
        # x = 1, y = 0, z = 2
        @testset "LIN1" begin
            c = [-3.0, -2.0, -4.0]
            A = [ 1.0   1.0   1.0;
                  0.0   1.0   1.0]
            b = [ 3.0,  2.0]
            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m, c, A, b, [(:Zero,1:2)], [(:NonNeg, 1:3)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -11, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 0.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 2.0, atol=tol)
            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(d[1], 3.0, atol=tol)
                @test isapprox(d[2], 1.0, atol=tol)
                vardual = c + A'd
                var = MathProgBase.getvardual(m)
                @test isapprox(vardual, var, atol=tol)
            end
        end

        # Problem LIN1A - same as Problem LIN1, but with variable bounds
        #              as constraints instead
        # min   -3x - 2y - 4z
        # st  [3] - [x + y + z] ZERO
        #     [2] - [    y + z] ZERO
        #     [0] - [x        ] NONPOS   x>=0 -> 0<=x -> 0-x<=0
        #     [0] - [    y    ] NONPOS   y>=0 -> 0<=y -> 0-y<=0
        #     [0] - [       -z] NONNEG   z>=0 -> 0<=z -> 0-z<=0 -> 0+z>=0
        # Opt solution = -11
        # x = 1, y = 0, z = 2
        @testset "LIN1A" begin
            c = [-3.0, -2.0, -4.0]
            A = [ 1.0   1.0   1.0;
                  0.0   1.0   1.0;
                  1.0   0.0   0.0;
                  0.0   1.0   0.0;
                  0.0   0.0  -1.0]
            b = [ 3.0,  2.0,  0.0,  0.0,  0.0]
            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m,c, A, b, [(:Zero,1:2),(:NonPos,3:4),(:NonNeg,5)],
            [(:Free, 1:3)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -11, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 0.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 2.0, atol=tol)
            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(d[1], 3.0, atol=tol)
                @test isapprox(d[2], 1.0, atol=tol)
                @test isapprox(d[3], 0.0, atol=tol)
                @test isapprox(d[4], -2.0, atol=tol)
                @test isapprox(d[5], 0.0, atol=tol)
                vardual = c + A'd
                var = MathProgBase.getvardual(m)
                @test isapprox(vardual, var, atol=tol)
            end
        end

        # Problem LIN2 - mixed free, nonneg, nonpos, zero, shuffled cones
        # min  3x + 2y - 4z + 0s
        # st    x           -  s  == -4    (i.e. x >= -4)
        #            y            == -3
        #       x      +  z       == 12
        #       x free
        #       y <= 0
        #       z >= 0
        #       s zero
        # Opt solution = -82
        # x = -4, y = -3, z = 16, s == 0
        @testset "LIN2" begin
            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m,
            [ 3.0,  2.0, -4.0,  0.0],
            [ 1.0   0.0   0.0  -1.0;
              0.0   1.0   0.0   0.0;
              1.0   0.0   1.0   0.0],
            [-4.0, -3.0, 12.0],
            [(:Zero,1:3)],
            [(:Free,1), (:NonNeg,3), (:Zero,4), (:NonPos,2)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -82, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], -4.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], -3.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 16.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[4], 0.0, atol=tol)
            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(d[1], -7.0, atol=tol)
                @test isapprox(d[2], -2.0, atol=tol)
                @test isapprox(d[3], 4.0, atol=tol)
            end
        end

        # Problem LIN2A - Problem LIN2 but with y,z variable bounds as constraints
        # min       3x + 2y - 4z + 0s
        # st  [-4]-[ x           -  s] ZERO    (i.e. x >= -4)
        #     [-3]-[      y          ] ZERO
        #     [12]-[ x      +  z     ] ZERO
        #     [ 0]-[      y          ] NONEG
        #     [ 0]-[           z     ] NONPOS
        #       x free
        #       s zero
        # Opt solution = -82
        # x = -4, y = -3, z = 16, s == 0
        @testset "LIN2A" begin
            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m,
            [ 3.0,  2.0, -4.0,  0.0],
            [ 1.0   0.0   0.0  -1.0;
              0.0   1.0   0.0   0.0;
              1.0   0.0   1.0   0.0;
              0.0   1.0   0.0   0.0;
              0.0   0.0   1.0   0.0],
            [-4.0, -3.0, 12.0,  0.0,  0.0],
            [(:Zero,1:3),(:NonNeg,4),(:NonPos,5)],
            [(:Free,1), (:NonNeg,3), (:Zero,4), (:NonPos,2)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -82, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], -4.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], -3.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 16.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[4], 0.0, atol=tol)
            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(d[1], -7.0, atol=tol)
                @test isapprox(d[2], -2.0, atol=tol)
                @test isapprox(d[3], 4.0, atol=tol)
                @test isapprox(d[4], 0.0, atol=tol)
                @test isapprox(d[5], 0.0, atol=tol)
            end
        end

        # Problem LIN3 - Infeasible LP
        # min  0
        # s.t. x ≥ 1
        #      x ≤ -1
        # in conic form:
        # min 0
        # s.t. -1 + x ∈ R₊
        #       1 + x ∈ R₋
        @testset "LIN3 infeasible" begin
            b = [-1, 1]
            A = reshape([-1, -1], 2, 1)
            c = [0]
            constr_cones = [(:NonNeg,1:1),(:NonPos,2:2)]
            var_cones = [(:Free,1:1)]

            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m, c, A, b, constr_cones, var_cones)
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Infeasible
            if duals
                y = MathProgBase.getdual(m)
                @test y[1] > 0
                @test y[2] < 0
                @test isapprox((A'y)[1], 0.0, atol=tol)
                @test -dot(b,y) > 0
            end
        end

        # Problem LIN4 - Infeasible LP
        # min  0
        # s.t. x ≥ 1
        #      x ≤ 0
        # in conic form:
        # min 0
        # s.t. -1 + x ∈ R₊
        #           x ∈ R₋
        @testset "LIN4 infeasible" begin
            b = [-1]
            A = reshape([-1], 1, 1)
            c = [0]
            constr_cones = [(:NonNeg,1:1)]
            var_cones = [(:NonPos,1:1)]

            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m, c, A, b, constr_cones, var_cones)
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Infeasible
            if duals
                y = MathProgBase.getdual(m)
                @test y[1] > 0
                @test -dot(b,y) > 0
            end
        end
    end
end

function conicSOCtest(solver::MathProgBase.AbstractMathProgSolver;duals=false, tol=1e-6)
    @testset "Testing SOC problems through conic interface with $solver" begin
        # Problem SOC1
        # min 0x - 1y - 1z
        #  st  x            == 1
        #      x >= ||(y,z)||
        @testset "SOC1" begin
            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m,
            [ 0.0, -1.0, -1.0],
            [ 1.0   0.0   0.0],
            [ 1.0],
            [(:Zero,1)],
            [(:SOC,1:3)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 1.0/sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 1.0/sqrt(2.0), atol=tol)
            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(d[1], sqrt(2.0), atol=tol)
            end
        end

        # Problem SOC1A - Problem SOC1 but in ECOS form
        # min      0x - 1y - 1z
        #  st [1]-[ x          ] ZERO
        #     [0]-[-x          ] SOC
        #     [0]-[     -y     ] SOC
        #     [0]-[          -z] SOC
        @testset "SOC1A" begin
            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m,
            [ 0.0, -1.0, -1.0],
            [ 1.0   0.0   0.0;
             -1.0   0.0   0.0;
              0.0  -1.0   0.0;
              0.0   0.0  -1.0],
            [ 1.0, 0.0, 0.0, 0.0],
            Any[(:Zero,1),(:SOC,2:4)],
            [(:Free,1:3)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 1.0/sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 1.0/sqrt(2.0), atol=tol)
            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(d[1], sqrt(2.0), atol=tol)
                @test isapprox(d[2], sqrt(2.0), atol=tol)
                @test isapprox(d[3], -1.0, atol=tol)
                @test isapprox(d[4], -1.0, atol=tol)
            end
            # Permute indices - used to cause Mosek failure
            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m,
            [ 0.0, -1.0, -1.0],
            [ 1.0   0.0   0.0;
              0.0   0.0  -1.0;
              0.0  -1.0   0.0;
             -1.0   0.0   0.0],
            [ 1.0, 0.0, 0.0, 0.0],
            Any[(:Zero,1),(:SOC,[4,3,2])],
            [(:Free,1:3)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 1.0/sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 1.0/sqrt(2.0), atol=tol)
            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(d[1], sqrt(2.0), atol=tol)
                @test isapprox(d[4], sqrt(2.0), atol=tol)
                @test isapprox(d[3], -1.0, atol=tol)
                @test isapprox(d[2], -1.0, atol=tol)
            end

        end

        # Problem SOC2
        # min  x
        # s.t. y ≥ 1/√2
        #      x² + y² ≤ 1
        # in conic form:
        # min  x
        # s.t.  -1/√2 + y ∈ R₊
        #        1 - t ∈ {0}
        #      (t,x,y) ∈ SOC₃
        @testset "SOC2" begin
            b = [-1/sqrt(2), 1, 0, 0, 0]
            A = [ 0 -1 0
                  0 0 1
                  0 0 -1
                 -1 0 0
                  0 -1 0 ]
            c = [ 1, 0, 0 ]
            constr_cones = [(:NonNeg,1:1),(:Zero,2:2),(:SOC,3:5)]
            var_cones = [(:Free,1:3)]

            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m, c, A, b, constr_cones, var_cones)
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -1/sqrt(2), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], -1/sqrt(2), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 1/sqrt(2), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 1.0, atol=tol)

            if duals
                y = MathProgBase.getdual(m)
                @test isapprox(-dot(b,y), -1/sqrt(2), atol=tol)
                @test isapprox(norm(c + A'y), 0.0, atol=tol)
                s = MathProgBase.getvardual(m)
                @test isapprox(norm(s), 0.0, atol=tol)
            end
        end

        # Problem SOC2A
        # Same as above but with NonPos instead of NonNeg
        # min  x
        # s.t.  1/√2 - y ∈ R₋
        #        1 - t ∈ {0}
        #      (t,x,y) ∈ SOC₃
        @testset "SOC2A" begin
            b = [1/sqrt(2), 1, 0, 0, 0]
            A = [ 0 1 0
                  0 0 1
                  0 0 -1
                  -1 0 0
                  0 -1 0 ]
            c = [ 1, 0, 0 ]
            constr_cones = [(:NonPos,1:1),(:Zero,2:2),(:SOC,3:5)]
            var_cones = [(:Free,1:3)]

            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m, c, A, b, constr_cones, var_cones)
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -1/sqrt(2), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], -1/sqrt(2), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 1/sqrt(2), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 1.0, atol=tol)

            if duals
                y = MathProgBase.getdual(m)
                @test isapprox(-dot(b,y), -1/sqrt(2), atol=tol)
                @test isapprox(norm(c + A'y), 0.0, atol=tol)
                s = MathProgBase.getvardual(m)
                @test isapprox(norm(s), 0.0, atol=tol)
            end
        end

        # Problem SOC3 - Infeasible
        # min 0
        # s.t. y ≥ 2
        #      x ≤ 1
        #      |y| ≤ x
        # in conic form:
        # min 0
        # s.t. -2 + y ∈ R₊
        #      -1 + x ∈ R₋
        #       (x,y) ∈ SOC₂
        @testset "SOC3 infeasible" begin
            b = [-2, -1]
            A = [0 -1; -1 0]
            c = [0,0]
            constr_cones = [(:NonNeg,1:1),(:NonPos,2:2)]
            var_cones = [(:SOC,1:2)]

            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m, c, A, b, constr_cones, var_cones)
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Infeasible

            if duals
                y = MathProgBase.getdual(m)
                @test y[1] > 0
                @test y[2] < 0
                @test (A'y)[1]/abs((A'y)[2]) ≥ 1-tol
                @test -dot(b,y) > 0
            end
        end

        if duals
            # Problem SOC4
            # Like SOCINT1 but with copies of variables and integrality relaxed
            # Tests out-of-order indices in cones
            @testset "SOC4" begin
                b = [1.0,0.0,0.0]
                A = [1.0 0.0 0.0 0.0 0.0
                     0.0 1.0 0.0 -1.0 0.0
                     0.0 0.0 1.0 0.0 -1.0]
                c = [0.0,-2.0,-1.0,0.0,0.0]
                constr_cones = [(:Zero,[1]),(:Zero,[2,3])]
                var_cones = [(:SOC,[1,4,5]),(:Free,[2,3])]

                m = MathProgBase.ConicModel(solver)
                MathProgBase.loadproblem!(m, c, A, b, constr_cones, var_cones)
                MathProgBase.optimize!(m)
                @test MathProgBase.status(m) == :Optimal

                x = MathProgBase.getsolution(m)
                y = MathProgBase.getdual(m)
                s = MathProgBase.getvardual(m)

                @test isapprox(dot(c,x), -dot(y,b), atol=tol)
                @test x[1]^2 ≥ x[4]^2 + x[5]^2 - tol
                @test s[1]^2 ≥ s[4]^2 + s[5]^2 - tol
                @test isapprox(s[2], 0.0, atol=tol)
                @test isapprox(s[3], 0.0, atol=tol)
                @test isapprox(norm((c+A'y) - s), 0.0, atol=tol)
            end
        end
    end
end

function conicSOCRotatedtest(solver::MathProgBase.AbstractMathProgSolver;duals=false, tol=1e-6)
    @testset "Testing SOCRotated problems through conic interface with $solver" begin
        # Problem SOCRotated1
        # min 0a + 0b - 1x - 1y
        #  st  a            == 1/2
        #  st  b            == 1
        #      2a*b >= y^2+z^2
        @testset "SOCRotated1" begin
            c = [ 0.0, 0.0, -1.0, -1.0]
            A = [ 1.0  0.0   0.0   0.0
                  0.0  1.0   0.0   0.0]
            b = [ 0.5, 1.0]
            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m,c,A,b,[(:Zero,1:2)],[(:SOCRotated,1:4)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 0.5, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 1.0/sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[4], 1.0/sqrt(2.0), atol=tol)
            if duals
                d = MathProgBase.getdual(m)
                dualobj = -dot(b,d)
                @test isapprox(dualobj, -sqrt(2.0), atol=tol)
                @test all(d .>= 0)
                vardual = MathProgBase.getvardual(m)
                @test isapprox(norm(vardual - (c+ A'd)), 0.0, atol=tol)
                @test 2*vardual[1]*vardual[2] ≥ vardual[3]^2 + vardual[4]^2 - tol
            end
        end

        # Problem SOCRotated1A - Problem SOCRotated1 with a and b substituted
        # min          -y - z
        #  st [0.5] - [      ] SOCRotated
        #     [1.0] - [      ] SOCRotated
        #     [0.0] - [-y    ] SOCRotated
        #     [0.0] - [    -z] SOCRotated
        @testset "SOCRotated1A" begin
            c = [-1.0,-1.0]
            A = [0.0 0.0; 0.0 0.0; -1.0 0.0; 0.0 -1.0]
            b = [0.5, 1.0, 0.0, 0.0]
            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m,c,A,b,
            [(:SOCRotated,1:4)],
            [(:Free,1:2)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 1.0/sqrt(2.0), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 1.0/sqrt(2.0), atol=tol)
            if duals
                d = MathProgBase.getdual(m)
                @test 2*d[1]*d[2] ≥ d[3]^2 + d[4]^2 - tol
                dualobj = -dot(b,d)
                @test isapprox(dualobj, -sqrt(2.0), atol=tol)

                vardual = MathProgBase.getvardual(m)
                @test isapprox(norm(vardual - (c+ A'd)), 0.0, atol=tol)
                @test isapprox(norm(vardual), 0.0, atol=tol)
            end
        end

        # Problem SOCRotated2 - Infeasible
        # min 0
        # s.t. z ≥ 2
        #      x ≤ 1
        #      y = 1/2
        #      z^2 ≤ 2x*y
        # in conic form:
        # min 0
        # s.t. -2 + z ∈ R₊
        #      -1 + x ∈ R₋
        #     1/2 - y ∈ {0}
        #       (x,y,z) ∈ SOCRoated
        @testset "SOCRotated2 infeasible" begin
            b = [-2, -1, 1/2]
            A = [0 0 -1; -1 0 0; 0 1 0]
            c = [0,0,0]
            constr_cones = [(:NonNeg,1:1),(:NonPos,2:2),(:Zero,3:3)]
            var_cones = [(:SOCRotated,1:3)]

            m = MathProgBase.ConicModel(solver)
            MathProgBase.loadproblem!(m, c, A, b, constr_cones, var_cones)
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Infeasible

            if duals
                y = MathProgBase.getdual(m)
                @test y[1] > 0
                @test y[2] < 0
                vardual = MathProgBase.getvardual(m)
                @test isapprox(norm(A'y-vardual), 0.0, atol=tol)
                @test 2*vardual[1]*vardual[2] ≥ vardual[3]^2
                @test -dot(b,y) > 0
            end
        end
    end
end

function conicSOCINTtest(solver::MathProgBase.AbstractMathProgSolver;tol=1e-6)
    @testset "Testing SOCINT problems through conic interface with $solver" begin
        # Problem SOCINT1
        # min 0x - 2y - 1z
        #  st  x            == 1
        #      x >= ||(y,z)||
        #      (y,z) binary
        m = MathProgBase.ConicModel(solver)
        MathProgBase.loadproblem!(m,
        [ 0.0, -2.0, -1.0],
        [ 1.0   0.0   0.0],
        [ 1.0],
        [(:Zero,1)],
        [(:SOC,1:3)])
        MathProgBase.setvartype!(m, [:Cont,:Bin,:Bin])
        MathProgBase.optimize!(m)
        @test MathProgBase.status(m) == :Optimal
        @test isapprox(MathProgBase.getobjval(m), -2, atol=tol)
        @test isapprox(MathProgBase.getsolution(m)[1], 1.0, atol=tol)
        @test isapprox(MathProgBase.getsolution(m)[2], 1.0, atol=tol)
        @test isapprox(MathProgBase.getsolution(m)[3], 0.0, atol=tol)
    end
end

function conicEXPtest(solver::MathProgBase.AbstractMathProgSolver;duals=false, tol=1e-6)
    @testset "Testing EXP problems through conic interface with $solver" begin
        # Problem EXP1 - ExpPrimal
        # min x + y + z
        #  st  y e^(x/y) <= z, y > 0 (i.e (x, y, z) are in the exponential primal cone)
        #      x == 1
        #      y == 2
        @testset "EXP1" begin
            m = MathProgBase.ConicModel(solver)
            c = [1.0, 1.0, 1.0]
            A = [0.0 1.0 0.0;
                 1.0 0.0 0.0]
            b = [2.0, 1.0]
            MathProgBase.loadproblem!(m, c, A, b, [(:Zero,1:2)], [(:ExpPrimal, 1:3)])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), (2*exp(1/2)+3), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 2.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 2*exp(1/2), atol=tol)

            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(-dot(b,d), MathProgBase.getobjval(m), atol=tol)
                u,v,w = c+A'd
                # should belong to the ExpDual cone
                @test u < 0
                @test -u*exp(v/w) ≤ exp(1)*w + tol
                s = MathProgBase.getvardual(m)
                @test isapprox(norm(s - (c+A'd)), 0.0, atol=tol)
            end

            # Permute indices - used to cause Mosek failure
            m = MathProgBase.ConicModel(solver)
            c = [1.0, 1.0, 1.0]
            A = [0.0 1.0 0.0;
                 0.0 0.0 1.0]
            b = [2.0, 1.0]
            MathProgBase.loadproblem!(m, c, A, b, [(:Zero,1:2)], [(:ExpPrimal, [3,2,1])])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), (2*exp(1/2)+3), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 2.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 2*exp(1/2), atol=tol)

            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(-dot(b,d), MathProgBase.getobjval(m), atol=tol)
                w,v,u = c+A'd
                # should belong to the ExpDual cone
                @test u < 0
                @test -u*exp(v/w) ≤ exp(1)*w + tol
                s = MathProgBase.getvardual(m)
                @test isapprox(norm(s - (c+A'd)), 0.0, atol=tol)
            end
        end

        # Problem EXP1A - ExpPrimal
        # Same as previous, except we put :ExpPrimal on constr_cones
        @testset "EXP1A" begin
            m = MathProgBase.ConicModel(solver)
            c = [1.0, 1.0, 1.0]
            A = [0.0 1.0 0.0;
                 1.0 0.0 0.0;
                -1.0 0.0 0.0;
                 0.0 -1.0 0.0;
                 0.0 0.0 -1.0]
            b = [2.0, 1.0, 0.0, 0.0, 0.0]
            MathProgBase.loadproblem!(m, c, A, b,
            [(:Zero,1:2), (:ExpPrimal, 3:5)],
            [(:Free, [2,3,1])])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), (2*exp(1/2)+3), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 2.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 2*exp(1/2), atol=tol)

            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(-dot(b,d), MathProgBase.getobjval(m), atol=tol)
                @test isapprox(norm(c+A'd), 0.0, atol=tol)
                u,v,w = d[3:5]
                # should belong to the ExpDual cone
                @test u < 0
                @test -u*exp(v/w) ≤ exp(1)*w + tol
                s = MathProgBase.getvardual(m)
                @test isapprox(norm(s), 0.0, atol=tol)
            end

            # Permute indices - used to cause Mosek failure
            m = MathProgBase.ConicModel(solver)
            c = [1.0, 1.0, 1.0]
            A = [0.0 1.0 0.0;
                -1.0 0.0 0.0;
                 1.0 0.0 0.0;
                 0.0 -1.0 0.0;
                 0.0 0.0 -1.0]
            b = [2.0, 0.0, 1.0, 0.0, 0.0]
            MathProgBase.loadproblem!(m, c, A, b,
            [(:Zero,[1,3]), (:ExpPrimal, [2,4,5])],
            [(:Free, [3,2,1])])
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), (2*exp(1/2)+3), atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[1], 1.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[2], 2.0, atol=tol)
            @test isapprox(MathProgBase.getsolution(m)[3], 2*exp(1/2), atol=tol)

            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(-dot(b,d), MathProgBase.getobjval(m), atol=tol)
                @test isapprox(norm(c+A'd), 0.0, atol=tol)
                u,v,w = d[2],d[4],d[5]
                # should belong to the ExpDual cone
                @test u < 0
                @test -u*exp(v/w) ≤ exp(1)*w + tol
                s = MathProgBase.getvardual(m)
                @test isapprox(norm(s), 0.0, atol=tol)
            end
        end

        # Problem EXP2
        # A problem where ECOS was failing
        if duals
            @testset "EXP2" begin
                c = [0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0]
                b = [0.0,1.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0]

                I = [8,11,1,4,9,12,1,4,10,13,3,7,6,7,7,8,11,14,15,9,12,14,10,13,14]
                J = [1,1,2,2,2,2,3,3,3,3,4,4,5,5,6,7,7,7,7,8,8,8,9,9,9]
                V = [-1.0,1.0,-1.0,-1.0,-1.0,1.0,-1.0,1.0,-1.0,1.0,-1.0,-0.5,-1.0,-0.5,1.0,-0.3,-0.3,1.0,-1.0,-0.3,-0.3,1.0,-0.3,-0.3,1.0]
                A = sparse(I, J, V, length(b), length(c))

                cone_con = [(:ExpPrimal,1:3),(:ExpPrimal,4:6),(:Zero,7:7),(:NonNeg,8:10),(:NonNeg,11:13),(:NonNeg,14:14),(:Zero,15:15)]
                cone_var = [(:Free,1:9)]
                m = MathProgBase.ConicModel(solver)
                MathProgBase.loadproblem!(m, c, A, b, cone_con, cone_var)
                MathProgBase.optimize!(m)

                primalobj = MathProgBase.getobjval(m)
                primalsol = MathProgBase.getsolution(m)
                # check primal feasibility
                conval = b-A*primalsol
                @test conval[3] + tol ≥ conval[2]*exp(conval[1]/conval[2])
                @test conval[6] + tol ≥ conval[5]*exp(conval[4]/conval[5])
                @test isapprox(conval[7], 0.0, atol=tol)
                for i in 8:14
                    @test conval[i] ≥ -tol
                end

                @test isapprox(conval[15], 0.0, atol=tol)

                y = MathProgBase.getdual(m)
                dualobj = -dot(b,y)
                @test isapprox(primalobj, dualobj, atol=tol)
                dualconval = c + A'*y
                var = MathProgBase.getvardual(m)
                @test isapprox(dualconval, var, atol=tol)
                for i in 1:9
                    @test isapprox(dualconval[i], 0.0, atol=tol)
                end
                if y[1] == 0.0
                    @test y[2] ≥ -tol
                    @test y[3] ≥ -tol
                else
                    @test y[1] < 0.0
                    @test y[3] > 0.0
                    @test -y[1]*log(-y[1]/y[3]) + y[1] - y[2] ≤ tol
                end
                if y[4] == 0.0
                    @test y[5] ≥ -tol
                    @test y[6] ≥ -tol
                else
                    @test y[4] < 0.0
                    @test y[6] > 0.0
                    @test -y[4]*log(-y[4]/y[6]) + y[4] - y[5] ≤ tol
                end
                for i in 8:14
                    @test y[i] ≥ -tol
                end
            end
        end

        # Problem EXP3
        # Another problem where ECOS was failing
        if duals
            @testset "EXP3" begin
                b = [4.0,0.0,1.0,0.0,5.0]
                c = [-1.0,0.0]

                I = [1,2,4,5]
                J = [1,1,2,2]
                V = [2.0,-1.0,-1.0,1.0]
                A = sparse(I,J,V,5,2)
                cone_var = [(:Free,[1,2])]
                cone_con = [(:NonNeg,[1]),(:ExpPrimal,[2,3,4]),(:NonNeg,[5])]
                m = MathProgBase.ConicModel(solver)
                MathProgBase.loadproblem!(m, c, A, b, cone_con, cone_var)
                MathProgBase.optimize!(m)

                primalobj = MathProgBase.getobjval(m)
                primalsol = MathProgBase.getsolution(m)
                # check primal feasibility
                conval = b-A*primalsol
                @test conval[4] + tol ≥ conval[3]*exp(conval[2]/conval[3])
                @test conval[1] ≥ -tol
                @test conval[5] ≥ -tol

                y = MathProgBase.getdual(m)
                dualobj = -dot(b,y)
                @test isapprox(primalobj, dualobj, atol=tol)
                dualconval = c + A'*y
                var = MathProgBase.getvardual(m)
                @test isapprox(norm(dualconval - var), 0.0, atol=tol)
                for i in 1:2
                    @test isapprox(dualconval[1], 0.0, atol=tol)
                end
                @test y[1] ≥ -tol
                @test -y[2]*log(-y[2]/y[4]) + y[2] - y[3] ≤ tol
                @test y[5] ≥ -tol
            end
        end

        # Problem EXPSOC1
        # A problem where MOSEK was failing
        @testset "EXPSOC1" begin
            m = MathProgBase.ConicModel(solver)
            c = [0.0, 0.0, 1.0, 0.0, 0.0]
            A = [-1.0 0.0 0.0 0.0 0.0; 1.0 3.0 1.0 0.0 0.0; 0.0 -1.0 0.0 0.0 0.0; -1.0 0.0 0.0 0.0 0.0; 2.0 3.0 0.0 0.0 0.0; 0.0 0.0 0.0 -1.0 0.0; 0.0 0.0 0.0 1.0 0.0; -2.0 0.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0 -1.0; 0.0 0.0 0.0 0.0 0.0; 0.0 0.0 0.0 -1.0 0.0; 0.0 1.0 0.0 0.0 1.0]
            b = [0.0, 0.0, -1.0, 2.0, 30.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 7.0]
            con_cones = Tuple{Symbol,Array{Int64,1}}[(:NonNeg, [1]), (:Zero, [2]), (:NonNeg, [3]), (:NonNeg, [4]), (:NonNeg, [5]), (:SOC, [6, 7, 8]), (:NonNeg, [9]), (:ExpPrimal, [12, 11, 10]), (:NonNeg, [13])]
            var_cones = Tuple{Symbol,Array{Int64,1}}[(:Free, [1, 2, 3, 4, 5])]
            MathProgBase.loadproblem!(m, c, A, b, con_cones, var_cones)
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            @test isapprox(MathProgBase.getobjval(m), -18.082226, atol=tol)

            if duals
                d = MathProgBase.getdual(m)
                @test isapprox(-dot(b,d), MathProgBase.getobjval(m), atol=tol)
                u,v,w = d[12],d[11],d[10]
                # should belong to the ExpDual cone
                @test u < 0
                @test -u*exp(v/w) ≤ exp(1)*w + tol
            end
        end
    end
end

function conicSDPtest(solver::MathProgBase.AbstractMathProgSolver;duals=true, tol=1e-6)
    s2 = sqrt(2)
    @testset "Testing SDP problems through conic interface with $solver" begin
        # Problem SDP1 - sdo1 from MOSEK docs
        # From Mosek.jl/test/mathprogtestextra.jl, under license:
        #   Copyright (c) 2013 Ulf Worsoe, Mosek ApS
        #   Permission is hereby granted, free of charge, to any person obtaining a copy of this
        #   software and associated documentation files (the "Software"), to deal in the Software
        #   without restriction, including without limitation the rights to use, copy, modify, merge,
        #   publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons
        #   to whom the Software is furnished to do so, subject to the following conditions:
        #   The above copyright notice and this permission notice shall be included in all copies or
        #   substantial portions of the Software.
        #
        #     | 2 1 0 |
        # min | 1 2 1 | . X + x1
        #     | 0 1 2 |
        #
        #
        # s.t. | 1 0 0 |
        #      | 0 1 0 | . X + x1 = 1
        #      | 0 0 1 |
        #
        #      | 1 1 1 |
        #      | 1 1 1 | . X + x2 + x3 = 1/2
        #      | 1 1 1 |
        #
        #      (x1,x2,x3) in C^3_q
        #      X in C_sdp
        #
        @testset "SDP1" begin
            m = MathProgBase.ConicModel(solver)
            #     x1   x2   x3    X11  X21  X31  X22  X32  X33
            c = [ 1.0, 0.0, 0.0,  2.0, s2,  0.0, 2.0, s2,  2.0 ]
            A = [ 1.0  0.0  0.0   1.0  0.0  0.0  1.0  0.0  1.0 ;  # A1
                  0.0  1.0  1.0   1.0  s2   s2   1.0  s2   1.0 ]  # A2
            b = [ 1.0, 0.5 ]

            MathProgBase.loadproblem!(m, c, A, b, [(:Zero,1:2)], [(:SOC,1:3),(:SDP,4:9)] )
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            pobj = MathProgBase.getobjval(m)
            @test isapprox(pobj, 7.05710509e-01, atol=tol)

            xx = MathProgBase.getsolution(m)
            x123 = xx[1:3]
            X = xx[4:9]

            if duals
                y = MathProgBase.getdual(m)
                # Check primal objective
                comp_pobj = dot(X,[2.0,s2,0.0, 2.0,s2, 2.0]) + x123[1]
                # Check dual objective
                comp_dobj = -dot(y,b)
                @test isapprox(comp_pobj, comp_dobj, atol=tol)

                var = c + A' * y
                @test (var[2]^2 + var[3]^2 - var[1]^2) < tol # (var[1],var[2],var[3]) in SOC
                s = MathProgBase.getvardual(m)
                @test isapprox(norm(var - s), 0.0, atol=tol)

                M = [s[4]    s[5]/s2 s[6]/s2
                     s[5]/s2 s[7]    s[8]/s2
                     s[6]/s2 s[8]/s2 s[9]]

                @test eigmin(M) > -tol
            end

            # Reverse row and column indices to check nonconsecutive conic indices
            m = MathProgBase.ConicModel(solver)
            row = collect(2:-1:1)
            col = collect(9:-1:1)
            MathProgBase.loadproblem!(m, c[col], A[row,col], b[row], [(:Zero,row)], [(:SOC,col[1:3]),(:SDP,col[4:9])] )
            MathProgBase.optimize!(m)
            @test MathProgBase.status(m) == :Optimal
            pobj = MathProgBase.getobjval(m)
            @test isapprox(pobj, 7.05710509e-01, atol=tol)

            xx = MathProgBase.getsolution(m)
            x123 = xx[col[1:3]]
            X = xx[col[4:9]]

            if duals
                y = MathProgBase.getdual(m)
                # Check primal objective
                comp_pobj = dot(X,[2.0,s2,0.0, 2.0,s2, 2.0]) + x123[1]
                # Check dual objective
                comp_dobj = -dot(y,b[row])
                @test isapprox(comp_pobj, comp_dobj, atol=tol)

                var = c[col] + A[row,col]' * y
                @test (var[col[2]]^2 + var[col[3]]^2 - var[col[1]]^2) < tol # (var[1],var[2],var[3]) in SOC
                s = MathProgBase.getvardual(m)
                @test isapprox(norm(var - s), 0.0, atol=tol)

                M = [s[col[4]]    s[col[5]]/s2 s[col[6]]/s2
                     s[col[5]]/s2 s[col[7]]    s[col[8]]/s2
                     s[col[6]]/s2 s[col[8]]/s2 s[col[9]]]

                @test eigmin(M) > -tol
            end
        end

        # Problem SDP2
        # Caused getdual to fail on SCS and Mosek
        @testset "SDP2" begin
            m = MathProgBase.ConicModel(solver)

            c = [-0.0,-0.0,-0.0,-0.0,-0.0,-0.0,-1.0]
            b = [10.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
            I = [1,2,8,9,10,11,1,3,8,9,10,11,1,4,8,9,10,11,1,5,8,1,6,8,9,10,11,1,7,8,9,10,11,8,10]
            J = [1,1,1,1,1,1,2,2,2,2,2,2,3,3,3,3,3,3,4,4,4,5,5,5,5,5,5,6,6,6,6,6,6,7,7]
            V = [1.0,1.0,-0.44999999999999996,0.45000000000000007,-0.4500000000000001,0.0,1.0,1.0,-0.7681980515339464,0.31819805153394654,-0.13180194846605373,0.0,1.0,1.0,-0.9000000000000001,0.0,0.0,0.0,1.0,1.0,-0.22500000000000003,1.0,1.0,-0.11250000000000003,0.1125,-0.11249999999999999,0.0,1.0,1.0,0.0,0.0,-0.22500000000000003,0.0,1.0,1.0]
            A = sparse(I, J, V, length(b), length(c))
            cone_con = [(:NonNeg,[1]),(:NonPos,[2,3,4,5,6,7]),(:SDP,8:10),(:Zero,11:11)]
            cone_var = [(:NonNeg,[1,2,3,4,5,6]),(:Free,[7])]

            MathProgBase.loadproblem!(m, c, A, b, cone_con, cone_var)
            MathProgBase.optimize!(m)

            @test MathProgBase.status(m) == :Optimal
            pobj = MathProgBase.getobjval(m)

            x = MathProgBase.getsolution(m)
            @test all(x[1:6] .> -tol)
            con = b - A * x
            @test eigmin([con[8] con[9]/s2 ; con[9]/s2 con[10]]) > -tol
            @test con[1] >= -tol
            @test all(con[2:7] .<= tol)
            @test isapprox(con[11], 0.0, atol=tol)

            if duals
                y = MathProgBase.getdual(m)
                @test eigmin([y[8] y[9]/s2 ; y[9]/s2 y[10]]) > -tol
                @test y[1] >= -tol
                @test all(y[2:7] .<= tol)
                var = c + A' * y
                @test all(var[1:6] .>= -tol)
                @test isapprox(var[7], 0.0, atol=tol)
                dobj = -dot(y,b)
                @test isapprox(pobj, dobj, atol=tol)
                s = MathProgBase.getvardual(m)
                @test isapprox(norm(s - var), 0.0, atol=tol)
            end
        end
    end
end


a = 355 // 113

Float64(a)