@testset "Woodbury" begin
seed!(123)
n = 5

d = 1 .+ rand(n)
dl = -rand(n-1)
du = -rand(n-1)
v = randn(n)
B = randn(n,2)

U = randn(n,2)
V = randn(2,n)
C = randn(2,2)

for elty in (Float32, Float64, ComplexF32, ComplexF64, Int)
    if elty == Int
        seed!(61516384)
        d = rand(1:100, n)
        dl = -rand(0:10, n-1)
        du = -rand(0:10, n-1)
        v = rand(1:100, n)
        B = rand(1:100, n, 2)

        # Woodbury
        U = rand(1:100, n, 2)
        V = rand(1:100, 2, n)
        C = rand(1:100, 2, 2)
    else
        d = convert(Vector{elty}, d)
        dl = convert(Vector{elty}, dl)
        du = convert(Vector{elty}, du)
        v = convert(Vector{elty}, v)
        B = convert(Matrix{elty}, B)
        U = convert(Matrix{elty}, U)
        V = convert(Matrix{elty}, V)
        C = convert(Matrix{elty}, C)
    end
    ε = eps(abs2(float(one(elty))))
    T = Tridiagonal(dl, d, du)

    # Matrix for A
    W = Woodbury(T, U, C, V)
    @test size(W, 1) == n
    @test size(W) == (n, n)
    @test axes(W) === (Base.OneTo(n), Base.OneTo(n))
    @test axes(W, 1) === Base.OneTo(n)
    F = Matrix(W)
    @test F ≈ T + U*C*V
    @test Array(W) == Matrix(W)
    if elty <: Real
        @test eltype(Matrix{Float64}(W)) === Float64
        @test eltype(Array{Float64}(W)) === Float64
    end
    @test W*v ≈ F*v
    iFv = F\v
    @test norm(W\v - iFv)/norm(iFv) <= n*cond(F)*ε # Revisit. Condition number is wrong
    @test norm(inv(W)*v - iFv)/norm(iFv) <= n*cond(F)*ε
    @test abs((det(W) - det(F))/det(F)) <= n*cond(F)*ε # Revisit. Condition number is wrong
    iWv = similar(iFv)
    if elty != Int
        iWv = ldiv!(W, copy(v))
        @test iWv ≈ iFv
    end
    @test W'*v ≈ F'*v
    @test transpose(W)*v ≈ transpose(F)*v
    @test (W + W) \ v ≈ (2*Matrix(W)) \ v

    # Diagonal matrix for A (lu(A) fails)
    D = Diagonal(d)
    W = Woodbury(D, U, C, V)
    F = Matrix(W)
    @test F ≈ D + U*C*V
    @test W*v ≈ F*v
    iFv = F\v
    @test norm(W\v - iFv)/norm(iFv) <= n*cond(F)*ε # Revisit. Condition number is wrong

    # Check division with diagonal matrix
    iFvmat = F\Diagonal(v)
    @test norm(W\Diagonal(v) - iFvmat)/norm(iFvmat) <= n*cond(F)*ε # Revisit. Condition number is wrong

    # Factorization for A
    W = Woodbury(lu(T), U, C, V)
    F = Matrix(W)
    @test_broken W*v ≈ F*v   # needs mul!(dst, ::LU, v, ...)
    iFv = F\v
    @test norm(W\v - iFv)/norm(iFv) <= n*cond(F)*ε # Revisit. Condition number is wrong
    @test abs((det(W) - det(F))/det(F)) <= n*cond(F)*ε # Revisit. Condition number is wrong
    iWv = similar(iFv)
    if elty != Int
        iWv = ldiv!(W, copy(v))
        @test iWv ≈ iFv
    end
end

# Sparse U and V
n = 5
d = 1 .+ rand(n)
dl = -rand(n-1)
du = -rand(n-1)
T = Tridiagonal(dl, d, du)
U = sprandn(n,2,0.2)
V = sprandn(2,n,0.2)
C = randn(2,2)
v = randn(n)
ε = eps()

W = Woodbury(T, U, C, V)
F = Matrix(W)
iFv = F\v
@test norm(W\v - iFv)/norm(iFv) <= n*cond(F)*ε

W = Woodbury(lu(T), U, C, V)
F = Matrix(W)
iFv = F\v
@test norm(W\v - iFv)/norm(iFv) <= n*cond(F)*ε

# Display
iob = IOBuffer()
show(iob, W)
str = String(take!(iob))
@test occursin("C:", str)

# Vector-valued U and scalar-valued C
U = rand(n)
C = 2.3
V = rand(1,n)
W = Woodbury(T, U, C, V)
F = Matrix(W)

v = randn(n)
ε = eps()
iFv = F\v
@test norm(W\v - iFv)/norm(iFv) <= n*cond(F)*ε

@test Matrix(2*W) ≈ 2*Matrix(W)
@test Matrix(W*2) ≈ 2*Matrix(W)
R = rand(size(W)...)
@test Matrix(W + R) ≈ Matrix(W) + R
@test Matrix(R + W) ≈ Matrix(W) + R

show(iob, W)

Wc = copy(W)

# Mismatched sizes
@test_throws DimensionMismatch Woodbury(rand(5,5),rand(5,2),rand(2,3),rand(3,5))
@test_throws DimensionMismatch Woodbury(rand(5,5),rand(5,2),rand(3,3),rand(2,5))

# spec builder
n = 10
num_nz = 3
for i in 1:5  # repeat 5 times
    args = [(rand(1:n), rand(1:n), rand()) for j in 1:num_nz]
    r, v, c = WoodburyMatrices.sparse_factors(Float64, n, args...)
    out = r*v*c

    by_hand = zeros(n, n)
    for (i, j, v) in args
        by_hand[i, j] = v
    end

    @test maximum(abs,out - by_hand) == 0.0
end

end  # @testset "Woodbury"


"""
Set of functions for generating signals.
"""

function gendata_RW(process_noise,
                    measurement_noise,
                    mean_state0,
                    precision_state0;
                    time_horizon=100)
    "Generate data according to a random walk"

    # Dimensionality
    d = size(process_noise, 1)

    # Preallocate
    y = zeros(time_horizon, d)
    x = zeros(time_horizon+1, d)

    # Initialize state
    x[1, :] = randn(d)./sqrt(precision_state0) .+ mean_state0

    for t = 1:time_horizon

        # Evolve state
        x[t+1, :] = randn(d).*sqrt(process_noise) + x[t, :]

        # Observe
        y[t, :] = randn(d).*sqrt(measurement_noise) + x[t+1, :]

    end
    return y, x
end

function gendata_LGDS(transition_matrix,
                      emission_matrix,
                      process_noise,
                      measurement_noise,
                      mean_state0,
                      precision_state0;
                      time_horizon=100)
    "Generate data according to a linear Gaussian dynamical system"

    # Dimensionality
    d = size(transition_matrix, 1)

    # Preallocate
    y = zeros(time_horizon, d)
    x = zeros(time_horizon+1, d)

    # Initialize state
    x[1, :] = randn(d)./sqrt(precision_state0) .+ mean_state0

    for t = 1:time_horizon

        # Evolve state
        x[t+1, :] = randn(d).*sqrt(process_noise) + transition_matrix*x[t, :]

        # Observe
        y[t, :] = randn(d).*sqrt(measurement_noise) + emission_matrix*x[t+1, :]

    end
    return y, x
end

function gendata_LGDSBO(transition_matrix,
                       emission_matrix,
                       process_noise,
                       measurement_noise,
                       mean_state0,
                       precision_state0;
                       time_horizon=100)
    "Generate data according to a linear Gaussian dynamical system with binary observations"

    # Dimensionality
    d = size(transition_matrix, 1)

    # Preallocate
    y = zeros(time_horizon, d)
    x = zeros(time_horizon+1, d)

    # Initialize state
    x[1, :] = randn(d)./sqrt(precision_state0) .+ mean_state0

    for t = 1:time_horizon

        # Evolve state
        x[t+1, :] = randn(d).*sqrt(process_noise) + transition_matrix*x[t, :]

        # Observe
        y[t, :] = round.(1 ./(1 .+ exp.(-x[t,:]))).*2 .-1

    end

    return y, x
end


using Test
using SumOfSquares
using DynamicPolynomials

# Like `term_test` but with a variable `y` fixed to 1.
function term_fixed_test(
    optimizer, config::MOIT.Config, cone::SumOfSquares.PolyJuMP.PolynomialSet)
    atol = config.atol
    rtol = config.rtol

    model = _model(optimizer)

    @variable(model, α)

    @polyvar x y
    set = @set y == 1
    cref = @constraint(model, (α - 1) * x^2 * y in cone, domain = set)

    @objective(model, Min, α)
    optimize!(model)

    @test termination_status(model) == MOI.OPTIMAL
    @test objective_value(model) ≈ 1.0 atol=atol rtol=rtol

    @test primal_status(model) == MOI.FEASIBLE_POINT
    @test value(α) ≈ 1.0 atol=atol rtol=rtol

    test_constraint_primal(cref, 0.0)

    p = gram_matrix(cref)
    @test getmat(p) ≈ zeros(1, 1) atol=atol rtol=rtol
    @test p.basis.monomials == [x]

    @test dual_status(model) == MOI.FEASIBLE_POINT
    for (m, μ) in [(x^2 * y, dual(cref)), (x^2, moments(cref))]
        @test μ isa AbstractMeasure{Float64}
        @test length(moments(μ)) == 1
        @test moment_value(moments(μ)[1]) ≈ 1.0 atol=atol rtol=rtol
        @test monomial(moments(μ)[1]) == m
    end

    ν = moment_matrix(cref)
    @test getmat(ν) ≈ ones(1, 1) atol=atol rtol=rtol
    @test ν.basis.monomials == [x]

    S = SumOfSquares.SOSPolynomialSet{
        typeof(set), Monomial{true}, MonomialVector{true}, SumOfSquares.Certificate.Remainder{SumOfSquares.Certificate.Newton{typeof(cone), MonomialBasis, Tuple{}}}
    }
    @test list_of_constraint_types(model) == [(Vector{JuMP.AffExpr}, S)]
    test_delete_bridge(
        model, cref, 1,
        ((MOI.VectorOfVariables, MOI.Nonnegatives, 0),
         (MOI.VectorAffineFunction{Float64},
          SumOfSquares.PolyJuMP.ZeroPolynomialSet{
              typeof(set), MonomialBasis,
              Monomial{true}, MonomialVector{true}},
          0)))
end
sos_term_fixed_test(optimizer, config)   = term_fixed_test(optimizer, config, SOSCone())
sd_tests["sos_term_fixed"] = sos_term_fixed_test
sdsos_term_fixed_test(optimizer, config) = term_fixed_test(optimizer, config, SDSOSCone())
soc_tests["sdsos_term_fixed"] = sdsos_term_fixed_test
dsos_term_fixed_test(optimizer, config)  = term_fixed_test(optimizer, config, DSOSCone())
linear_tests["dsos_term_fixed"] = dsos_term_fixed_test


using DiffEqEnvironments
using DifferentialEquations
using ReinforcementLearning
using Test

"""
This test defines an autonomous system and checks
whether the integration and terminal condition work
when stepping through the environment.
"""

# Define ODEProblem of scalar autonomous system
ode(s,a,t) = 1.01 * s
s0 = 1 / 2
tspan = (1f0, 2f0)
problem = ODEProblem(ode, s0, tspan)

# Solve ODE Problem for ground truth
sol = solve(problem, Tsit5())

# Define DiffEqEnv
r = ASReward((a, s) -> sum(abs2, s))
n_actions = 1
dt = 0.1
env = DiffEqEnv(problem, r, n_actions, dt; T=Float32, solver=Tsit5())

#= ==== Testing starts here ==== =#
# Test initialization
@test env.t == problem.tspan[1]
@test env.state[1] == s0

# Step through env until done
while !env.done
    env(0f0) # action doesn't matter for autonomous system
end

@test env.steps == (tspan[2] - tspan[1]) / dt
@test env.t ≈ problem.tspan[2]
@test env.state[1] ≈ sol.u[end]

#= ==== Reset and try again ==== =#
# Test initialization
RLBase.reset!(env)
@test env.t == problem.tspan[1]
@test env.state[1] == s0

# Step through env until done
while !env.done
    env(0f0) # action doesn't matter for autonomous system
end

@test env.steps == (tspan[2] - tspan[1]) / dt
@test env.t ≈ problem.tspan[2]
@test env.state[1] ≈ sol.u[end]

#= = Remake env using different types T = =#
env64 = DiffEqEnv(problem, r, n_actions, dt, T=Float64)
@test env64.state[1] == s0

env16 = DiffEqEnv(problem, r, n_actions, dt, T=Float16)
@test env16.state[1] == s0

#= = Run random policy using ReinforcementLearning.jl = =#
RLBase.reset!(env)
hook = TotalRewardPerEpisode()
run(
    Agent(
        ;policy=RandomPolicy(env),
        trajectory=VectCompactSARTSATrajectory(
            state_type=Any,
            action_type=Any,
            reward_type=Real,
            terminal_type=Bool,
        ),
    ),
    env,
    StopAfterEpisode(10),
    hook
)


# Create example data
x = 2 .* rand(100) .- 1
y = 2 .* rand(100) .- 1
z = 2 .* rand(100) .- 1
c = 999 .* rand(100) .+ 1
# Plot the points with colors
scatter3(x, y, z, c)


function SeisSmoothGathers(m::String,d::String,adj;Nsmooth=3,Nrepeat=1)

	if (adj==false)
		m1,h,ext = SeisRead(m)
		ngather = size(m1,3)
		for igather = 1 : ngather
			A = m1[:,:,igather];
			A = smooth_angles(A;Nsmooth=Nsmooth,Nrepeat=Nrepeat)
			m1[:,:,igather] = A
		end
		SeisWrite(d,m1,h,ext)
	else
		d1,h,ext = SeisRead(d)
		ngather = size(d1,3)
		for igather = 1 : ngather
			A = d1[:,:,igather];
			A = smooth_angles(A;Nsmooth=Nsmooth,Nrepeat=Nrepeat)
			d1[:,:,igather] = A
		end
		SeisWrite(m,d1,h,ext)
	end

end

function smooth_angles(A;Nsmooth=3,Nrepeat=1)

	for iz = 1 : size(A,1)
		a = A[iz,:]
		for irepeat = 1 : Nrepeat
			a = mean_filter(a,Nsmooth)
		end
		A[iz,:] = a
	end

	return A;
end

function mean_filter(a,nw)

	b = vec(zeros(length(a),1))
	for ix = 1 : length(a)
		sum  = 0.
		nsum = 0.
		for iw = 1 : nw
			index1 = ix - Int(floor(nw/2)) - 1 + iw
			if (index1>0 && index1<length(a))
				sum += a[index1]
				nsum += 1.
			end
		end
		b[ix] = sum/nsum
	end
	return b
end


"""
Calculate standard deviation used in transition kerenl, computes weighted standard
deviation on each set of particles individually
`wstd(pmcstep)`
`wstd(x, w)`
`wstd(A, w)`
"""
function weightedstd(x::Array{G, 1}, w::W) where {G <: Real, W <: StatsBase.AnalyticWeights}
    return std(x, w, corrected = true)
end

function weightedstd(x::Array{G, 2}, w::W) where {G <: Real, W <: StatsBase.AnalyticWeights}
    return vec(std(x, w, 1, corrected = true))
end

function weightedstd(abcpmc::A) where A <: AbcPmcStep
    return weightedstd(abcpmc.particles, abcpmc.weights)
end

"""
Function
to calculate a variance used in the transition kernel.  Calculates the
weighted variance of particles and then multiples by 2.  In d-dimensional
case returns a vector of length d.

# Arguments
* `particles::Array{G <: Real, 1}`: accepted particles from previous ABC step
* `weights::WeightVec`: weights associated with particles

# Value
transition kernel variance, 2 * var(particles).  Calculated variance within particle types.
"""

function weightedstd2(x::Array{G, 1}, w::W) where {G <: Real, W <: StatsBase.AnalyticWeights}
    return sqrt(2.0) * std(x, w, corrected = true)
end

function weightedstd2(x::Array{G, 2}, w::W) where {G <: Real, W <: StatsBase.AnalyticWeights}
    return sqrt(2.0) .* vec(std(x, w, 1, corrected = true))
end

function weightedstd2(abcpmc::A) where A <: AbcPmcStep
    return weightedstd2(abcpmc.particles, abcpmc.weights)
end


using PyPlot, PrintFig

x = linspace(0,2pi,100);
fig = plt.figure();
plot(x,sin(x),color="red");
title("Test plot");
xlabel(L"$x$");
ylabel(L"$\sin(x)$");
printfig(fig)	# Saves to file "FIG1.tex"
printfig(fig,filename="test.tex") # Saves to file "test.tex"


"""

    Implements all three variants of the anomaly detectors based on k-nearest neighbors from
    From outliers to prototypes: Ordering data,
    Stefan Harmelinga and Guido Dornhegea and David Tax and Frank Meinecke and Klaus-Robert Muller, 2005

"""
struct KNNAnomaly{V<:Val}
    t::NNTree
    X::Matrix
    v::V
end

"""
    function KNNAnomaly(X::Matrix, v::Symbol, tree_type::Symbol = :BruteTree)

    create the k-nn anomaly detector with variant v::Symbol

"""
KNNAnomaly(X::Matrix, v::Symbol, tree_type::Symbol = :BruteTree) = KNNAnomaly(eval(tree_type)(X), X, Val(v))

"""
    kappa is the radius of a ball containing all points

"""
function StatsBase.predict(model::KNNAnomaly, x, k, v::V) where {V<:Val{:kappa}}
    inds, dists = NearestNeighbors.knn(model.t, x, k,true)
    map(d -> d[end],dists)
end

"""
    gamma is the average distance to all k-nearest neighbors

"""
function StatsBase.predict(model::KNNAnomaly, x, k, v::V) where {V<:Val{:gamma}}
    inds, dists = NearestNeighbors.knn(model.t, x, k)
    map(d -> Statistics.mean(d),dists)
end

"""
    delta is the average distance to all k-nearest neighbors

"""
function StatsBase.predict(model::KNNAnomaly, x, k, v::V) where {V<:Val{:delta}}
    inds, dists = NearestNeighbors.knn(model.t, x, k)
    map(i -> LinearAlgebra.norm(x[:,i[1]] - Statistics.mean(model.X[:,i[2]],dims=2)) ,enumerate(inds))
end

StatsBase.predict(model::KNNAnomaly, x, k, v::Symbol) = StatsBase.predict(model, x, k, Val(v))
StatsBase.predict(model::KNNAnomaly, x, k) = StatsBase.predict(model, x, k, model.v)


using Test
using BoxSymmetries
using BoxSymmetries: Perm

@testset "1d" begin
    @inferred BoxSym(1)([1,2,3])
    @test BoxSym(-1)([1,2,3]) == [3,2,1]
    @test BoxSym( 1)([1,2,3]) == [1,2,3]

    @test inverse(BoxSym(1)) === BoxSym(1)
    @test inverse(BoxSym(-1)) === BoxSym(-1)
    @test BoxSym(1) ∘ BoxSym(-1) === BoxSym(-1)
    @test BoxSym(-1) ∘ BoxSym(1) === BoxSym(-1)
    @test BoxSym(1) ∘ BoxSym(1) === BoxSym(1)
    @test BoxSym(-1) ∘ BoxSym(-1) === BoxSym(1)
    @test unit(BoxSym{1}) === BoxSym(1)
end

@testset "2d" begin
    m = [1 2 3;
         4 5 6
    ]
    @test BoxSym( 1, 2)(m) == [1 2 3; 4 5 6]
    @test BoxSym( 1,-2)(m) == [3 2 1; 6 5 4]
    @test BoxSym(-1, 2)(m) == [4 5 6; 1 2 3]
    @test BoxSym(-1,-2)(m) == [6 5 4; 3 2 1]

    @test BoxSym( 2, 1)(m) == [1 4; 2 5; 3 6]
    @test BoxSym( 2,-1)(m) == [4 1; 5 2; 6 3]
    @test BoxSym(-2, 1)(m) == [3 6; 2 5; 1 4]
    @test BoxSym(-2,-1)(m) == [6 3; 5 2; 4 1]

    A = [:a11 :a12; :a21 :a22]
    @test (BoxSym{2}(:unit     ))(A) == [:a11 :a12; :a21 :a22]
    @test (BoxSym{2}(:rot90    ))(A) == [:a12 :a22; :a11 :a21]
    @test (BoxSym{2}(:rot180   ))(A) == [:a22 :a21; :a12 :a11]
    @test (BoxSym{2}(:rot270   ))(A) == [:a21 :a11; :a22 :a12]
    @test (BoxSym{2}(:flipx    ))(A) == [:a21 :a22; :a11 :a12]
    @test (BoxSym{2}(:flipy    ))(A) == [:a12 :a11; :a22 :a21]
    @test (BoxSym{2}(:flipdiag ))(A) == [:a11 :a21; :a12 :a22]
    @test (BoxSym{2}(:flipadiag))(A) == [:a22 :a12; :a21 :a11]
end

@testset "action associative" begin
    for dim in 1:3
        for _ in 1:10
            dims = Tuple(rand(1:5) for _ in 1:dim)
            x = randn(dims)
            g1 = rand(BoxSym{dim})
            g2 = rand(BoxSym{dim})
            @test (g1∘g2)(x) == g1(g2(x))
        end
    end
end

@testset "sugar" begin
    @test_throws ArgumentError BoxSym(2,)
    @test_throws ArgumentError BoxSym(0,)
    @test_throws ArgumentError BoxSym(-1,1)
end

@testset "instances" begin
    G = instances(BoxSym{1})
    @test allunique(G)
    @test length(G) == 2
    @test rand(G) isa BoxSym{1}
    @test rand(BoxSym{1}) isa BoxSym{1}

    G = instances(BoxSym{2})
    @test allunique(G)
    @test length(G) == 8
    @test rand(G) isa BoxSym{2}
    @test rand(BoxSym{2}) isa BoxSym{2}

    G = instances(BoxSym{3})
    @test allunique(G)
    @test length(G) == 48
    @test rand(G) isa BoxSym{3}
    @test rand(BoxSym{3}) isa BoxSym{3}
end

@testset "show it like you build it" begin
    for dim in 1:3
        for G in [BoxSym{dim}, Perm{dim}]
            for g in instances(G)
                s = sprint(show, g)
                g2 = eval(Meta.parse(s))
                @test g === g2
            end
        end
    end
end

function test_group_laws(G)
    @testset "unit" begin
        @test unit(G) isa G
        for g in instances(G)
            @test unit(G) ∘ g === g
            @test g ∘ unit(G) === g
        end
    end
    @testset "inverse" begin
        for g in instances(G)
            @test inverse(g) isa G
            @test (g∘inverse(g)) === unit(G)
            @test (inverse(g)∘g) === unit(G)
        end
    end
    @testset "associative" begin
        for g1 in instances(G)
            for g2 in instances(G)
                for g3 in instances(G)
                    @test g1∘(g2∘g3) === (g1∘g2)∘g3
                end
            end
        end
    end
end

@testset "group laws" begin
    test_group_laws(Perm{1})
    test_group_laws(Perm{2})
    test_group_laws(Perm{3})

    test_group_laws(BoxSym{1})
    test_group_laws(BoxSym{2})
    test_group_laws(BoxSym{3})
end


@testset "Fminbox" begin
    # Quadratic objective function
    # For (A*x-b)^2/2
    function quadratic!(g, x, AtA, Atb, tmp)
        mul!(tmp, AtA, x)
        v = dot(x,tmp)/2 + dot(Atb,x)
        if g !== nothing
            g .= tmp .+ Atb
        end
        return v
    end

    Random.seed!(1)
    N = 8
    boxl = 2.0
    outbox = false
    # Generate a problem where the bounds-free solution lies outside of the chosen box
    local _objective
    while !outbox
        A = randn(N,N)
        AtA = A'*A
        b = randn(N)
        initial_x = randn(N)
        tmp = similar(initial_x)
        funcg! = (g, x) -> quadratic!(g, x, AtA, A'*b, tmp)
        _objective = OnceDifferentiable(x->funcg!(nothing, x), (g, x)->funcg!(g, x), funcg!, initial_x)
        results = optimize(_objective, initial_x, ConjugateGradient())
        results = optimize(_objective, Optim.minimizer(results), ConjugateGradient())  # restart to ensure high-precision convergence
        @test Optim.converged(results)
        opt_x = Optim.minimizer(results)
        g = similar(opt_x)
        @test funcg!(g, opt_x) + dot(b,b)/2 < 1e-8
        @test norm(g) < 1e-4
        outbox = any(t -> abs(t) > boxl, opt_x)
    end

    # fminbox
    l = fill(-boxl, N)
    u = fill(boxl, N)
    initial_x = (rand(N) .- 0.5) .* boxl
    for _optimizer in (ConjugateGradient(), GradientDescent(), LBFGS(), BFGS(), NGMRES(), OACCEL())
        debug_printing && printstyled("Solver: ", summary(_optimizer), "\n", color=:green)
        results = optimize(_objective, l, u, initial_x, Fminbox(_optimizer))
        if _optimizer isa NGMRES && Sys.iswindows() && Sys.WORD_SIZE == 64
            @test_broken Optim.converged(results)
        else
            @test Optim.converged(results)
        end
        @test summary(results) == "Fminbox with $(summary(_optimizer))"
        opt_x = Optim.minimizer(results)
        NLSolversBase.gradient!(_objective, opt_x)
        g = NLSolversBase.gradient(_objective)
        # check first-order constrained optimality conditions
        for i = 1:N
            @test abs(g[i]) < 3e-3 || (opt_x[i] < -boxl+1e-3 && g[i] > 0) || (opt_x[i] > boxl-1e-3 && g[i] < 0)
        end
    end

    # Throw ArgumentError when initial guess is outside the box
    @test_throws ArgumentError optimize(_objective, l, u, 2u, Fminbox(GradientDescent()))

    # tests for #180
    results = optimize(_objective, l, u, initial_x, Fminbox(), Optim.Options(outer_iterations = 2))
    @test Optim.iterations(results) == 2
    @test Optim.minimum(results) == _objective.f(Optim.minimizer(results))

    # Warn when initial condition is not in the interior of the box
    initial_x = rand([-1,1],N)*boxl
    @test_logs (:warn, "Initial position cannot be on the boundary of the box. Moving elements to the interior.\nElement indices affected: [1, 2, 3, 4, 5, 6, 7, 8]") optimize(_objective, l, u, initial_x, Fminbox(), Optim.Options(outer_iterations = 1))

    # might fail if changes are made to Optim.jl
    # TODO: come up with a better test
    #results = Optim.optimize(_objective, initial_x, l, u, Fminbox(); optimizer_o = Optim.Options(iterations = 2))
    #@test Optim.iterations(results) == 470
    @testset "simple input" begin
        function exponential(x)
            return exp((2.0 - x[1])^2) + exp((3.0 - x[2])^2)
        end

        function exponential_gradient!(storage, x)
            storage[1] = -2.0 * (2.0 - x[1]) * exp((2.0 - x[1])^2)
            storage[2] = -2.0 * (3.0 - x[2]) * exp((3.0 - x[2])^2)
            storage
        end

        function exponential_gradient(x)
            storage = similar(x)
            storage[1] = -2.0 * (2.0 - x[1]) * exp((2.0 - x[1])^2)
            storage[2] = -2.0 * (3.0 - x[2]) * exp((3.0 - x[2])^2)
            storage
        end

        initial_x = [0.0, 0.0]
        optimize(exponential, exponential_gradient!, initial_x, BFGS())
        lb = fill(-0.1, 2)
        ub = fill(1.1, 2)
        od = OnceDifferentiable(exponential, initial_x)
        optimize(od, lb, ub, initial_x, Fminbox())
        od_forward = OnceDifferentiable(exponential, initial_x; autodiff = :forward)
        optimize(od_forward, lb, ub, initial_x, Fminbox())
        optimize(exponential, lb, ub, initial_x, Fminbox())
        optimize(exponential, exponential_gradient!, lb, ub, initial_x, Fminbox())
        optimize(od, lb, ub, initial_x)
        optimize(od_forward, lb, ub, initial_x)
        optimize(exponential, lb, ub, initial_x)
        optimize(exponential, exponential_gradient!, lb, ub, initial_x)
        @testset "inplace and autodiff keywords #616" begin
            optimize(exponential, lb, ub, initial_x, Fminbox())
            optimize(exponential, lb, ub, initial_x, Fminbox(); autodiff = :finite)
            optimize(exponential, lb, ub, initial_x, Fminbox(); autodiff = :forward)
            optimize(exponential, exponential_gradient, lb, ub, initial_x, Fminbox(), inplace = false)
        end
        @testset "error for second order methods #616" begin
            @test_throws ArgumentError optimize(x->x, (G,x)->x, rand(1),rand(1),rand(1), Fminbox(Newton()))
            @test_throws ArgumentError optimize(x->x, (G,x)->x, rand(1),rand(1),rand(1), Fminbox(NewtonTrustRegion()))
        end
        @testset "allow for an Optim.Options to be passed #623" begin
            optimize(exponential, lb, ub, initial_x, Fminbox(), Optim.Options())
            optimize(exponential, exponential_gradient!, lb, ub, initial_x, Fminbox(), Optim.Options())
            @test_broken optimize(exponential, exponential_gradient, lb, ub, initial_x, Optim.Options())
        end
    end
end


const R = R_GAS
const F = FARADAY

struct SimpleLinearModel{T<:Real}
    standardpotential::T
    slope::T
    rsquared::T
    SimpleLinearModel{T}(a,b,c) where {T<:Real} = new(a,b,c)
end

SimpleLinearModel(a::Float64, b::Float64, c::Float64) = SimpleLinearModel{Float64}(a,b,c)
#SimpleLinearModel(a::Real, b::Real)  = SimpleLinearModel{Real}(promote(a, b)...)
function (s::SimpleLinearModel)(x)
    return s.slope .* x .+ s.standardpotential
end

struct ModelResult{S, T}
    themodel::S
    theplot::T
end

function gen_model(n, T=723.0)
  coeff = R*T*log(10.0)/(n*F)
    model(x, E0) = E0[1] .+ coeff .* x
    return model
end

function get_rsquared(model, xdata, ydata)
    ȳ = mean(ydata)
    sstot = sum((ydata .- ȳ).^2)
    ssres = sum((ydata .- model(xdata)).^2)
    R² = 1 - ssres/sstot
    return R²
end

function fitamodel(tofit, xdata, ydata)
    p0 = [1.0]
    fit = curve_fit(tofit, xdata, ydata, p0)
    e0 = fit.param[1]
    slope = tofit(1.0, e0)  - tofit(0.0, e0)
    Rsq = get_rsquared(x->tofit(x,e0), xdata, ydata)
    return SimpleLinearModel(e0, slope, Rsq)
end

function infinitedilution_extrapolate(xdata, vdata, n, T)
    logxs = log10.(xdata)
    fitmodel = gen_model(n, T)
    initialmodel = fitamodel(fitmodel, logxs, vdata)
    Rsq = initialmodel.rsquared
    finalmodel = initialmodel
    modeltwopoints = fitamodel(fitmodel, logxs[1:2], vdata[1:2])
    oldRsq = modeltwopoints.rsquared
    # this is to initialize finalmodel with the right type; we're going
    # to figure out what the REAL final model is in a bit
    for i in 3:length(xdata)
        if Rsq > 0.99
            break # assume if Rsq is already high, model is already good
        elseif i == 3
            if oldRsq > Rsq finalmodel = modeltwopoints end
        end
        xn = logxs[1:i]
        vn = vdata[1:i]
        newmodel = fitamodel(fitmodel, xn, vn)
        Rsq = get_rsquared(newmodel, xn, vn)
        if Rsq < oldRsq
            if Rsq - oldRsq > 0.02 || Rsq < 0.95 || Rsq /oldRsq < 0.95
                break
            end
        end
        oldRsq = Rsq
        finalmodel = newmodel
    end
    return finalmodel
end

function plot_model(model, xdata, vdata, mixname="Mixture", addname="Additive")
    logxs = log10.(xdata)
    myxrange = minimum(logxs) .- 0.1:0.01:0.0
    mynewxs = collect(myxrange)
    mynewys = broadcast(model, mynewxs)
    scene = scatter(logxs, vdata, label="Experimental", legend=:topleft)
    plot!(scene, mynewxs, mynewys, label="Extrapolated")
    title!(scene, "Standard Potential Extrapolation")
    xlabel!(scene, "Log10 of Concetration of $addname in $mixname")
    ylabel!(scene, "Measured Potential (V)")
    return ModelResult(model, scene)
end

function compare_actcoeffs(extrap_model, purecomp_enot, xdata, edata,mixname="Mixture", addname="Additive")
    logxs = log10.(xdata)
    rightslope = extrap_model(1.0) - extrap_model(0.0)
    puremodel(x) = x .* rightslope .+ purecomp_enot
    unrefined_pure = edata .- puremodel(logxs)
    unrefined_infd = edata .- extrap_model(logxs)
    newcoeff = 1.0/rightslope
    pureγ = 10 .^ (unrefined_pure .* newcoeff)
    infdγ = 10 .^ (unrefined_infd .* newcoeff)
    print("Pure Component Activities for $addname in $mixname: ", pureγ)
    print("Infinite Dilution Activities for $addname in $mixname: ", infdγ)
    scene = scatter(log10.(xdata), log10.(infdγ), label="Extrapolated", legend=:topleft, size=(400,400))
    scatter!(scene, log10.(xdata), log10.(pureγ), label="Pure Component")
    #title!(scene, latexstring("\\mathrm{Activity \\: Coefficient \\: Comparison \\: for \\: $addname \top\: in \\: $mixname}"))
    xlabel!(scene, latexstring("\\mathrm{Log}_{10} \\mathrm{ \\: of \\: concentration \\: (mole \\: fraction)}"))
    ylabel!(scene, latexstring("\\mathrm{Log}_{10} \\mathrm{ \\: of \\: activity \\: coefficient}"))
    return scene
end


# This file contains the implementation of the Reference Region Model
# and Constrained Reference Region Model, which are to be used with non-linear
# least-squares fitting

@everywhere function RRM(t::Vector{Float64}, p::Vector{Float64}, Crr::Vector{Float64})
  # Reference Region Model
  # Tofts modle using a reference tissue curve (Crr) instead of AIF (Cp)
	# Ct = kR * Crr + kR * (kepRR-kep) * conv(Crr,exp(-kep*t))
	# Reference: Yankeelov et al. (2005), MRI, 23(4), 519–29. doi:10.1016/j.mri.2005.02.013
  kR = p[1] # relative kTrans
  kepRR = p[2] # kep of reference tissue
  kep = p[3] # kep of TOI
  Ct = zeros(t)
  for k in 1:length(Ct)
    tk = t[k]
    @simd for j = 1:k
      @inbounds y = exp(kep*(t[j] - tk)) * Crr[j]
      @inbounds Ct[k] += ifelse((j == 1 || j == k) && k > 1, 0.5*y, y)
    end
  end
  dtp = (t[2] - t[1])
  @simd for k = 1:length(Ct)
    @inbounds Ct[k] = kR * (Crr[k] + dtp*(kepRR-kep)*Ct[k])
  end
  Ct
end

@everywhere function CRRM(tmpT::Vector{Float64}, p::Vector{Float64}, Crr::Vector{Float64})
  # Constrained Reference Region Model
  # Tofts modle using a reference tissue curve (Crr) instead of AIF (Cp)
	# Ct = kR * Crr + kR * (kepRR-kep) * conv(Crr,exp(-kep*t)) --- but kepRR is held fixed
	# Reference: Ahmed & Levesque. (2016), MRM, http://doi.org/10.1002/mrm.26530
  kR = p[1] # relative kTrans
  kep = p[2] # kep of TOI
	# This part is ugly. We needed to pass an extra fixed parameter (i.e. kepRR)
	# but I didn't want to modify the nlsfit() function...
	# ... So, I snuck kepRR as the last element of time.
  kepRR = tmpT[end] # use last element of time as kepRR (because that's how we smuggled it into this function)
  t=tmpT[1:end-1] # delete that last element from time because it doesn't represent an actual time point
  # The rest of the function is similar to RRM()
  Ct = zeros(t)
  for k in 1:length(Ct)
    tk = t[k]
    @simd for j = 1:k
      @inbounds y = exp(kep*(t[j] - tk)) * Crr[j]
      @inbounds Ct[k] += ifelse((j == 1 || j == k) && k > 1, 0.5*y, y)
    end
  end
  dtp = (t[2] - t[1])
  @simd for k = 1:length(Ct)
    @inbounds Ct[k] = kR * (Crr[k] + dtp*(kepRR-kep)*Ct[k])
  end
  Ct
end


using jInv.Mesh;
using Helmholtz
using Multigrid
using LinearAlgebra
using SparseArrays

const plotting = false;

if plotting
	using PyPlot;
	close("all");
end

# m = readdlm("SEGmodel2Dsalt.dat"); m = Matrix(m'); m = m*1e-3;
m = 1.5*ones(257,129);

Minv = getRegularMesh([0.0,13.5,0.0,4.2],collect(size(m)).-1);

pad = 16;

m = 1.0./m.^2

f = 2.5;

w = 2*pi*f

println("omega*h:");
println(w*Minv.h*sqrt(maximum(m)));
pad = pad*ones(Int64,Minv.dim);

maxOmega = getMaximalFrequency(m,Minv);
ABLamp = maxOmega;

Sommerfeld = true;
NeumannAtFirstDim = true;
H,gamma = GetHelmholtzOperator(Minv,m,w,w*ones(size(m))*0.01,NeumannAtFirstDim,pad,ABLamp,Sommerfeld);
shift = [0.2;0.2;0.2];
SH = H .+ GetHelmholtzShiftOP(m, real(w),shift[1]);

# n = Minv.n .+1 ; n_tup = tuple(n...);
# src = div.(n,2);
# src[end] = 1;
# q = zeros(ComplexF64,n_tup)
# q[loc2cs(n,src)] = 1.0/(Minv.h[1]^2);

levels      = 3;
numCores 	= 2;
maxIter     = 30;
relativeTol = 1e-6;
# relaxType   = "Jac-GMRES";
relaxType   = "Jac";
relaxParam  = 0.75;
relaxPre 	= 2;
relaxPost   = 2;
cycleType   ='W';
coarseSolveType = "NoMUMPS";

MG = getMGparam(ComplexF64,Int64,levels,numCores,maxIter,relativeTol,relaxType,relaxParam,
				relaxPre,relaxPost,cycleType,coarseSolveType,0.5,0.0)

## Preparing a point shource RHS ############################
n = Minv.n .+ 1; n_tup = tuple(n...);
src = div.(n,2);
src[end] = 1;
q = zeros(ComplexF64,n_tup)
q[loc2cs(n,src)] = 1.0/(Minv.h[1]^2);
###############################################
b = q[:];

Hparam = HelmholtzParam(Minv,gamma,vec(m),w,NeumannAtFirstDim,Sommerfeld);
Ainv = getShiftedLaplacianMultigridSolver(Hparam, MG,shift,"GMRES",5,true);
Ainv = copySolver(Ainv);

x = solveLinearSystem(sparse(SH'),b,Ainv)[1];
MG.relaxType = "Jac";
MG.cycleType = 'W';
Ainv = getShiftedLaplacianMultigridSolver(Hparam, MG,shift,"BiCGSTAB",0,true);
x = solveLinearSystem(sparse(SH'),b,Ainv)[1];

if plotting
	reX = real(reshape(x,n_tup));
	reX[My_sub2ind(n,src)] = 0.0;
	println(norm(q[:]-H*x)/norm(q[:]));
	println(norm(q[:]-H*y)/norm(q[:]));
	figure();
	imshow(reX');title("Helmholtz Iterative Solution");
end

if plotting
	s = H\q[:];
	s = real(reshape(s,n_tup));
	s[My_sub2ind(n,src)] = 0.0;
	figure();
	imshow(s');title("Helmholtz True Solution");
end

# println("Doing Transpose")
# y = solveLinearSystem(sparse(SH'),b,Ainv,1)[1];

# println(norm(H'*y-b)/norm(b));

################## WITHOUT THE COARSEST GRID SOL ########################
# coarseSolveType = "DDNodal";
# MG = getMGparam(ComplexF64,Int64,levels,numCores,maxIter,relativeTol,relaxType,relaxParam,
				# relaxPre,relaxPost,cycleType,coarseSolveType,0.5,0.0)
# Ainv = getShiftedLaplacianMultigridSolver(Hparam, MG,shift,"BiCGSTAB",0,true);

# y = solveLinearSystem(SH',b,Ainv)[1];
###########################################################################

println("SOLVING MULTIPLE RHSs")
coarseSolveType = "Julia";
MG = getMGparam(ComplexF64,Int64,levels,numCores,maxIter,relativeTol,relaxType,relaxParam,
				relaxPre,relaxPost,cycleType,coarseSolveType,0.5,0.0)
Ainv = getShiftedLaplacianMultigridSolver(Hparam, MG,shift,"BiCGSTAB",0,true);
nrhs = 2;
b = rand(ComplexF64,length(b),nrhs);
clear!(Ainv);
Ainv.helmParam = Hparam;
x = solveLinearSystem(sparse(SH'),b,Ainv)[1];
println(norm(H*x .- b)/norm(b))

MG = getMGparam(ComplexF64,Int64,levels,numCores,maxIter,relativeTol,relaxType,relaxParam,relaxPre,relaxPost,cycleType,coarseSolveType);
MG.relaxType = "Jac-GMRES";
MG.cycleType = 'K';
Ainv = getShiftedLaplacianMultigridSolver(Hparam, MG,shift,"GMRES",5,true);
x = solveLinearSystem(sparse(SH'),b,Ainv)[1];
println(norm(H*x .- b)/norm(b))


using Base: @nloops, @ntuple
using Einsum
using LinearAlgebra
using TensorOperations

function computemarginal(d::DiscreteSpinGlass)
    # compute the marginal probiliby for each variable to take +1
    n = length(d)
    exp = quote
        Z = 0.0
        mP = zeros($n) # marginal probability
        @nloops $n i (d)->[-1.0,1.0] begin
            x = @ntuple $n i
            x = collect(x)
            local p = exp(Distributions.logpdf($d,x))
            term = p.*x
            term[term.<0] .= 0.0
            global mP .+= term
            global Z += p
        end
        return mP./Z, Z
    end
    eval(exp)
end

function get_neighbours(d::Union{DiscreteSpinGlass, ThirdOrderExpFamily})
    """
    Get list of neighbor ids for both factor nodes and variable nodes
    """
    fidx = d.edge_index
    vidx = Array{Array{Int64,1},1}([[] for _ in 1:d.nnode])
    for i in 1:d.nfactor
        for j in fidx[i]
            push!(vidx[j], i)
        end
    end
    return fidx, vidx
end

function get_factor_tensor(d::DiscreteSpinGlass)
    """
    get factor tensor on support [-1,1]^r where r is the rank of the tensor
    """
    res = []
    for i in 1:d.nfactor
        idx = d.edge_index[i]
        supp = [1.; -1.]
        alpha = d.edge_attr[i][1] # assume scalar
        if length(idx) == 1
            @tensor begin
                T[i] := supp[i]
            end
        elseif length(idx) == 2
            @tensor begin
                T[i,j] := supp[i] * supp[j]
            end
        elseif length(idx) == 3
            @tensor begin
                T[i,j,k] := supp[i] * supp[j] * supp[k]
            end
        end
        T = @. exp(-alpha * T)
        T ./= maximum(T)
        push!(res,T)
    end
    return res
end

function bp_3spin(fidx::AbstractVector, vidx::AbstractVector, ftensor::AbstractVector; niter::Integer=1, atol=1e-5)
    nf = length(fidx)
    nv = length(vidx)
    ## Initialzie messages
    mv2f = Dict{Tuple,Array}()
    mf2v = Dict{Tuple,Array}()
    for i in 1:nf
        for j in fidx[i]
            mf2v[(i,j)] = ones(2) / 2.  # 2 state spin
            mv2f[(j,i)] = ones(2) / 2.
        end
    end

    ## main loop
    isconverged=false
    for iiter in 1:niter
        mv2f_new = deepcopy(mv2f)
        mf2v_new = deepcopy(mf2v)
        # update variable-to-factor messages
        for j in 1:nv
            source_idx = combinations(1:length(vidx[j]),length(vidx[j])-1) |> collect |> reverse
            for (neighbor_id, i) in enumerate(vidx[j])
                if length(source_idx) > 1
                    mv2f_new[(j,i)] = foldl((x,y)->x.*y,[mf2v[(vidx[j][k],j)] for k in source_idx[neighbor_id]])
                end
            end
        end
        # update factor-to-variable messages
        for i in 1:nf
            factor_size = length(fidx[i])
            source_idx = combinations(1:factor_size,factor_size-1) |> collect |> reverse
            for (neighbor_id, j) in enumerate(fidx[i])
                if factor_size == 1
                    mf2v_new[(i,j)] = ftensor[i]
                else
                    perm = collect(1:factor_size )
                    perm[neighbor_id] = 1
                    perm[1] = neighbor_id
                    ft_perm = permutedims(ftensor[i],perm)
                    if factor_size == 2
                        T = ft_perm
                        k1 = source_idx[neighbor_id][1]
                        m1 = mv2f[(fidx[i][k1],i)]
                        @einsum tmp[a] := T[a,b] * m1[b]
                        mf2v_new[(i,j)] .= tmp
                    elseif factor_size == 3
                        T = ft_perm
                        k1 = source_idx[neighbor_id][1]
                        k2 = source_idx[neighbor_id][2]
                        m1 = mv2f[(fidx[i][k1],i)]
                        m2 = mv2f[(fidx[i][k2],i)]
                        @einsum tmp[a] := T[a,b,c] * m1[b] * m2[c]
                        mf2v_new[(i,j)] .= tmp
                    end
                    @assert all(tmp .>= 0) "$tmp\n$(T) \n$(m1)\n$(m2)"
                end
            end
        end
        # normalize message
        for k in keys(mv2f)
            mv2f_new[k] = mv2f_new[k] ./ sum(mv2f_new[k])
        end
        for k in keys(mf2v)
            mf2v_new[k] = mf2v_new[k] ./ sum(mf2v_new[k])
        end
        # check convergence
        # assert convergent if all message change < atol
        isconverged = true
        for k in keys(mv2f)
            if abs(mv2f[k][1]-mv2f_new[k][1]) > atol
                isconverged = false
                break
            end
        end
        for k in keys(mf2v)
            if abs(mf2v[k][1]-mf2v_new[k][1]) > atol
                isconverged = false
                break
            end
        end
        mv2f = mv2f_new
        mf2v = mf2v_new
    end

    ## Calculate marginal distributions
    # singleton marginals
    marginals = []
    for j in 1:nv
        incoming_messages = [mf2v[(i,j)] for i in vidx[j]]
        if length(incoming_messages)>1
            mP = foldl((x,y)->x.*y,incoming_messages)
        else
            mP = ones(2) / .2
        end
        mP ./= sum(mP)
        push!(marginals,mP[1])
    end

    return marginals, isconverged, mf2v, mv2f
end

function bp(d::DiscreteSpinGlass; niter::Integer=10, atol::Real=1e-5)
    if !has_singleton_factor(d)
        d = add_singleton_factor(d)
    end
    fidx, vidx = get_neighbours(d)
    ftensor = get_factor_tensor(d)
    mP_bp, isconverged, mf2v, mv2f = bp_3spin(fidx,vidx,ftensor, niter=niter, atol=atol)
    return mP_bp, isconverged
end


using VarianceCovarianceEstimators
using Test

srand(0)
using StatsBase

PID = repeat(1:10, inner = 10)
TID = repeat(1:10, outer = 10)
X = hcat(ones(100), rand(100, 2))
y = (X * ones(3,1) + repeat(rand(10), inner = 10) + repeat(rand(10), outer = 10) + rand(100))[:]
β = X \ y
ŷ = X * β
û = Vector(y - ŷ)

mutable struct MyModel <: StatsBase.RegressionModel
end

model = MyModel()
StatsBase.modelmatrix(obj::MyModel) = X
StatsBase.residuals(obj::MyModel) = û
StatsBase.dof_residual(obj::MyModel) = reduce(-, size(StatsBase.modelmatrix(obj)))
StatsBase.deviance(obj::MyModel) = StatsBase.residuals(obj).'StatsBase.residuals(obj) / StatsBase.dof_residual(obj)
StatsBase.nobs(obj::MyModel) = length(StatsBase.residuals(obj))

makegroups(obj::AbstractVector) =
	find.(map(val -> obj .== val, unique(obj)))

groups = makegroups.([PID, TID])

V = vcov(model, :OLS)
@test mapreduce(idx -> V[idx:end, idx], vcat, 1:size(V, 1)) ≈
    [0.016812950819883037,
    -0.013785344698385674,
    -0.015506005910982551,
    0.023981369378055544,
    0.0032463406452211837,
    0.02804994376185539]


# Example adapted from Luxor.jl documentation
# https://juliagraphics.github.io/Luxor.jl/stable/polygons/#Offsetting-polygons

using Alexya

@init "Basic example" 800 600 # create a window and canvas
@layout aside(:v, 100)

t = 0

# @create macro creates and adds the widget to the window
velocity = @create Slider(1/10:1/100:1, @margin(10))

# update function is called every frame
@use function update()
    w, h = @width, @height
    background("#e1e1e1")
    origin()

    spine = [Point(20x, 15sin(x + t)) for x in -4π:pi/24:4pi]

    f(t, b, c, d) = 2sin(t * π)

    pg = offsetpoly(spine, startoffset=1, endoffset=10, easingfunction=f)
    sethue("black")
    poly(pg, :fill)

    sethue("#e1e1e1")
    poly(spine, :stroke)

    global t += value(velocity)

    if t >= 6π
        global t = 0
    end
end

start()


module UStatsTest

using Test
using Random, LinearAlgebra, Statistics
using DiffPrecTest: compute, variance,
                    createVarianceStatistic, createCovarianceStatistic, createSecondMomentStatistic

@testset "one sample" begin
  x = randn(10)
  @test compute(createVarianceStatistic(), x) ≈ var(x)

  s = createSecondMomentStatistic()
  @test compute(s, x) ≈ sum(abs2, x) / 10
  @test variance(s, x) ≈ var( x .* x)

  X = randn(10, 2)
  @test compute(createCovarianceStatistic(1, 2), X) ≈ cov(X[:, 1], X[:, 2])

  s = createSecondMomentStatistic(1, 2)
  @test compute(s, X) ≈ mean(X[:, 1] .* X[:, 2])
  @test variance(s, X) ≈ var(X[:, 1] .* X[:, 2])
end

end


struct Basis{N,T<:AbstractMatrix}
    dims::NTuple{N}
    B::T
end

function construct_basis(bf, xs::Tuple, params)
    dims = length.(xs)
    nr = prod(dims)
    nc = length(params)
    B = zeros(nr,nc)
    w = trues(nc)
    for (i,p) in enumerate(params)
        Bc = vec(bf(xs,p))
        if sum(Bc) != 0.0
            B[:,i] .= vec(bf(xs,p))
            w[i] = true
        end
    end
    return Basis(dims,B[:,w])
end

function construct_basis(bf, xs, params)
    construct_basis(bf,(xs,),params)
end

Base.:(*)(B::Basis,A::AbstractMatrix) = B.B*A
Base.:(*)(A::AbstractMatrix,B::Basis) = A*B.B

function evaluate(B::Basis, coeffs)
    if length(coeffs) != size(B.B,2)
        @error "Wrong Number of Coefficients for basis"
    end
    reshape(max.(B.B*coeffs,0.0),B.dims)
end


using VortexDistributions, FourierGPE, Plots, LaTeXStrings, SpecialFunctions

## Initialize simulation
# units of healing length, chemical potential
L = (100.,100.)
N = (512,512)
sim = Sim(L,N)
@unpack_Sim sim

## homogeneous state
μ = 1.0
g = 0.01
ψ0(x,y,μ,g) = sqrt(μ/g)*sqrt(max(1.0-V(x,y,0.0)/μ,0.0)+im*0.0)
healinglength(x,y,μ,g) = 1/sqrt(g*abs2(ψ0(x,y,μ,g)))
x,y = X
kx,ky = K
ψg = ψ0.(x,y',μ,g)
showpsi(x,y,ψg)

## initial dipole, with periodic phase
ψd = copy(ψg)
d = 12
ξv = healinglength(0.,0.,μ,g)
# can use default ξ = 1
xp,yp = 0.,d/2
xn,yn = 0.,-d/2
pv = PointVortex(xp,yp,1)
nv = PointVortex(xn,yn,-1)
dipole = [nv;pv]

psi = Torus(ψd,x,y) # set field topology for VortexDistributions
vortex!(psi,dipole) # make dipole

## make another dipole
# xp,yp = 6.,d/2
# xn,yn = 6.,-d/2
# pv = PointVortex(xp,yp,1)
# nv = PointVortex(xn,yn,-1)
# dipole = [nv;pv]
# vortex!(psi,dipole) # make dipole

showpsi(x,y,ψd)

## test new methods
kd = 2*pi/d
kL = 2*pi/L[1]
kξ = 2*pi
kmin = 0.1*kL
kmax = kξ

Np = 300
k = log10range(kmin,kmax,Np)

# @time Ek = kespectrum(k,ψd,X,K)
# plot(k,Ek,scale=:log10)

# ## incompressible spectrum
# @time Eki = ikespectrum(k,ψd,X,K)
# plot!(k,Eki,scale=:log10)

## power-law plot in logspace
@time Eki = ikespectrum(k,ψd,X,K)
Eqp = qpespectrum(k,ψd,X,K)
Ekc = ckespectrum(k,ψd,X,K)

n0 = abs2.(ψd[1,1])
E0 = π*n0
p1 = plot(k,Eki/E0,scale=:log10,grid=false,label=L"\epsilon_I(k)",legend=:bottomleft)
plot!(k,Eqp/E0,label=L"\epsilon_Q(k)")
plot!(k,2.5k.^(-3),label=L"k^{-3}")
plot!(k,200*k,label=L"k")
ylims!(5e-4,1e2)
vline!([kL],ls=:dash,label=L"k_L")
vline!([kd],ls=:dash,label=L"k_d")
vline!([kξ],ls=:dash,label=L"k_\xi")
xlabel!(L"k\xi")
ylabel!(L"\epsilon_I(k)")

# Analytic form (pure dipole)
# vort = findvortices(Torus(ψd,x,y)) |> vortex_array
# d = vort[:,2] |> diff
# Λ = 0.8249
# f(x) = x*(besselk(1,x)*besseli(0,x)-besselk(0,x)*besseli(1,x))
# F(x,Λ) = f(x/(2Λ))^2/x
# F(x) = F(x,Λ)
# Ed(k,d) = 2*F(k)*(1-besselj0(k*d))
# plot!(k,Ed.(k,d),label=L"\epsilon_{a}^v(k)")

## two point correlator (approx)
ddk = diff(k); push!(ddk,last(ddk))
gtwo(r)=sum(@. k^2*Eki*besselj0(k*r)*ddk)
Gtwo(r)=gtwo(r)/gtwo(0)

## plot Gtwo
r = LinRange(0,10,100)
plot(r,Gtwo.(r),label=L"G_2(r)")
vline!([d],ls=:dash,label=L"d")

## FULL SPECTRAL DENSITIES

## ke spectrum
kd = 2*pi/d
kL = 2*pi/L[1]
kξ = 2*pi
kmin = 0.1*kL
kmax = kξ

Np = 300
k = log10range(kmin,kmax,Np)

## quantum spectra
# all kinetic
Ek = kespectrum(k,ψd,X,K)
plot(k,Ek,scale=:log10,label="total",legend=:bottom)
# plot!(k,k.^3,label=L"k^3")

## try Eh^v spectrum (incompressible)
using SpecialFunctions

function qikespectrum(k,ψ,X,K)
    x,y = X; kx,ky = K
 	dx,dy = diff(x)[1],diff(y)[1]
	DX,DK = dfftall(X,K)
    k2field = k2(K)
    psi = XField(ψ,X,K,k2field)
    vx,vy = velocity(psi)

    ux = @. abs(ψ)*vx; uy = @. abs(ψ)*vy
    Wi, Wc = helmholtz(ux,uy,psi)
    wix,wiy = Wi
    @. wix *= exp(im*angle(ψ)) # restore phase factors
    @. wiy *= exp(im*angle(ψ))
	cix = autocorrelate(wix,X,K)
	ciy = autocorrelate(wiy,X,K)
    C = cix .+ ciy

    return bessel_projection(k,x,y,C)
end

Eki = qikespectrum(k,ψd,X,K)
plot!(k,Eki,label="incompressible")
plot!(k,1000k.^(-3))

## sound
function skespectrum(k,ψ,X,K)
    x,y = X; kx,ky = K
 	dx,dy = diff(x)[1],diff(y)[1]
	DX,DK = dfftall(X,K)
    k2field = k2(K)
    psi = XField(ψ,X,K,k2field)
    vx,vy = velocity(psi)

    ux = @. abs(ψ)*vx; uy = @. abs(ψ)*vy
    Wi, Wc = helmholtz(ux,uy,psi)
    wcx,wcy = Wc
    @. wcx *= exp(im*angle(ψ)) # restore phase factors
    @. wcy *= exp(im*angle(ψ))
	ccx = autocorrelate(wcx,X,K)
	ccy = autocorrelate(wcy,X,K)
    C = ccx .+ ccy

    return bessel_projection(k,x,y,C)
end

Ekc = ckespectrum(k,ψd,X,K)
plot!(k,Ekc,label="compressible")

## quantum pressure
function qkespectrum(k,ψ,X,K)
    x,y = X; kx,ky = K
 	dx,dy = diff(x)[1],diff(y)[1]
	DX,DK = dfftall(X,K)
    k2field = k2(K)
    psi = XField(abs.(ψ) |> complex,X,K,k2field)
    rnx,rny = gradient(psi)
    @. rnx *= exp(im*angle(ψ)) # restore phase factors
    @. rny *= exp(im*angle(ψ)) # restore phase factors
	cx = autocorrelate(rnx,X,K)
	cy = autocorrelate(rny,X,K)
    C = cx .+ cy

    return bessel_projection(k,x,y,C)
end

Ekq = qkespectrum(k,ψd,X,K)
plot!(k,Ekq,label="quantum pressure")
ylims!(0.1,1e5)
vline!([kL],ls=:dash,label=L"k_L")
vline!([kd],ls=:dash,label=L"k_d")
vline!([kξ],ls=:dash,label=L"k_\xi")


using Primes

function solve(A, M)
    L = 50 # ≥ log₂ M
    if M == 1
        return L
    end

    φₘ = totient(M)
    x = solve(A, φₘ)
    l = lcm(φₘ, M)
    res = powermod(A, x, l)

    while res < L
        res += l
    end

    return res
end

function main()
    Q = parse(Int, readline())

    for _ in 1:Q
        A, M = parse.(Int, split(readline()))
        println(solve(A, M))
    end
end

main()


function ∂  end
function ∂ₛ end

  ##########################################################################################

function (sim::Simulation)(::typeof(∂),dir::Direction,gridtype::GridType)
    D(dir,sim.grid,sim.e⁻ⁱᵏᴸ,gridtype)
end

function (sim::Simulation)(::typeof(∂ₛ),dir::Direction,gridtype::GridType)
       S =  get_S_comp(sim,  dir)
       spdiagm((1 ./ S)[:])  *  sim(∂,dir,gridtype)
end

  ##########################################################################################

  function D(dir::Direction{NDir},g::Grid{NDim},e⁻ⁱᵏᴸ::NTuple,gridtype::GridType) where {NDim,NDir}
      if NDir > NDim return spzeros(ncells(g),ncells(g)) end
      dw = spacing(g,dir);
      nw = size(g,dir);
      ∂w = D(nw,e⁻ⁱᵏᴸ[NDir],gridtype) / dw;
      return kron(∂w, dir, g)
    end

 ################################################################################
 function  D(N::Int, e⁻ⁱᵏᴸ::Number,gridtype::GridType{:Primal})
    T = promote_type(Float, eltype(e⁻ⁱᵏᴸ))
    Df =  spdiagm(0=>  -ones(T,N),1=> ones(T,N-1))
    Df[N,1] =  1.0 * e⁻ⁱᵏᴸ
    return  Df
 end

 function  D(N::Int, e⁻ⁱᵏᴸ::Number,gridtype::GridType{:Dual})
    T = promote_type(Float, eltype(e⁻ⁱᵏᴸ))
    Db =  spdiagm(0=> ones(T,N),  -1=> -ones(T,N-1))
    Db[1,N] = -1.0 * (e⁻ⁱᵏᴸ)^-1
    return  Db
 end
 #############################################################################


function no_whitening(
    dataset::DataSet{T, I})::WhiteningResult{T} where {T<:AbstractFloat, I<:Integer}

    datamean = zeros(T, dataset.P)

    transform_data!(dataset, Array{T, 2}(LinearAlgebra.I, dataset.P, dataset.P), datamean)
end

function cholesky_whitening!(
    dataset::DataSet{T, I})::WhiteningResult{T} where {T<:AbstractFloat, I<:Integer}

    datamean = zeros(T, dataset.P)

    for p in eachindex(datamean)
        datamean[p] = mean(view(dataset.data, p, :))
    end

    for n in 1:dataset.N
        buffer = view(dataset.data, :, n) - datamean
        setindex!(dataset.data, buffer, :, n)
    end

    #cov of a Transpose object yields the wrong (0.0) result in 1D, if all weights are equal (e.g. direct sampling)!
    covmatrix = cov(convert(typeof(dataset.data), transpose(dataset.data)), FrequencyWeights(dataset.weights), corrected=true)
    symcovmatrix = Symmetric(covmatrix)
    covmatrix_inv = inv(symcovmatrix)
    w = cholesky(covmatrix_inv).U
    wres = convert(Matrix{T}, w)

    transform_data!(dataset, wres, datamean)
end

function cholesky_partial_whitening!(
    dataset::DataSet{T, I})::WhiteningResult{T} where {T<:AbstractFloat, I<:Integer}

    datamean = zeros(T, dataset.P)
	avoid_whitening_dims = find_steep_edges(dataset)

    for p in eachindex(datamean)
        datamean[p] = mean(view(dataset.data, p, :))
    end

    for n in 1:dataset.N
        buffer = view(dataset.data, :, n) - datamean
        setindex!(dataset.data, buffer, :, n)
    end

    covmatrix = cov(convert(typeof(dataset.data), transpose(dataset.data)), FrequencyWeights(dataset.weights), corrected=true)

	for param in avoid_whitening_dims # replace non-diagonal elements with zeros to preserve rectangular bounds.
		covmatrix[1:param-1, param] .= 0.0
		covmatrix[param+1:end, param] .= 0.0
		covmatrix[param, param+1:end] .= 0.0
		covmatrix[param, 1:param-1] .= 0.0
	end

    symcovmatrix = Symmetric(covmatrix)
    covmatrix_inv = inv(symcovmatrix)
    w = cholesky(covmatrix_inv).U
    wres = convert(Matrix{T}, w)

    transform_data!(dataset, wres, datamean)
end

function find_steep_edges(dataset::DataSet{T, I})::Array{Int64,1} where {T<:AbstractFloat, I<:Integer}

    N_bins::Int64 = 100
    treshold::Float64 = 0.00
    avoid_whitening_dims::Array{Int64,1} = []

    for param in 1:dataset.P
        hist = fit(Histogram, dataset.data[param, :], FrequencyWeights(dataset.weights), nbins = N_bins, closed = :left)
        treshold = 0.01 * maximum(hist.weights)
		if ((hist.weights[1] < treshold) && (hist.weights[end] < treshold)) == false
            push!(avoid_whitening_dims, param)
        end
    end

    return avoid_whitening_dims
end

function statistical_whitening!(
    dataset::DataSet{T, I})::WhiteningResult{T} where {T<:AbstractFloat, I<:Integer}

    datamean = zeros(T, dataset.P)

    for p in eachindex(datamean)
        datamean[p] = mean(view(dataset.data, p, :))
    end

    dataset.data .-= datamean

    covmatrix = cov(transpose(dataset.data), FrequencyWeights(dataset.weights), corrected=true)

    E = eigfact(covmatrix).vectors
    w_d = transpose(E)
    D = Diagonal(inv(E) * covmatrix * E)
    w = inv(full(sqrt.(D))) * w_d
    wres = convert(Matrix{T}, w)

    transform_data!(dataset, wres, datamean)
end

function transform_data!(
    dataset::DataSet{T, I},
    W::Matrix{T},
    datamean::Vector{T},
    substractmean::Bool = false)::WhiteningResult{T} where {T<:AbstractFloat, I<:Integer}

    local determinant::T

    if substractmean
        dataset.data .-= datamean
    end

    if diag(W) == ones(T, dataset.P)
        determinant = 1.0
    else
        dataset.data = W * dataset.data

        determinant = abs(det(W))
    end

    maxP::T = maximum(dataset.logprob)
    suggTargetProb::T = 500 #exp(maxP - partialsort(dataset.logprob, floor(Int64, dataset.N * 0.2)))

    dataset.iswhitened = true

    @debug "Determinant:\t" * string(determinant)
    @debug "Suggested Target Probability Factor:\t" * string(suggTargetProb)

    WhiteningResult(determinant, suggTargetProb, W, datamean)
end


using GLMakie

width = 3
height = 3

dir = [225 180 135 270 0 90 315 360 45]

Wy = -sind.(mod.(-dir .+ 90, 360))
Wx = -cosd.(mod.(-dir .+ 90, 360))

data1 = Array{Float32}(undef, round(Int, width), round(Int, height))

let k = 1
    for j::Int in 1:height
        for i::Int in 1:width
            data1[i, j] = k
            k = k + 1
        end
    end
end
fig, ax, hm = heatmap(data1)
Colorbar(fig[:, end+1], hm)

xs = 1:width
ys = 1:height
arrows!(fig[:, 1], xs, ys, Wx, Wy, arrowsize = 30, lengthscale = 0.2, linewidth = 3)
display(fig)


"""
    compute_laplace_eig(mesh, matrices, pde, eiglim, neig_max)

Compute the Laplace eigenvalues, eigenfunctions and first order moments of products of pairs of eigenfunctions.
"""
function compute_laplace_eig(model, matrices, eiglim = Inf, neig_max = Inf)

    # Measure function evaluation time
    starttime = Base.time()

    # Extract parameters
    @unpack mesh, D, T₂ = model
    @unpack M, S, R, Mx, Q = matrices
    ncompartment = length(mesh.points)

    # Compute at most all eigenvalues in the given domain
    neig = Int(min(neig_max, size(M, 1)))

    println("Solving Laplace eigenvalue problem, computing $neig eigenvalues.")
    println("Problem size: $(size(M, 1)) points.")

    # Solve generalized eigenproblem, computing the smallest eigenvalues only.
    # If 2 * neig_max_domain >= nnode, a full decomposition is performed,
    # calling the eig function inside eigs
    λ, ϕ = eigs(S + Q, M, nev = neig, which = :SR)
    # λ, ϕ = eigs(Hermitian(S + Q), Hermitian(M), nev = neig, which = :SR);
    # λ, ϕ = eigen(Hermitian(Matrix(S + Q)), Hermitian(Matrix(M)))

    # All Laplace eigenvalues are nonnegative
    all(0 .≤ λ) ||
        @warn "Obtained negative eigenvalues for Laplace operator." findall(λ .< 0) λ[λ.<0]

    # Only keep modes with length scales larger than minimum
    inds = λ .≤ eiglim
    λ = λ[inds]
    ϕ = ϕ[:, inds]
    isinf(eiglim) ||
        length(λ) < neig ||
        @warn "No eigenvalues were outside the interval. Consider increasing `neig_max`." eiglim neig_max

    # Normalize eigenfunctions with mass weighting
    ϕ ./= .√sum(ϕ .* (M * ϕ), dims = 1)

    # Compute first order moments of product of pairs of eigenfunctions
    moments = cat([ϕ' * Mx[dim] * ϕ for dim = 1:3]..., dims = 3)

    # Compute Laplace relaxation matrix
    massrelax = ϕ' * R * ϕ

    values = λ
    funcs = ϕ
    totaltime = Base.time() - starttime

    (; values, funcs, moments, massrelax, totaltime)
end


function update_weights(data, e, closest_same, closest_other, weights, weights_mult, m, k, max_f_vals, min_f_vals)

	# Distance weights for penalty term

	# Compute weights for each nearest hit
	d_vals_closest_same = 1 ./ (sum(abs.(e .- closest_same)./((max_f_vals .- min_f_vals) .+ eps(Float64)), dims=2) .+ eps(Float64))
	dist_weights_penalty = d_vals_closest_same ./ sum(d_vals_closest_same)

	# Distance weights for reward term
	d_vals_closest_other = 1 ./ (sum(abs.(e .- closest_other)./((max_f_vals .- min_f_vals) .+ eps(Float64)), dims=2) .+ eps(Float64))
	dist_weights_reward = d_vals_closest_other ./ sum(d_vals_closest_other)

	# Penalty term
	penalty = sum(dist_weights_penalty .* (abs.(e .- closest_same)./((max_f_vals .- min_f_vals) .+ eps(Float64))), dims=1)

	# Reward term
	reward = sum(weights_mult .* (dist_weights_reward .* (abs.(e .- closest_other)./((max_f_vals .- min_f_vals) .+ eps(Float64)))), dims=1)

	# Weights update
	weights = weights .- penalty./m .+ reward./m

	# Return updated weights.
	return vec(weights)
end


Random.seed!(5678)

p = 5
n = 8
k = 3

for T in (Float64, Float32)
    Wg = max.(rand(T, p, k) .- T(0.3), zero(T))
    Hg = max.(rand(T, k, n) .- T(0.3), zero(T))
    X = Wg * Hg

    for alg in (:multmse, :multdiv, :projals, :alspgrad, :cd, :greedycd)
        for init in (:random, :nndsvd, :nndsvda, :nndsvdar, :spa)
            ret = NMF.nnmf(X, k, alg=alg, init=init)
        end
    end

    # replicates test
    rep = NMF.nnmf(X, k, replicates=10, maxiter=10, alg=:multmse)
    ret = NMF.nnmf(X, k, W0=rep.W, H0=rep.H, init=:custom)

    # spa test
    ret = NMF.nnmf(X, k, alg=:spa, init=:spa)

    # update_H test
    W = max.(rand(T, p, k) .- T(0.3), zero(T))
    H = max.(rand(T, k, n) .- T(0.3), zero(T))
    for alg in (:multmse, :multdiv, :projals, :alspgrad, :cd, :greedycd)
        ret = NMF.nnmf(X, k, alg=alg, init=:custom, W0=copy(W), H0=copy(H), update_H=false)
        @test all(H .== ret.H)
        @test any(W .!= ret.W)
    end
end


# Algorithm 6 from [Joldes, Muller, Popescu 2017]
# relative error < 3u², 20 FP Ops
# rel accuracy [102.4, 104.4] bits
# "The largest relative error we have obtain through many tests is around 2.25 × 2^−2p = 2.25 u^2.
#  For Float64, this is 2.25*2^(-106) [rel accuracy 104.8 bits]." pg 11
function Base.:(+)(x::Double64, y::Double64)
    hi, lo   = two_sum(Hi(x), Hi(y))
    thi, tlo = two_sum(Lo(x), Lo(y))
    c = lo + thi
    hi, lo = two_hilo_sum(hi, c)
    c = tlo + lo
    hi, lo = two_hilo_sum(hi, c)
    return Double64((hi, lo))
end

biterror(::typeof(+)) = 2.25

function Base.:(+)(x::Double64, y::Float64)
    hi, lo = two_sum(Hi(x), y)
    c = lo + Lo(x)
    hi, lo = two_hilo_sum(hi, c)
    return Double64((hi, lo))
end

function Base.:(+)(x::Float64, y::Double64)
    hi, lo = two_sum(x, Hi(y))
    c = lo + Lo(y)
    hi, lo = two_hilo_sum(hi, c)
    return Double64((hi, lo))
end

# relative error < 3u², 20 FP Ops, 102.4 bits (relative)
# Algorithm 6 from [Joldes, Muller, Popescu 2017]
# reworked for subtraction
function Base.:(-)(x::Double64, y::Double64)
    hi, lo   = two_diff(Hi(x), Hi(y))
    thi, tlo = two_diff(Lo(x), Lo(y))
    c = lo + thi
    hi, lo = two_hilo_sum(hi, c)
    c = tlo + lo
    hi, lo = two_hilo_sum(hi, c)
    return Double64((hi, lo))
end

biterror(::typeof(-)) = 2.25

function Base.:(-)(x::Double64, y::Float64)
    hi, lo = two_diff(Hi(x), y)
    c = lo + Lo(x)
    hi, lo = two_hilo_sum(hi, c)
    return Double64((hi, lo))
end

function Base.:(-)(x::Float64, y::Double64)
    hi, lo = two_diff(x, Hi(y))
    c = lo + Lo(y)
    hi, lo = two_hilo_sum(hi, c)
    return Double64((hi, lo))
end

# relative error < 5u², 9 FP Ops, 101.6 bits (relative)
# "The largest relative error we have obtain through many tests is 3.936 × 2^−2p = 3.936 * 2^(-106).
# Algorithm 12 from [Joldes, Muller, Popescu 2017]
function Base.:(*)(x::Double64, y::Double64)
    hi, lo = two_prod(Hi(x), Hi(y))
    t = Lo(x) * Lo(y)
    t = fma(Hi(x), Lo(y), t)
    t = fma(Lo(x), Hi(y), t)
    t = lo + t
    hi, lo = two_hilo_sum(hi, t)
    return Double64((hi, lo))
end

biterror(::typeof(*)) = 3.936

function Base.:(*)(x::Double64, y::Float64)
    hi, lo = two_prod(Hi(x), y)
    t = Lo(x) * y
    t = t + lo
    hi, lo = two_hilo_sum(hi, t)
    return Double64((hi, lo))
end

function Base.:(*)(x::Float64, y::Double64)
    hi, lo = two_prod(x, Hi(y))
    t = x * Lo(y)
    t = lo + t
    hi, lo = two_hilo_sum(hi, t)
    return Double64((hi, lo))
end

# from DoubleFloats
# Base.:(/)(x::Double64, y::Double64) = Double64(divide_dddd_dd(Hi(x), Lo(x), Hi(y), Lo(y)))
(div0)(x::Double64, y::Double64) = Double64(divide_dddd_dd(Hi(x), Lo(x), Hi(y), Lo(y)))

@inline function divide_dddd_dd(xhi::T, xlo::T, yhi::T, ylo::T) where {T<:Float64}
    hi = xhi / yhi
    uh, ul = two_prod(hi, yhi)
    lo = ((((xhi - uh) - ul) + xlo) - hi*ylo)/yhi
    hi,lo = two_hilo_sum(hi, lo)
    return hi, lo
end

# Algorithm 17 from [Joldes, Muller, Popescu 2017]
# relative error <= 15u² + 56u³
function div17(x::Double64, y::Double64)
    xhi, xlo = HiLo(x)
    yhi, ylo = HiLo(y)
    thi = xhi / yhi
    rhi, rlo = DWTimesFP3(yhi, ylo, thi)
    dhi = xhi - rhi
    dlo = xlo - rlo
    d = dhi + dlo
    tlo = d / yhi
    return Double64(two_hilo_sum(thi, tlo))
end

function (div18)(x::Double64, y::Double64)
    yhi, ylo = HiLo(y)
    th = inv(yhi)
    rh = fma(-yhi, th, 1.0)
    rl = -ylo * th
    eh, el = two_hilo_sum(rh, rl)
    dh, dl = DWTimesFP3(eh, el, th) # (eh, el) * rh
    mh, ml = DWPlusFP(dh, dl, th) # (dh, dl) + th
    zh, zl = DWTimesDW2(Hi(x), Lo(x), mh, ml) # (xh, xl) * (mh, ml)
    return Double64((zh, zl))
end

# relative error < 9.8u², 31 FP Ops, 100.7 bits (relative)
# " 5.922 x 2^(-106) "
# Algorithm 18 from [Joldes, Muller, Popescu 2017]
function Base.:(/)(x::Double64, y::Double64)
    yhi, ylo = HiLo(y)
    th = inv(yhi)
    rh = fma(-yhi, th, 1.0)
    rl = -ylo * th
    eh, el = two_hilo_sum(rh, rl)
    dh, dl = DWTimesFP3(eh, el, th) # (eh, el) * rh
    mh, ml = DWPlusFP(dh, dl, th) # (dh, dl) + th
    zh, zl = DWTimesDW2(Hi(x), Lo(x), mh, ml) # (xh, xl) * (mh, ml)
    return Double64((zh, zl))
end

    biterror(::typeof(/)) = 5.922
biterror(::typeof(inv)) = 5.922

function Base.:(/)(x::Double64, y::Float64)
    th = inv(y)
    rh = fma(-y, th, 1.0)
    dh, dl = DWTimesFP3(rh, 0.0, th) # (eh, el) * rh
    mh, ml = DWPlusFP(dh, dl, th) # (dh, dl) + th
    zh, zl = DWTimesDW2(Hi(x), Lo(x), mh, ml) # (xh, xl) * (mh, ml)
    return Double64((zh, zl))
end

function Base.:(/)(x::Float64, y::Double64)
    yhi, ylo = HiLo(y)
    th = inv(yhi)
    rh = fma(-yhi, th, 1.0)
    rl = -ylo * th
    eh, el = two_hilo_sum(rh, rl)
    dh, dl = DWTimesFP3(eh, el, th) # (eh, el) * rh
    mh, ml = DWPlusFP(dh, dl, th) # (dh, dl) + th
    zh, zl = DWTimesDW2(x, 0.0, mh, ml) # (xh, xl) * (mh, ml)
    return Double64((zh, zl))
end

# algorithm 4 from [Joldes, Muller, Popescu 2017]
@inline function DWPlusFP(xh::Float64, xl::Float64, y::Float64)
    sh, sl = two_sum(xh, y)
    v = xl + sl
    zh, zl = two_hilo_sum(sh, v)
    return zh, zl
end
# algorithm 7 from [Joldes, Muller, Popescu 2017]
# calculation of (xh, xl) * y
@inline function DWTimesFP1(xh, xl, y)
    ch, cl1 = two_prod(xh, y)
    cl2 = xl*y
    th, tl1 = two_hilo_sum(ch, cl2)
    tl2 = tl1 + cl1
    zh, zl = two_hilo_sum(th, tl2)
    return zh, zl
end
# algorithm 9 from [Joldes, Muller, Popescu 2017]
# calculation of (xh, xl) * y
@inline function DWTimesFP3(xh, xl, y)
    ch, cl1 = two_prod(xh, y)
    cl3 = fma(xl, y, cl1)
    zh, zl = two_hilo_sum(ch, cl3)
    return zh, zl
end

# algorithm 10 from [Joldes, Muller, Popescu 2017]
# calcualtes (xh, xl) * (yh, yl)
# the largest relative error found is  4.9916 × 2^(-106)
@inline function DWTimesDW1(xh, xl, yh, yl)
    ch, cl1 = two_prod(xh, yh)
    tl1 = xh * yl
    tl2 = xl * yh
    cl2 = tl1 + tl2
    cl3 = cl1 + cl2
    zh, zl = two_hilo_sum(ch, cl3)
    return zh, zl
end

# algorithm 12 from [Joldes, Muller, Popescu 2017]
# calcualtes (xh, xl) * (yh, yl)
# the largest relative error found is  3.936 × 2^(-106)
@inline function DWTimesDW2(xh, xl, yh, yl)
    ch, cl1 = two_prod(xh, yh)
    tl0 = xl * yl
    tl1 = fma(xh, yl, tl0)
    cl2 = fma(xl, yh, tl1)
    cl3 = cl1 + cl2
    zh, zl = two_hilo_sum(ch, cl3)
    return zh, zl
end

# relative error < 9.8u², 31 FP Ops, 100.7 bits (relative)
# Algorithm 18 from [Joldes, Muller, Popescu 2017]
function Base.:(inv)(y::Double64)
    yhi, ylo = HiLo(y)
    th = inv(yhi)
    rh = fma(-yhi, th, 1.0)
    rl = -ylo * th
    eh, el = two_hilo_sum(rh, rl)
    dh, dl = DWTimesFP3(eh, el, th) # (eh, el) * rh
    mh, ml = DWPlusFP(dh, dl, th) # (dh, dl) + th
    # zh, zl = DWTimesDW2(1.0, 0.0, mh, ml) # (1, 0) * (mh, ml)
    return Double64((mh, ml))
end

Base.:(\)(x::Double64, y::Double64) = (/)(y, x)
Base.:(\)(x::Double64, y::Float64) = (/)(y, x)
Base.:(\)(x::Float64, y::Double64) = (/)(y, x)

for (T1, T2) in ((:ComplexD64, :ComplexD64), (:ComplexD64, :ComplexF64), (:ComplexF64, :ComplexD64))
  @eval begin

    function Base.:(+)(x::$T1, y::$T2)
        re = real(x) + real(y)
        im = imag(x) + imag(y)
        hi = ComplexF64(Hi(re), Hi(im))
        lo = ComplexF64(Lo(re), Lo(im))
        return ComplexD64((hi, lo))
    end

    function Base.:(-)(x::$T1, y::$T2)
        re = real(x) - real(y)
        im = imag(x) - imag(y)
        hi = ComplexF64(Hi(re), Hi(im))
        lo = ComplexF64(Lo(re), Lo(im))
        return ComplexD64((hi, lo))
    end

    function Base.:(*)(x::$T1, y::$T2)
        xr = real(x); xi = imag(x); yr = real(y); yi = imag(y)
        re = xr * yr - xi * yi
        im = xr * yi + xi * yr
        hi = ComplexF64(Hi(re), Hi(im))
        lo = ComplexF64(Lo(re), Lo(im))
        return ComplexD64((hi, lo))
    end
  end
end

#=
   This is adapted from
   "Improved Complex Division"
    by Michael Baudin (DIGITEO) and Robert Smith (Stanford University)
    Version 0.1. February 2011

    "A Robust Complex Division in Scilab
    by Michael Baudin, Robert L. Smith
    2012
=#

function Base.:(/)(x::ComplexD64, y::ComplexD64)
    a = real(x); b = imag(x); c = real(y); d = imag(y)
    if abs(d) <= abs(c)
        r = d/c
        t = inv(fma(d, r, c))
        if r == 0
            d = d * t
            e = fma( d, b/c, a) # (a + d * (b/c)) * t
            f = fma(-d, a/c, b) # (b - d * (a/c)) * t
        else
            b = b * t
            a = a * t
            e = fma( b, r, a) # (a + b * r) * t
            f = fma(-a, r, b) # (b - a * r) * t
        end
    else
        r = c/d
        t = inv(fma(c, r, d)) # 1/(c * r + d )
        if r == 0
            e = fma(c, a/d,  b) * t # (c * (a/d) + b) * t
            f = fma(c, b/d, -a) * t # (c * (b/d) - a) * t
        else
            e = fma(a, r,  b) * t # (a * r + b) * t
            f = fma(b, r, -a) * t #(b * r - a) * t
        end
    end
    hi = ComplexF64(Hi(e), Hi(f))
    lo = ComplexF64(Lo(e), Lo(f))
    return ComplexD64((hi, lo))
end

function divide(x::ComplexD64, y::ComplexD64)
    a = real(x); b = imag(x); c = real(y); d = imag(y)
    if abs(d) <= abs(c)
        r = d/c
        t = inv(muladd(d, r, c))
        if r == 0
            d = d * t
            e = muladd( d, b/c, a) # (a + d * (b/c)) * t
            f = muladd(-d, a/c, b) # (b - d * (a/c)) * t
        else
            b = b * t
            a = a * t
            e = muladd( b, r, a) # (a + b * r) * t
            f = muladd(-a, r, b) # (b - a * r) * t
        end
    else
        r = c/d
        t = inv(muladd(c, r, d)) # 1/(c * r + d )
        if r == 0
            e = muladd(c, a/d,  b) * t # (c * (a/d) + b) * t
            f = muladd(c, b/d, -a) * t # (c * (b/d) - a) * t
        else
            e = muladd(a, r,  b) * t # (a * r + b) * t
            f = muladd(b, r, -a) * t #(b * r - a) * t
        end
    end
    hi = ComplexF64(Hi(e), Hi(f))
    lo = ComplexF64(Lo(e), Lo(f))
    return ComplexD64((hi, lo))
end

#=
adapted from
On complex multiplication and division with an FMA
Jean-Michel Muller, C.-P. Jeannerod, P. Kornerup and N. Louvet
INVA 2014

function mul(ab::ComplexD64, cd::ComplexD64)
    re = Kahan(real(ab), real(cd), -imag(ab), imag(cd))
    im = Kahan(real(ab), imag(cd), imag(ab), real(cd))
    return ComplexD64((ComplexF64(Hi(re), Hi(im)), ComplexF64(Lo(re), Lo(im))))
end

function divide(ab::ComplexD64, cd::ComplexD64)
    a = real(ab); b = imag(ab); c = real(cd); d = imag(cd)
    delta = c*c + d*d
    gre = Kahan(a, b,c,d) # ac + bd
    gim = Kahan(b,-a,c,d) # bc - ad
    qre = gre / delta
    qim = gim / delta
    return qre, qim
end

function Kahan(a, b, c, d)
    w = c * d
    e = fma(c, d, -w)
    f = fma(a, b, w)
    r = f + e
    return r
end

Base.fma(x::Double64,y::Double64,z::Double64) = muladd(x,y,z)
=#
#=

function normalizedenom(n,d)
    fr,xp = frexp(d)
    n = ldexp(n, -xp)
    return n, fr
end

function normalized(n::Double64, d::Double64)
    scaleby = inv(Hi(d))
    return n*scaleby, d*scaleby
end

function Goldschmidt(n::T,d::T,k) where {T}
    n, d = normalized(n,d)
    e = 1 - d
    q = n;  qold = zero(T)
    while !iszero(k) && (qold != q)
        k -= 1
        qold = q
        q = q * (1+e)
        e = e * e
    end
    return q
end

function Goldschmidt(n::Double64,d::Double64,k=4)}
    n1, d1 = normalized(n,d)
    e = 1 - d1
    q = n1;  qold = zero(T)
    while (qold != q) && !iszero(k)
       k -= 1
       qold = q
       q = q * (1+e) # q = q + q*e
       e = e * e
    end
    delta = (n1 - q*d1)/2
    return q + delta
end

=#


using GraphGLRM, LowRankModels
using Base.Test

# write your own tests here
include("realvalueglrm.jl")
include("classificationglrm.jl")
include("multidimglrm.jl")
include("sparseglrm.jl")

#Graph regularizer
A1 = randn(6,2) * diagm([128, 16])
A2 = 2*A1 + randn(6,2)
A = [A1 A2]
A /= vecnorm(A)/√(8)
ig = IndexGraph(1:4, [(1,3),(2,4)])
GRG = GGLRM(A, QuadLoss(), QuadReg(0.02), GraphQuadReg(ig, 2., 0.01), 2)
GRGM = GGLRM(A, QuadLoss(), QuadReg(0.02), GraphQuadReg(ig, 2., 0.01), 2, X=GRG.X, Y=GRG.Y)
NGRG = GGLRM(A, QuadLoss(), NonNegConstraint(), NonNegGraphReg(ig, 2.), 2)
fit!(GRG)
fit_multithread!(GRGM)
fit!(NGRG)
GRGobj = whole_objective(GRG, GRG.X'GRG.Y)
GRGMobj = whole_objective(GRGM, GRGM.X'GRGM.Y)
@test ≈(GRGobj, GRGMobj, atol=0.1*length(A))


function mandel( c )
    z = c
    itrMax = 80
    for n in 1:itrMax
        if abs(z) > 2
            return n-1
        end
        z = z^2 + c
    end
    return itrMax
end


# ROUTINE FOR PROJECTING ONTO FIRST-STAGE FEASIBLE REGION

# project on to the first-stage feasible region
function projectOntoFeasibleSet(p0_val::Array{Float64},rp_val::Array{Float64},rm_val::Array{Float64},alpha_val::Array{Float64})

	proj_mod = Model(solver=GurobiSolver(gurobi_env,Presolve=0,OutputFlag=0))

	# first-stage variables
	@variable(proj_mod, pmin[i] <= p0[i=1:numGen] <= pmax_FS[i])
	@variable(proj_mod, 0 <= rp[i=1:numGen] <= pmax_FS[i] - pmin[i])
	@variable(proj_mod, 0 <= rm[i=1:numGen] <= pmax_FS[i] - pmin[i])
	@variable(proj_mod, 0 <= alpha[i=1:numGen] <= 1)

	# projection objective function
	@objective(proj_mod, Min, sum( (p0[i] - p0_val[i])^2 + (rp[i] - rp_val[i])^2 + (rm[i] - rm_val[i])^2 + (alpha[i] - alpha_val[i])^2 for i = 1:numGen))

	# first-stage constraints
	@constraint(proj_mod, sum(p0[i] for i=1:numGen) == sum(meanDemand[j] for j=1:numLoads))
	@constraint(proj_mod, [i=1:numRegGen], p0[i] + rp[i] <= pmax[i])
	@constraint(proj_mod, [i=1:numRegGen], p0[i] - rm[i] >= pmin[i])
	@constraint(proj_mod, [i=1:numGen; genGuarRes[i]], alpha[i] >= min_alpha)
	@constraint(proj_mod, [i=1:numRegGen; !genRes[i]], alpha[i] == 0)
	@constraint(proj_mod, sum(alpha[i] for i=1:numGen) == 1)

	if(caseID == "no_wind_reserves")
		@constraint(proj_mod, [i=1:numWindGen], alpha[i+numRegGen] == min_alpha/10.0)
	end

	solve_status = solve(proj_mod)

	p0_proj = getvalue(p0)
	rp_proj = getvalue(rp)
	rm_proj = getvalue(rm)
	alpha_proj = getvalue(alpha)

	# Terminate if the projection model does not solve even to suboptimality
	if(solve_status != :Optimal && solve_status != :Suboptimal)
		println("p0_val: ",p0_val,"  p0_proj: ",p0_proj)
		println("rp_val: ",rp_val,"  rp_proj: ",rp_proj)
		println("rm_val: ",rm_val,"  rm_proj: ",rm_proj)
		println("alpha_val: ",alpha_val,"  alpha_proj: ",alpha_proj)
		println("solve_status: ",solve_status)
		throw(ErrorException("Projection problem wasn't solved to (sub)optimality!!!"))
	end

	return p0_proj, rp_proj, rm_proj, alpha_proj, solve_status
end


# Load input
masses = open("day01_input.txt") do f
    [parse(Int32, line) for line in eachline(f)]
end

# Part 1
fuel_requirement = sum([Int32(floor(mass / 3)) - 2 for mass in masses])
println("Part 1: Fuel requirement: $fuel_requirement")

# Part 2
function compute_mass_recursive(fuel_mass::Int32)::Int32
    extra_fuel_mass = Int32(floor(fuel_mass / 3)) - 2
    if extra_fuel_mass <= 0
        return fuel_mass
    else
        return fuel_mass + compute_mass_recursive(extra_fuel_mass)
    end
end

fuel_requirement = sum([compute_mass_recursive(mass) - mass for mass in masses])
println("Part 2: Fuel requirement: $fuel_requirement")


@testset "Simplicial Complex" begin
    cplx = SimplicialComplex(Simplex(1,2,3), Simplex(2,4), Simplex(4))

    @test celltype(cplx) == Simplex{Int}
    @test length(cells(cplx)) == 3
    cs = cells(cplx,2)
    @test cs !== nothing
    @test length(cs) == 1
    cs = cells(cplx,3)
    @test cs === nothing
    @test size(cplx) == (4,4,1)
    @test size(cplx,0) == 4
    @test size(cplx,1) == 4
    @test size(cplx,2) == 1
    @test size(cplx,3) == 0
    @test cplx[Simplex(4), 0] == 4
    @test cplx[Simplex(10), 0] > size(cplx,0)
    @test cplx[Simplex(10), 0] == 5

    @test length(simplices(cplx)) == 9
    scitr = simplices(cplx, 1)
    @test length(scitr) == 4
    for s in simplices(cplx, 2)
        @test s == Simplex(1,2,3)
    end

    cplx = SimplicialComplex(Simplex(1,2,3),
                       Simplex(2,4),
                       Simplex(3,4),
                       Simplex(5,4),
                       Simplex(6))

    qch = Chain(2, [1], [1])
    resch = Chain(1, [1, -1, 1], [3, 2, 1])
    @testset "Complex boundary chain" for (a,b) in zip(boundary(cplx, qch), resch)
        @test a == b
    end

    qch = Chain(1, [1], [1])
    resch = Chain(2, [-1, -1], [1, 2])
    @testset "Complex coboundary chain" for (a,b) in zip(coboundary(cplx, qch), resch)
        @test a == b
    end

    @test convert(Matrix, boundary_matrix(cplx, 2)) == [1 -1 1 0 0 0]'

    cplx = SimplicialComplex(Int)

    splx = push!(cplx, Simplex(1))
    @test splx[1][:index] == 1
    @test dim(splx[1]) == 0
    @test sum(size(cplx)) == 1
    @test cplx.cells[0][1][:values] == [1]

    splx = push!(cplx, Simplex(1,2))
    @test splx[1][:index] == 1
    @test dim(splx[1]) == 1
    @test sum(size(cplx)) == 2
    @test cplx.cells[1][1][:values] == [1,2]

    cplx = SimplicialComplex(Int)
    splxs = push!(cplx, Simplex(1,2), recursive=true)
    @test splxs[1][:index] == 1
    @test splxs[1][:values] == [1,2]
    @test splxs[2][:index] == 1
    @test splxs[2][:values] == [1]
    @test splxs[3][:index] == 2
    @test splxs[3][:values] == [2]
    @test sum(size(cplx)) == 3
    @test size(cplx, 0) == 2
    @test size(cplx, 1) == 1

    splxs = push!(cplx, Simplex(1,3), recursive=true)
    @test splxs[1][:index] == 2
    @test splxs[1][:values] == [1,3]
    @test splxs[2][:index] == 3
    @test splxs[2][:values] == [3]
    @test sum(size(cplx)) == 5
    @test size(cplx, 0) == 3
    @test size(cplx, 1) == 2

    io = IOBuffer()
    write(io, cplx)
    seekstart(io)
    cplx2 = read(io, SimplicialComplex{Int})
    @test size(cplx2, 0) == 3
    @test size(cplx2, 1) == 2

    ch = boundary(cplx2, 1, 0, Int)
    @test dim(ch) == -1
end


using Gadfly, Distributions

plot(y=rand(Normal(), 100), Geom.boxplot)


#
# Routines for modified quadrature weights originally written by Johan Helsing
#

using FastGaussQuadrature
using Vandermonde

@inline function add_logcorr_block!(B, ztrg, zsrc, nsrc, wsrc, scale, trans, alpha)
    numpoints = length(wsrc)
    glpoints, glweights = gausslegendre(numpoints)
    if trans==0
        glpoints_scaled = glpoints
    else
        glpoints_scaled = @. trans + scale*glpoints
    end
    #WfrakL = WfrakLcomp(trans, scale, glpoints)
    WfrakL = WfrakLcomp(glpoints_scaled, glpoints)
    # Assemble block (including diagonal)
    KL = Array{Float64}(2,2)
    tdist = Array{Float64}(numpoints)
    for i=1:numpoints
        # tdist = Distance in parametrization of src panel
        @. tdist = abs(glpoints_scaled[i] - glpoints[:])
        # Very empirical limit for when neighbors need special quad due to log kernel:
        # 0.2 for 32, 0.4 for 16
        # Too gives nearly sing error
        # Too large makes special quad inaccurate, though not very sensitive
        if (trans == 0) || (minimum(tdist) < 0.2*32/numpoints)
            for j=1:numpoints
                nb_wcorr = WfrakL[i, j]/glweights[j] - log(tdist[j])
                # i==j term on self panel omitted since GL(0)=0 for modified stresslet
                zi = ztrg[1:2, i]
                zj = zsrc[1:2, j]
                nj = nsrc[1:2, j]
                wj = wsrc[j]
                rij = zj.-zi
                dblkernel_log!(rij, nj, alpha, KL)
                block_2x2_add!(B, i, j, KL.*(nb_wcorr*wj))
            end
        end
    end
end

"""
The choice of input arguments trans=0 and scale=1 gives WL
for ri and rj on the same quadrature panel γp. The choice
trans=±2 and scale=1 gives WL for ri on a neighboring panel
γp±1, assuming it is equal in parameter length. The input
argument tfrak is a column vector whose entries contain the
canonical nodes ti.
"""
function WfrakLcomp(trans::Float64,
                    scale::Float64,
                    tfrak::Array{Float64,1})
    npt = length(tfrak)
    tt=trans+scale*tfrak
    Q=zeros(npt, npt)
    p=zeros(1,npt+1)
    for j=1:npt
        p[1]=log(abs((1-tt[j])/(1+tt[j])))
        for k=1:npt
            c=(1-(-1)^k)/k
            p[k+1]=p[k]*tt[j]+c
        end
        for i=1:2:npt-1
            Q[i,j]=log(abs(1-tt[j]^2))-p[i+1] # 1658880
            Q[i+1,j]=p[1]-p[i+2]
        end
        for i=1:npt
            Q[i,j]=Q[i,j]/i
        end
    end

    # Direct solution:
    # A=ones(npt, npt)
    # for k=2:npt
    #     A[k,:]=tfrak.*A[k-1,:]
    # end
    # WfrakL =(A\Q).'

    # Bjorck-Pereyra:
    WfrakL = zeros(npt, npt);
    for i=1:npt
        WfrakL[i,:] = pvand(tfrak, Q[:,i]);
    end

    return WfrakL
end

function WfrakLcomp(targets, sources)
    tt = targets
    tfrak = sources
    nsrc = length(sources)
    ntrg = length(targets)
    Q=zeros(nsrc, ntrg)
    p=zeros(1,nsrc+1)
    for j=1:ntrg
        p[1]=log(abs((1-tt[j])/(1+tt[j])))
        for k=1:nsrc
            c=(1-(-1)^k)/k
            p[k+1]=p[k]*tt[j]+c
        end
        for i=1:2:nsrc-1
            Q[i,j]=log(abs(1-tt[j]^2))-p[i+1] # 1658880
            Q[i+1,j]=p[1]-p[i+2]
        end
        for i=1:nsrc
            Q[i,j]=Q[i,j]/i
        end
    end
    # Direct solution:
    # A=ones(npt, npt)
    # for k=2:npt
    #     A[k,:]=tfrak.*A[k-1,:]
    # end
    # WfrakL =(A\Q).'

    # Bjorck-Pereyra:
    WfrakL = zeros(ntrg, nsrc);
    for i=1:ntrg
        WfrakL[i,:] = pvand(tfrak, Q[:,i]);
    end

    return WfrakL
end

"""
    This function relies on complex arithmetic and takes, as
    input, points and vec- tors in R2 represented as points in
    C. Otherwise the notation follows Section 6: input
    parameters za and zb correspond to tau(ta) and tau(tb); z is the
    target point z ∈ E;and zj, nuj,and zpwj are column vectors
    whose entries contain the points zj , the exterior unit
    normals nu at rj , and the weighted velocity function zpwj
    , j = 1,...,npt.
    """
function modifiedweights(za::Complex{Float64},
                         zb::Complex{Float64},
                         z::Complex{Float64},
                         zj::Array{Complex{Float64}}
                         )
    npt = length(zj)
    # Transform endpoints to [-1,1]
    dz = (zb-za)/2
    ztr = (z-(zb+za)/2)/dz
    zjtr = (zj-(zb+za)/2)/dz
    p = Array{Complex{Float64}}(npt+1)
    q = Array{Complex{Float64}}(npt)
    r = Array{Complex{Float64}}(npt)
    s = Array{Complex{Float64}}(npt)

    c = (1-(-1).^(1:npt))./(1:npt)
    p[1] = log(1-ztr) - log(-1-ztr)
    p1 = log(1-ztr) + log(-1-ztr)
    # Signs here depend on how we have defined
    # direction of curve and normals. I think.
    if imag(ztr)<0 && abs(real(ztr))<1
        p[1] += 2im*pi
        p1 -= 2im*pi
    end
    for k=1:npt
	p[k+1] = ztr*p[k] + c[k]
    end

    r[1] = -1/(1+ztr)-1/(1-ztr)
    s[1] = -1/2*(1/(1-ztr)^2 - 1/(-1-ztr)^2)
    for k=1:npt-1
	r[k+1] = ztr*r[k] + p[k]
        s[k+1] = ztr*s[k] + r[k]
    end

    q[1:2:npt-1] = p1 - p[2:2:npt]
    q[2:2:npt] = p[1] - p[3:2:npt+1]
    q = q./(1:npt)
    # Bjorck-Pereyra solve
    wL = pvand(zjtr, q)*dz
    wC = pvand(zjtr, p[1:npt])
    wQ = pvand(zjtr, r) / dz
    wT = pvand(zjtr, s) / dz^2

    # Direct solve:
    # A = Array{Complex{Float64}}(npt, npt)
    # A[1,:] = 1.0
    # for k=2:npt
    #     for j=1:npt
    #         A[k,j]=zjtr[j]*A[k-1,j] # vandermonde transposed
    #     end
    # end
    # wL = (A\q)*dz
    # wC = A\p[1:npt]
    # wQ = (A\r) / dz
    # Only do one solve
    # sol = A\[q p[1:npt] r];
    # wL = sol[:,1]*dz
    # wC = sol[:,2]
    # wQ = sol[:,3] / dz

    return (wL, wC, wQ, wT)
end

# Compute near eval weights for log, Cauchy and stresslet singularities
function near_eval_weights(za, zb, z, zj, nuj, zpwj)
    # Compute modified weights
    wL, wC, wQ = modifiedweights(za, zb, z, zj)
    # Create corrections
    return near_eval_weights(za, zb, z, zj, nuj, zpwj, wL, wC, wQ)
end

function near_eval_weights(za, zb, z, zj, nuj, zpwj, wL, wC, wQ)
    # Create corrections
    dz = (zb-za)/2
    nuj = -nuj # Helsing has n = -i*t
    ## Log
    wcorrL = @. imag(wL*conj(nuj)) + log(abs(dz))*abs(zpwj)
    ## Cauchy
    wcorrC = @. -imag(wC)
    ## Unmodified stresslet
    wcorrQ1 = -1/2*imag(wC)
    wcorrQ2 = @. 1/4im*conj(conj(nuj)^2*wC + conj(zj-z)*wQ)
    return wcorrL, wcorrC, wcorrQ1, wcorrQ2
end