# capsule contact point origin sphere-capsule
function contact_point_segment(ps, xc, qc, ka, kb)
    # capsule origin a
    ca = contact_point_origin(xc, qc, ka)

    # capsule origin b
    cb = contact_point_origin(xc, qc, kb)

    # point differences
    dab = cb - ca
    das = ps - ca

    t = dot(das, dab) / dot(dab, dab)
    # t_clamp = min(max(t, 0.0), 1.0)

    # closest point
    if t <= 0.0
        p = ca
    elseif t >= 1.0
        p = cb
    else
        p = ca + t * dab
    end

    return p
end

# capsule contact point origin sphere-capsule
function ∂contact_point_segment∂p(ps, xc, qc, ka, kb)
    # capsule origin a
    ca = contact_point_origin(xc, qc, ka)

    # capsule origin b
    cb = contact_point_origin(xc, qc, kb)

    # point differences
    dab = cb - ca
    das = ps - ca

    t = dot(das, dab) ./ dot(dab, dab)
    # t_clamp = min(max(t, 0.0), 1.0)

    # closest point
    if t <= 0.0
        # p = ca
        return szeros(3, 3)
    elseif t >= 1.0
        # p = cb
        return szeros(3, 3)
    else
        # p = ca + t * dab
        return dab * dab' ./ dot(dab, dab)
    end

    return p
end

# capsule contact point origin sphere-capsule
function ∂contact_point_segment∂x(ps, xc, qc, ka, kb)
    # capsule origin a
    ca = contact_point_origin(xc, qc, ka)

    # capsule origin b
    cb = contact_point_origin(xc, qc, kb)

    # point differences
    dab = cb - ca
    das = ps - ca

    t = dot(das, dab) ./ dot(dab, dab)
    # t_clamp = min(max(t, 0.0), 1.0)

    # closest point
    if t <= 0.0
        # p = ca
        return ∂contact_point_origin∂x(xc, qc, ka)
    elseif t >= 1.0
        # p = cb
        return ∂contact_point_origin∂x(xc, qc, kb)
    else
        # p = ca + t * dab
        X = ∂contact_point_origin∂x(xc, qc, ka)
        X += t * (∂contact_point_origin∂x(xc, qc, kb) - ∂contact_point_origin∂x(xc, qc, ka))

        t1 = 1.0 / (dab' * dab) * dab' * (-∂contact_point_origin∂x(xc, qc, ka))
        t2 = 1.0 / (dab' * dab) * das' * (∂contact_point_origin∂x(xc, qc, kb) - ∂contact_point_origin∂x(xc, qc, ka))
        t3 = -2.0 * das' * dab / (dab' * dab)^2 * dab' * (∂contact_point_origin∂x(xc, qc, kb) - ∂contact_point_origin∂x(xc, qc, ka))

        X += dab * (t1 + t2 + t3)
        return X
    end
end

# capsule contact point origin sphere-capsule
function ∂contact_point_segment∂q(ps, xc, qc, ka, kb)
    # capsule origin a
    ca = contact_point_origin(xc, qc, ka)

    # capsule origin b
    cb = contact_point_origin(xc, qc, kb)

    # point differences
    dab = cb - ca
    das = ps - ca

    t = dot(das, dab) ./ dot(dab, dab)
    # t_clamp = min(max(t, 0.0), 1.0)

    # closest point
    if t <= 0.0
        # p = ca
        return ∂contact_point_origin∂q(xc, qc, ka)
    elseif t >= 1.0
        # p = cb
        return ∂contact_point_origin∂q(xc, qc, kb)
    else
        # p = ca + t * dab
        Q = ∂contact_point_origin∂q(xc, qc, ka)
        Q += t * (∂contact_point_origin∂q(xc, qc, kb) - ∂contact_point_origin∂q(xc, qc, ka))

        t1 = 1.0 / (dab' * dab) * dab' * (-∂contact_point_origin∂q(xc, qc, ka))
        t2 = 1.0 / (dab' * dab) * das' * (∂contact_point_origin∂q(xc, qc, kb) - ∂contact_point_origin∂q(xc, qc, ka))
        t3 = -2.0 * das' * dab ./ (dab' * dab)^2 * dab' * (∂contact_point_origin∂q(xc, qc, kb) - ∂contact_point_origin∂q(xc, qc, ka))
        Q += dab * (t1 + t2 + t3)

        return Q
    end
end

# # case 1
# ps = [-2.0 - 0.0e-8; 0.0; 1.0]
# xc = [0.0; 0.0; 0.0]
# qc = Quaternion([1.0; 0.0; 0.0; 0.0])
# ka = [1.0; 0.0; 0.0]
# kb = [-1.0; 0.0; 0.0]

# contact_point_segment(ps, xc, qc, ka, kb)

# ∂contact_point_segment∂p(ps, xc, qc, ka, kb)
# FiniteDiff.finite_difference_jacobian(p -> contact_point_segment(p, xc, qc, ka, kb), ps)

# ∂contact_point_segment∂x(ps, xc, qc, ka, kb)
# FiniteDiff.finite_difference_jacobian(x -> contact_point_segment(ps, x, qc, ka, kb), xc)

# ∂contact_point_segment∂q(ps, xc, qc, ka, kb)
# FiniteDiff.finite_difference_jacobian(q -> contact_point_segment(ps, xc, Quaternion(q..., false), ka, kb), vector(qc))

# # case 2
# ps = [1.0 + 100.0e-8; 0.0; 1.0]
# xc = [0.0; 0.0; 0.0]
# qc = Quaternion([1.0; 0.0; 0.0; 0.0])
# ka = [1.0; 0.0; 0.0]
# kb = [-1.0; 0.0; 0.0]

# contact_point_segment(ps, xc, qc, ka, kb)

# ∂contact_point_segment∂p(ps, xc, qc, ka, kb)
# FiniteDiff.finite_difference_jacobian(p -> contact_point_segment(p, xc, qc, ka, kb), ps)

# ∂contact_point_segment∂x(ps, xc, qc, ka, kb)
# FiniteDiff.finite_difference_jacobian(x -> contact_point_segment(ps, x, qc, ka, kb), xc)

# ∂contact_point_segment∂q(ps, xc, qc, ka, kb)
# FiniteDiff.finite_difference_jacobian(q -> contact_point_segment(ps, xc, Quaternion(q..., false), ka, kb), vector(qc))

# # case 3
# ps = [0.0; 0.0; 1.0]
# xc = [0.0; 0.0; 0.0]
# qc = Quaternion([1.0; 0.0; 0.0; 0.0])
# ka = [1.0; 0.0; 0.0]
# kb = [-1.0; 0.0; 0.0]

# contact_point_segment(ps, xc, qc, ka, kb)

# ∂contact_point_segment∂p(ps, xc, qc, ka, kb)
# FiniteDiff.finite_difference_jacobian(p -> contact_point_segment(p, xc, qc, ka, kb), ps)

# ∂contact_point_segment∂x(ps, xc, qc, ka, kb)
# FiniteDiff.finite_difference_jacobian(x -> contact_point_segment(ps, x, qc, ka, kb), xc)

# ∂contact_point_segment∂q(ps, xc, qc, ka, kb)
# FiniteDiff.finite_difference_jacobian(q -> contact_point_segment(ps, xc, Quaternion(q..., false), ka, kb), vector(qc))

# function get_t(a, b)
#     return a' * b ./ (b' * b)
# end

# function dget_tda(a, b)
#     return b' ./ (b' * b)
# end

# function dget_tdb(a, b)
#     D = a' ./ (b' * b)
#     D += -1.0 * a' * b ./ (b' * b)^2 * 2.0 * b'
#     return D
# end

# a = rand(3)
# b = rand(3)

# get_t(a, b)

# dget_tda(a, b)
# FiniteDiff.finite_difference_jacobian(x -> get_t(x, b), a)

# dget_tdb(a, b)
# FiniteDiff.finite_difference_jacobian(x -> get_t(a, x), b)


using jlmie
using Plots

# settings
# structure
nmat = 4  # refractive index of a sphere
radius = 100*1e-9

# environment
nenv = 1.00

# wavelenth
lbd0 = (400:1000)*1e-9; lbdp = lbd0.*1e9;

# other settings
nmax = -1  # -1: namx large enough (determined from x)

# main
m,x = jlmie_mx(nmat,radius,lbd0,nenv)
Qsca = jlmie_Qsca(m,x,nmax)
Qsca_ED = jlmie_Qsca_a_n(m,x,1)
Qsca_MD = jlmie_Qsca_b_n(m,x,1)
Qsca_EQ = jlmie_Qsca_a_n(m,x,2)
Qsca_MQ = jlmie_Qsca_b_n(m,x,2)
Qsca_EH = jlmie_Qsca_a_n(m,x,3)
Qsca_MH = jlmie_Qsca_b_n(m,x,3)
println("calculation completed")

# show results
# validation can be done by comparing to ITMO Mie Calculator
# https://physics.itmo.ru/en/mie
plt = plot(lbdp,Qsca,
    xlabel       = "Wavelength (nm)",
    ylabel       = "Scattering efficiency",
    # legend       = false,
    label        = "Total",
    # xlims        =(-3,3),
    # ylims        =(-3,3),
    # aspect_ratio =0.5,
    # title        ="TITLE",
    # linecolor    =:blue,
    # linewidth    =5,
    # linestyle    =:dot,
    # size         =(400,300),
) # https://qiita.com/I_ppp/items/dca3552affa6a672e4bd
plot!(lbdp,Qsca_ED,
    label = "ED",
    linestyle = :dash,
)
plot!(lbdp,Qsca_MD,
    label = "MD",
    linestyle = :dash,
)
plot!(lbdp,Qsca_EQ,
    label = "EQ",
    linestyle = :dash,
)
plot!(lbdp,Qsca_MQ,
    label = "MQ",
    linestyle = :dash,
)
plot!(lbdp,Qsca_EH,
    label = "EH",
    linestyle = :dash,
)
plot!(lbdp,Qsca_MH,
    label = "MH",
    linestyle = :dash,
)
display(plt)

# save
if true
    runningfilename = splitext(splitpath(@__FILE__)[end])[1]
    outfilename = ".\\result\\" * runningfilename
    savefig(plt,outfilename)
    println("Graph has been saved in result directory")
end

# find peaks
lbdp_peaks = zeros(6)
lbdp_peaks[1] = lbdp[findmax(Qsca_ED)[2]]
lbdp_peaks[2] = lbdp[findmax(Qsca_MD)[2]]
lbdp_peaks[3] = lbdp[findmax(Qsca_EQ)[2]]
lbdp_peaks[4] = lbdp[findmax(Qsca_MQ)[2]]
lbdp_peaks[5] = lbdp[findmax(Qsca_EH)[2]]
lbdp_peaks[6] = lbdp[findmax(Qsca_MH)[2]]

println("maximum of ED is at " * string(round(lbdp_peaks[1])))
println("maximum of MD is at " * string(round(lbdp_peaks[2])))
println("maximum of EQ is at " * string(round(lbdp_peaks[3])))
println("maximum of MQ is at " * string(round(lbdp_peaks[4])))
println("maximum of EH is at " * string(round(lbdp_peaks[5])))
println("maximum of MH is at " * string(round(lbdp_peaks[6])))


"""
linear_aa()

Test aasol.jl for a two-dimensional linear problem
"""
function linear_aa()
maxit = 10
maxm = 2
vdim = 3 * maxm + 3
Vstore = zeros(2, vdim)
vdimS = 2 * maxm + 4
VstoreS = zeros(2, vdimS)
eigs=[.1, .5]
xstar=ones(2,)
pdata=makeLinpdata(eigs)
m=0
#
# Test for termination on entry
#
x0=[1.0, 1.0]; m=0;
aout = aasol(GLin!, x0, m, Vstore;
     rtol = 1.e-10, pdata=pdata, maxit=maxit, keepsolhist=true);
# Same results with low storage method?
aoutS = aasol(GLin!, x0, m, VstoreS;
     rtol = 1.e-10, pdata=pdata, maxit=maxit, keepsolhist=true);
linerrS = (
        norm(aoutS.history-aout.history,Inf) +
        norm(aoutS.stats.condhist-aout.stats.condhist,Inf) +
        norm(aoutS.stats.alphanorm-aout.stats.alphanorm,Inf)
    )
tflag = (aout.errcode===-1) && aout.idid && (linerrS < 1.e-15)
tflag || println("Failure in aasol terminate on entry test.")
#
# Test for failure to converge
#
x0=[2.0, 10.0]
m=0
maxit=10
aout = aasol(GLin!, x0, m, Vstore;
     rtol = 1.e-10, pdata=pdata, maxit=maxit, keepsolhist=true);
failflag = ~aout.idid && (aout.errcode == 10)
failflag || println("Linear iteration failure in aasol test fails.")
#
# Test for convergence is two iterations.
#
m=2
aout = aasol(GLin!, x0, m, Vstore;
     rtol = 1.e-10, pdata=pdata, maxit=maxit, keepsolhist=true);
termflag=(length(aout.history) == 4) &&
         (norm(aout.solution - xstar,Inf) < 1.e-14)
termflag || println("Terminate in two aa iterations test fails.")
#
# Now set the eigenvalues to [2.0, 10.0] and beta=-1/9
#
eigs=[2.0, 10.0]
pdata=makeLinpdata(eigs)
beta=-1.0/9.0
maxit=10
m=1
aout1 = aasol(GLin!, x0, m, Vstore;
     rtol = 1.e-10, pdata=pdata, maxit=maxit)
aout2 = aasol(GLin!, x0, m, Vstore;
     rtol = 1.e-10, pdata=pdata, maxit=maxit, beta=beta)
bflag= ~aout1.idid && aout2.idid && (length(aout2.history)==8)
return tflag && failflag && termflag && bflag

end

function GLin!(gout,xin,pdata)
M=pdata.M
b=pdata.b
gout=M*xin+b
return gout
end

function GLinBeta!(gout,xin,pdata)
M=pdata.M
b=pdata.b
beta=pdata.beta
gout = Glin!(gout,xin,pdata)
gout .*=beta
gout .+= (1.0-beta)*xin
return gout
end

function makeLinpdata(eigs, beta=1.0)
U=[1 -1; 1 1]./sqrt(2.0)
V=[3 -4; 4 3]./5.0
S=diagm(eigs)
M=U*S*V'
b=(I-M)*ones(2,);
return (M=M, b=b, beta=beta)
end


using SpikingNetworks
@unit
parseParameters"""
Cm = 1*uF
Iapp = 2*uA
gL = 0.1*msiemens
EL = -65*mV
ENa = 55*mV
EK = -90*mV
gNa = 35*msiemens
gK = 9*msiemens"""

parseEquations"""
dv/dt=(-gNa*m(v)^3*h*(v-ENa)-gK*n^4*(v-EK)-gL*(v-EL)+Iapp)/Cm
dh/dt=5*(α_h(v)*(1-h)-β_h(v)*h)
dn/dt=5*(α_n(v)*(1-n)-β_n(v)*n)
m(v)=α_m(v)/(α_m(v)+β_m(v))
α_m(v)=-0.1*(v+35)/(exp(-0.1*(v+35))-1)
β_m(v)=4*exp(-(v+60)/18)
α_h(v)=0.07*exp(-(v+58)/20)
β_h(v)=1./(exp(-0.1*(v+28))+1)
α_n(v)=-0.01*(v+34)/(exp(-0.1*(v+34))-1)
β_n(v)=0.125*exp(-(v+44)/80)"""

net=init"""
v=-70*ones(N)
h=1*ones(N)
n=0*ones(N)"""

stateMonitor"[1]"

net.dt=0.1
run!(net,100/net.dt)
using MatlabPlot
mplot(net.vRecord)

# workspace()


function floorsum(n, m, a, b)
    ans = 0
    if a >= m
        ans += (n-1) * n * (a÷m) ÷ 2
        a %= m
    end
    if b >= m
        ans += n * (b÷m);
        b %= m
    end

    ymax = (a * n + b) ÷ m
    xmax = (ymax * m - b)
    ymax == 0 && return ans
    ans += (n - (xmax + a - 1) ÷a ) * ymax
    ans += floorsum(ymax, a, m, (a - xmax % a) % a)
    return ans
end


using StructJuMP, JuMP
using StructJuMPSolverInterface

include("select_solver.jl")

#############
# A sample model
#############
scen = 2
m = StructuredModel(num_scenarios=scen)
@variable(m, x[1:2])
@variable(m, z[1:2])
@NLobjective(m, Min, x[1]^2+x[2]^2+x[1]*z[2] + x[1]*x[2])
@NLconstraint(m, x[1] + x[2]<= 100)

for i in 1:scen
    bl = StructuredModel(parent=m)
    @variable(bl, y[1:2])
    @NLconstraint(bl, -100<= x[2]*x[1] + x[1]*y[1]*y[2] <= 100)
    @NLconstraint(bl, -100<= x[2]*x[1]*y[1]*y[2] <= 100)
    @NLobjective(bl, Min, (y[1]+y[2])^2)
end

structJuMPSolve(m)

getVarValue(m)

#verification
# x[1]^2+x[2]^2+x[1]*x[4] + x[1]*x[2] + (y1[1]+y1[2])^2 + (y2[1]+y2[2])^2

# x[1] + x[2]
# x[2]*x[1] + x[1]*y1[1]*y1[2]
# x[2]*x[1]*y1[1]*y1[2]
# x[2]*x[1] + x[1]*y2[1]*y2[2]
# x[2]*x[1]*y2[1]*y2[2]


using Plots
using LinearAlgebra

using SparseArrays
using Arpack
#komvos1 komvos2 komvos3
xy = [ 0 0;
       0 1]; #orismos komvon san xyz
nodes = size(xy)[2]; #arithmos komvon

#komvos1_start komvos1_end
numel = [1 2;

]; #orismos ton stixion os grammes me toys akraious komvous
N = size(numel)[1]; #arithmos stoixion
dofs = 3*nodes;
#orismos elastikotitas stixion
a = 10e-2;
E = 220e9.*ones(N,1); I = a^4 .*ones(N,1)./12; A = a^2 .*ones(N,1) #is square with axa
rho = 7.8e3;

#orismos ton desmeumenon komvon
BC = zeros(dofs,1)
#ignored = 1:3:dofs;#ignore turn of axis=ignore freedom
BC[1:3] .= 1; #BC[ignored].=1;
fixedofs = findall(x->x==1,BC[:])
freedofs = findall(x->x==0,BC[:])
#boundary conditions
U = zeros(dofs,1);
F = zeros(dofs,1); F[[5]] .= 10e3;
#euresi olon ton deikton pou tha prospelasoun ton K
alldofs = zeros(N,6);
#orismos olikou mitroou stivarotitas
global K = sparse([],[],Float64[],dofs,dofs);
global M = sparse([],[],Float64[],dofs,dofs);

#global K = zeros(dofs,dofs);
#start main loop to calculate stiffness matrix
for i=1:N
    i1 = numel[i,1]
    i2 = numel[i,2]
    #calclulate directions
    x1 = xy[1,i1]; x2 = xy[1,i2]; dx = x2-x1;
    y1 = xy[2,i1]; y2 = xy[2,i2]; dy = y2-y1;
    L = sqrt(dx^2+dy^2);
    nx = dx/L; ny = dy/L;
    #direction matrix
    c = nx; s = ny;
    Bbend = [-s c 0  0 0 0;
              0 0 1  0 0 0;
              0 0 0 -s c 0;
              0 0 0  0 0 1];
    Btens = [c s 0 0;
             0 0 c s];
    kbend = E[i]*I[i]/L^3;

    #insert tensile
    ktens = E[i]*A[i]/L
    global Ketens = ktens*[1 -1; -1 1]
    Ketens = Btens'*Ketens*Btens
    global Metens = rho*A[i]*L*[2 1; 1 2]./6;
    Metens = Btens'*Metens*Btens;

    Kebend = kbend*[12 6*L   -12   6*L;
                     0 4*L^2 -6*L 2*L^2;
                     0  0     12  -6*L;
                     0  0     0   4*L^2]
    Kebend = Symmetric(Kebend);
    Kebend = Bbend'*Kebend*Bbend;
    Mebend = rho*A[i]*(L/420)*[156   22*L  54     -13*L;
                                0   4*L^2 13*L   -3*L^2;
                                0     0   156     -22*L;
                                0     0    0     4*L^2];
    Mebend = Symmetric(Mebend);
    Mebend = Bbend'*Mebend*Bbend;

    tensiledofs = [1 2 4 5];
    Ke = Kebend .+0;
    Ke[tensiledofs,tensiledofs] = Ke[tensiledofs,tensiledofs][1,:,1,:]  + Ketens;
        #println(size(Ke),"\n")
    edofs = [3*i1-2 3*i1-1 3*i1  3*i2-2 3*i2-1 3*i2]
    K[edofs,edofs] = K[edofs,edofs][1,:,1,:] + Ke;
    #println(K,"\n")
    Me = Mebend .+0;
    Me[tensiledofs,tensiledofs] = Me[tensiledofs,tensiledofs][1,:,1,:]  + Metens;
    M[edofs,edofs] = M[edofs,edofs][1,:,1,:] + Me;
end
Kfinal = K[freedofs,freedofs]
Mfinal = M[freedofs,freedofs]
rhs = F
rhs = rhs[freedofs]
rhs = rhs - K[freedofs,fixedofs]*U[fixedofs]

Ufree = Kfinal\rhs
U[freedofs] = Ufree
U = reshape(U,(3,nodes))
show(U[:])
#println(varinfo())
omega = sqrt.(eigvals(inv(Matrix(Mfinal))*Matrix(Kfinal)))/(2*pi)
scatter(omega)

#calculate


"""
Struct for VG Process

		vgProcess=VarianceGammaProcess(σ::num1,θ::num2,κ::num3) where {num1,num2,num3<: Number}

Where:\n
		σ =	volatility of the process.
		θ = variance of volatility.
		κ =	skewness of volatility.
"""
mutable struct VarianceGammaProcess{num <: Number, num1 <: Number, num2 <: Number, abstrUnderlying <: AbstractUnderlying, numtype <: Number} <: InfiniteActivityProcess{numtype}
    σ::num
    θ::num1
    κ::num2
    underlying::abstrUnderlying
    function VarianceGammaProcess(σ::num, θ::num1, κ::num2, underlying::abstrUnderlying) where {num <: Number, num1 <: Number, num2 <: Number, abstrUnderlying <: AbstractUnderlying}
        if σ <= 0
            error("volatility must be positive")
        elseif κ <= 0
            error("κappa must be positive")
        elseif 1 - σ * σ * κ / 2 - θ * κ < 0
            error("Parameters with unfeasible values")
        else
            zero_typed = zero(num) + zero(num1) + zero(num2)
            return new{num, num1, num2, abstrUnderlying, typeof(zero_typed)}(σ, θ, κ, underlying)
        end
    end
end

export VarianceGammaProcess;

function simulate!(X, mcProcess::VarianceGammaProcess, rfCurve::AbstractZeroRateCurve, mcBaseData::AbstractMonteCarloConfiguration, T::Number)
    r = rfCurve.r
    S0 = mcProcess.underlying.S0
    d = dividend(mcProcess)
    Nstep = mcBaseData.Nstep
    σ = mcProcess.σ
    θ1 = mcProcess.θ
    κ1 = mcProcess.κ
    @assert T > 0

    dt = T / Nstep
    psi1 = -1 / κ1 * log(1 - σ^2 * κ1 / 2 - θ1 * κ1)
    drift = r - d - psi1

    gammaRandomVariable = Gamma(dt / κ1, κ1)

    simulate!(X, SubordinatedBrownianMotion(σ, drift, gammaRandomVariable), mcBaseData, T)

    f(x) = S0 * exp(x)
    broadcast!(f, X, X)

    nothing
end


"""
    OlsSureFn(y,x,m=1)

LS of y on x; for one n dependent variables, same regressors

# Usage
(b,res,yhat,Covb,R2a) = OlsSureFn(y,x,m)

# Input
- `y::Array`:     Txn, the n dependent variables
- `x::Array`:     Txk matrix of regressors (including deterministic ones)
- `m::Int`:       scalar, bandwidth in Newey-West

# Output
- `b::Array`:     n*kx1, regression coefficients
- `u::Array`:     Txn, residuals y - yhat
- `yhat::Array`:  Txn, fitted values x*b
- `V::Array`:     matrix, covariance matrix of sqrt(T)vec(b)
- `R2a::Number`:  n vector, R2 value

"""
function OlsSureFn(y,x,m=0)
    (T,n) = (size(y,1),size(y,2))
    k     = size(x,2)
    b     = x\y
    yhat  = x*b
    u     = y - yhat
    g     = zeros(T,n*k)
    for i = 1:n
      vv      = (1+(i-1)*k):(i*k)   #1:k,(1+k):2k,...
      g[:,vv] = x.*u[:,i]           #moment conditions for y[:,i] regression
    end
    S0    = NWFn(g,m)            #Newey-West covariance matrix
    Sxxi  = -x'x/T
    Sxx_1 = kron(Matrix(1.0I,n,n),inv(Sxxi))    #Matrix(1.0I,n,n) is identity matrix(n)
    V     = Sxx_1 * S0 * Sxx_1
    R2a   = 1.0 .- Compat.var(u,dims=1)./Compat.var(y,dims=1)  #0.7 syntax
    return b,u,yhat,V,R2a
end


module CommutativeRings

using LinearAlgebra
using Base.Checked
using Primes

export category_trait, isfield
export Ring, RingInt, FractionRing, QuotientRing, Polynomial
export ZZ, QQ, ZZmod, Frac, Quotient, UnivariatePolynomial, MultivariatePolynomial
export GaloisField

export Hom, Ideal

export isunit, deg, content, primpart, isnegative
export LC, LM, LT, lcunit, multideg, modulus, value
export isdiv, pdivrem, pgcd, pgcdx, resultant, discriminant
export basetype, basetypes, depth, iszerodiv
export monom, ismonom, ismonic, issimpler, iscoprime, evaluate, derive
export isirreducible, irreducible, irreducibles, monic, factorise
export num_irreducibles, isreducible, reducible, reducibles
export characteristic, dimension, order
export ofindex, index
export log_zech, generator
export GF, homomorphism
export num_primitives, isprimitive

export VectorSpace, complement, sum, intersect, isequal, issubset
export groebnerbase, SPOL, lextend
export generators, varnames, varname, factors

export characteristic_polynomial, adjugate, companion

export coeffbounds

import Base: +, -, *, /, inv, ^, \, //, getindex, sign, log
import Base: iszero, isone, zero, one, div, rem, divrem, ==, hash, gcd, gcdx, lcm
import Base: copy, show, promote_rule, convert, abs, isless
import Primes: factor
import Base: numerator, denominator
import LinearAlgebra: checksquare, det

# RingClass subtypes describe the different categories
abstract type RingClass end
struct ZZClass{T<:Integer} <: RingClass end
abstract type FractionRingClass <:RingClass end
struct FractionClass{P} <: FractionRingClass end
struct QQClass{S<:Integer} <:FractionRingClass end
abstract type QuotientRingClass <:RingClass end
struct QuotientClass{Id,M} <: QuotientRingClass
    modulus::M
end
struct ZZmodClass{m,T<:Integer} <: QuotientRingClass
    modulus::T
end
abstract type PolyRingClass <: RingClass end
struct UniPolyRingClass{X,R} <: PolyRingClass end
struct MultiPolyRingClass{X,R,N} <: PolyRingClass
    varnames::Vector{Symbol}
end

struct GaloisFieldClass{Id,T,Q} <: QuotientRingClass
    exptable::Vector{T}
    zechtable::Vector{T}
end

const NCT = Val{:nocheck}
const NOCHECK = Val(:nocheck)

# Ring subtypes describe the ring elements
# They are connected to a corresponding RingClass by type parameters
# and accessible by method `owner`.
"""
    Ring{<:RingClass}

Abstract superclass of all commutative ring element types.
All subtypes support the arithmetic operators `+, -, *, and ^(power)`.
The operators `inv, /` may be defined on a subset.
"""
abstract type Ring{T<:RingClass} end

"""
    RingInt

Union of system `Integer` types and any `Ring` subtype.
"""
const RingInt = Union{Ring,Integer}

"""
    FractionRing{S<:RingInt,T<:FractionRingClass}

Rational extension of elements of ring `S`. Example the field of rational numbers.
"""
abstract type FractionRing{S<:RingInt,T<:FractionRingClass} <: Ring{T} end

"""
    QuotientRing{S<:Union{Integer,Ring},T<:QuotientRingClass}

Quotient ring of ring `S` by some ideal. If S = Z, the ring of integer numbers, and p
a positive number Z/p is calculation modulo p.
"""
abstract type QuotientRing{S<:RingInt,T<:QuotientRingClass} <: Ring{T} end

"""
    Polynomial{S<:Ring,T<:PolyRingClass}
"""
abstract type Polynomial{S<:Ring,T<:PolyRingClass} <: Ring{T} end

"""
    ZZ{S<:Signed}

The ring of integer numbers. There may be restrictions on the set of representable
elemets according to the chosen `S`.
"""
struct ZZ{T<:Signed} <: Ring{ZZClass{T}}
    val::T
    ZZ{T}(val::Integer) where T = new{T}(val)
    ZZ(val::T) where T<:Signed = ZZ{T}(val)
end

"""
    ZZmod{m,S<:Integer}

Quotient ring modulo integer `m > 0`, also noted as `ZZ/m`.
If `p` is a prime number, `ZZmod{p}` is the field `ZZ/p`.
"""
struct ZZmod{m,S<:Integer} <: QuotientRing{S,ZZmodClass{m,S}}
    val::S
    ZZmod{m,T}(a::Integer, ::NCT) where {m,T} = new{m,T}(T(a))
end

"""
    Frac{R}

The ring of fractions of `R`. The elements consist of pairs `num::R,den::R`.
During creation the values may be canceled to achieve `gcd(num, den) == one(R)`.
The special case of `R<:Integer` is handled by `QQ{R}`.
"""
struct Frac{P<:Union{Polynomial,ZZ}} <: FractionRing{P,FractionClass{P}}
    num::P
    den::P
    Frac{P}(num::P, den::P, ::NCT) where P = new{P}(num, den)
end

"""
    Quotient{R,I,m}

The quotient ring of `R` modulo `m` of type `I`, also written as `R / m`.
`m` may be an ideal of `R` or a (list of) element(s) of `R` generating the ideal.
Typically `m` is replaced by a symbolic `X`, and the actual `m` is given as argument(s)
to the type constructor like  `new_class(Quotient{ZZ,`X`}, m...)`.
If the ``X``is omitted, an anonymous symbol is used.

The preferred way of construction is via `Zm = Z/m`.
"""
struct Quotient{R<:Ring,I,X,Id} <: QuotientRing{R,QuotientClass{X,I}}
    val::R
    Quotient{R,I,X,Id}(v::R, ::NCT) where {I,X,R<:Ring,Id} = new{R,I,X,Id}(v)
end

"""
    QQ{S<:Integer}

"""
struct QQ{S<:Integer} <: FractionRing{S,QQClass{S}}
    num::S
    den::S
    QQ{T}(num::Integer, den::Integer, ::NCT) where T = new{T}(num, den)
end

"""
    UnivariatePolynomial{S<:RingInt,Id}

Polynomials of ring elemets `S` in one variable `Id` (by default `:x`).
The variable name is specified as a `Symbol`.

A convenience constructor `S[:x]` is the preferred way to construct this class.
"""
struct UnivariatePolynomial{S<:Ring,X} <: Polynomial{S,UniPolyRingClass{X,S}}
    coeff::Vector{S}
    UnivariatePolynomial{S,X}(v::Vector{S}, ::NCT) where {X,S<:Ring} = new{S,X}(v)
end

"""
    MultivariatePolynomial{S<:RingInt,N,Id}

Polynomials of ring elemets `S` in `N` variables.
The `Id` identifies on object of type `MultiPolyRingClass` which is needed to store
the variable names and properties.

A convenience constructor `S[:x,:y...]` is the preferred way to construct this class.
"""
struct MultivariatePolynomial{S<:Ring,N,Id,T,B} <: Polynomial{S,MultiPolyRingClass{Id,S,N}}
    ind::Vector{T}
    coeff::Vector{S}
end

struct GaloisField{Id,T,Q} <: QuotientRing{ZZmod{T},GaloisFieldClass{Id,T,Q}}
    val::T
    GaloisField{Id,T,Q}(v::Integer, ::NCT) where {Id,T,Q} = new{Id,T,Q}(T(v))
end

# Categorial traits specify algebraic properties of ring types
# (cf. https://en.wikipedia.org/wiki/Integral_domain)
# rings ⊃ commutative rings ⊃ integral domains ⊃ integrally closed domains ⊃ GCD domains ⊃
# unique factorization domains ⊃ principal ideal domains ⊃ Euclidean domains ⊃ fields ⊃ algebraically closed fields
abstract type RingTrait end
abstract type CommutativeRingTrait <: RingTrait end
abstract type IntegralDomainTrait <: CommutativeRingTrait end
abstract type IntegrallyClosedDomainTrait <: IntegralDomainTrait end
abstract type GCDDomainTrait <: IntegrallyClosedDomainTrait end
abstract type UniqueFactorizationDomainTrait <: GCDDomainTrait end
abstract type PrincipalIdealDomainTrait <: UniqueFactorizationDomainTrait end
abstract type EuclidianDomainTrait <: PrincipalIdealDomainTrait end
abstract type FieldTrait <: EuclidianDomainTrait end
abstract type AlgebraicallyClosedFieldTrait <: FieldTrait end

## End of Ring classes

"""
    Ideal{R}

Respresent in Ideal of Ring `R`. Only Ideals with a finite (Groebner) basis.
"""
struct Ideal{R<:Ring}
    base::Vector{R}
    Ideal{R}(a::Vector{R}) where R = new{R}(a)
end

"""
    Hom{function,R,R'}

Represent a ring homomorphism `function:R -> R'`.
"""
struct Hom{F,R<:RingInt,S<:RingInt}
    f::F
    Hom{R,S}(f::F) where {F<:Union{Function,Type},R,S} = new{F,R,S}(f)
end

struct VectorSpace{T}
    base::T
    pivr::Vector{Int} # row permutation vector
end

# implementation

include("typevars.jl")
include("generic.jl")
include("zz.jl")
include("qq.jl")
include("zzmod.jl")
include("quotient.jl")
include("fraction.jl")
include("univarpolynom.jl")
include("multivarpolynom.jl")
include("ideal.jl")
include("enumerations.jl")
include("factorization.jl")
include("intfactorization.jl")
include("numbertheoretical.jl")
include("galoisfields.jl")
include("linearalgebra.jl")

end # module


abstract type Field end

Base.:/(x::F, y::F) where F <: Field = x * inv(y)

Base.one(x::F) where F <: Field = one(F)
Base.zero(x::F) where F <: Field = zero(F)

square(x::Field) = x*x
Base.inv(x::Field) = x^(order(x) - 1)

Base.literal_pow(::typeof(^), x::F, ::Val{2}) where F <: Field = square(x)
Base.literal_pow(::typeof(^), x::F, ::Val{0}) where F <: Field = one(F)

abstract type BinaryField <: Field end

Base.:-(x::F, y::F) where F <: BinaryField = x + y

<|(::Type{F}, x) where F <: BinaryField = convert(F, x)

function Base.convert(::Type{F}, x::Integer) where F <: BinaryField
    if x == 0
        return zero(F)
    elseif x == 1
        return one(F)
    else
        error("Conversion of $x not possible")
    end
end

function Base.:^(g::F, a::Integer) where F <: BinaryField

    if a == 0
        return one(F)
    end

    e = tobits(a)

    r = length(e) - 1

    x = g

    for i in 2:r+1

        x = square(x)

        if e[i] == 1
            x = g*x
        end
    end

    return x
end

function (::Type{F})(x::Integer) where F <: BinaryField
    if x == 0
        return zero(F)
    elseif x==1
        return one(F)
    else
        error("Not supported")
    end
end

function Base.convert(::Type{F}, x) where F <: BinaryField

    N = bitlength(F)

    bits = tobits(x)

    return convert(F, bits[end - N + 1:end])
end

Base.convert(::Type{F}, x::BinaryField) where F <: BinaryField = error("Can't be done blindly.")
Base.convert(::Type{F}, x::F) where F <: BinaryField = x

tobits(::Type{F}, x::BinaryField) where F <: BinaryField = convert(BitVector, x)

Base.show(io::IO, x::BinaryField) = print(io, join(i ? "1" : "0" for i in tobits(x)))

Base.isless(x::F, y::F) where F <: BinaryField = tobits(x) < tobits(y)

abstract type PrimeField <: Field end

<|(::Type{F}, x) where F <: PrimeField = F(x)

function modulus end
function value end

Base.show(io::IO, x::PrimeField) = print(io, value(x))

modinv(s, q) = mod(gcdx(s, q)[2], q)

Base.inv(x::F) where F <: PrimeField = F(modinv(value(x), modulus(x)))

Base.zero(::Type{F}) where F <: PrimeField = F(0)
Base.one(::Type{F}) where F <: PrimeField = F(1)

Base.:+(x::F, y::F) where F <: PrimeField = F(mod(value(x) + value(y), modulus(F)))
Base.:-(x::F, y::F) where F <: PrimeField = x + F(modulus(F) - value(y))
Base.:-(x::F) where F <: PrimeField = F(modulus(F) - value(x))

Base.:*(x::F, y::F) where F <: PrimeField = F(mod(value(x) * value(y), modulus(F)))
Base.:*(x::Integer, y::F) where F <: PrimeField = F(x) * y
Base.:*(x::F, y::Integer) where F <: PrimeField = x * F(y)

Base.:^(x::F, n::Integer) where F <: PrimeField = F(powermod(value(x), n, modulus(F)))

Base.:(==)(x::F, y::F) where F <: PrimeField = value(x) == value(y)

Base.isless(x::F, y::F) where F <: PrimeField = value(x) < value(y)


#=
given a symmetric sparsity pattern with some sparse entries unknown, find integer values
(within some bounds) for the unknowns such that the matrix is PSD-completable,
maximimizing the minimum eigenvalue

max y :
S + X - y I ∈ SparsePSD⋆
Xᵢⱼ ∈ ℤ, -M ≤ Xᵢⱼ ≤ M, ∀ (i,j) unknown

where:
- sparse parameter matrix S contains is the known entries
- sparse variable matrix X contains the unknown entries
- M is a bound on the absolute value of all unknown entries
=#

struct CompletablePSD <: ExampleInstance
    d::Int # side dimension
    sparsity::Float64 # fraction of elements in the pattern
    frac_known::Float64 # fraction of sparse entries known
    use_nat::Bool # use sparse PSD cone, else PSD cone formulation
end

function build(inst::CompletablePSD)
    d = inst.d
    @assert d >= 2

    # generate underlying positive definite matrix S0
    S0 = sprand(Bool, d, d, inst.sparsity)
    S0 = Matrix{Float64}(S0 * S0' + I)
    pattern = tril!(sprand(Bool, d, d, inst.sparsity) + I)
    (rows, cols, _) = findnz(pattern)
    S0vals = [S0[r, c] for (r, c) in zip(rows, cols)]
    num_sparse = length(S0vals)

    # select which entries of S0 will be known/unknown
    known_pattern = sprand(Bool, num_sparse, inst.frac_known)
    known_pattern[1] = known_pattern[end] = true
    num_unknown = num_sparse - sum(known_pattern)
    @assert num_unknown >= 1
    M = maximum(abs, S0vals[.!known_pattern])

    # build model
    model = JuMP.Model()
    JuMP.@variable(model, y)
    JuMP.@objective(model, Max, y)

    JuMP.@variable(model, x[1:num_unknown], Int)
    JuMP.@constraint(model, -M .<= x .<= M)

    aff = zeros(JuMP.AffExpr, num_sparse)
    k = 1
    scal = (inst.use_nat ? rt2 : 1.0)
    for i in eachindex(known_pattern)
        if known_pattern[i]
            aff[i] = S0vals[i]
        else
            aff[i] = x[k]
            k += 1
        end
        if rows[i] == cols[i]
            aff[i] = aff[i] - y
        else
            aff[i] = scal * aff[i]
        end
    end
    @assert k == num_unknown + 1

    if inst.use_nat
        KT = Hypatia.PosSemidefTriSparseCone{Hypatia.Cones.PSDSparseDense, Float64, Float64}
        K = KT(d, rows, cols, true)
        JuMP.@constraint(model, aff in K)
    else
        mat = Matrix(sparse(rows, cols, aff))
        for i in 1:d, j in 1:i
            if iszero(mat[i, j])
                mat[i, j] = JuMP.@variable(model)
            end
        end
        LinearAlgebra.copytri!(mat, 'L')
        JuMP.@constraint(model, Symmetric(mat) in JuMP.PSDCone())
    end

    # save for use in tests
    model.ext[:x] = x
    model.ext[:S0_eigmin] = eigmin(Hermitian(S0))

    return model
end

function test_extra(inst::CompletablePSD, model::JuMP.Model)
    stat = JuMP.termination_status(model)
    @test stat in OPT_OR_LIMIT
    (stat == MOI.OPTIMAL) || return

    tol = eps()^0.2
    x = JuMP.value.(model.ext[:x])
    @test x ≈ round.(Int, x) atol = tol rtol = tol
    # check eigmin(S + X) >= eigmin(S0)
    S0_eigmin = model.ext[:S0_eigmin]
    @test JuMP.objective_value(model) >= S0_eigmin - tol
    return
end


# =========================================================================== #
# Test
# =========================================================================== #

function test_triangular_solve_upper()

    U = [
        4.0     2       7   9;
        0       12      2   8;
        0       0       1   10;
        0       0       0   6
    ]
    b = [10.0,  2,  5,  6]

    x_row = linalg.triangular_solve_upper_rowwise(U, b)
    x_col = linalg.triangular_solve_upper_colwise(U, b)
    x_theo = U \ b
    println( "x rowwise: $(x_row)" )
    println( "x colwise: $(x_col)" )
    println( "x_theo rowwise: $(x_theo)" )
    println( "residuum norm x rowwise:  $(norm( U * x_row - b )) " )
    println( "residuum norm x colwise:  $(norm( U * x_col - b )) " )
    println( "residuum norm x_theo:  $(norm( U * x_theo - b )) ")
end

function test_triangular_solve_lower()

    L = [
        6.0     0   0   0
        10      1   0   0;
        8       2   12  0;
        9       7   2   4;
    ]

    b = [10.0,  2,  5,  6]

    x_row = linalg.triangular_solve_lower_rowwise(L, b)
    x_col = linalg.triangular_solve_lower_colwise(L, b)
    x_theo = L \ b
    println( "x rowwise: $(x_row)" )
    println( "x colwise: $(x_col)" )
    println( "x_theo rowwise: $(x_theo)" )
    println( "residuum norm x rowwise:  $(norm( L * x_row - b )) " )
    println( "residuum norm x colwise:  $(norm( L * x_col - b )) " )
    println( "residuum norm x_theo:  $(norm( L * x_theo - b )) ")
end

# =========================================================================== #
# Benchmark
# =========================================================================== #

function benchmark_trisolve_default_upper(U, b)
    b = @benchmark $U \ $b

    println("=== benchmark_trisolve_default ===")
    show(
            stdout,
            MIME("text/plain"),
            b )
    println("\n\n")
end

function benchmark_trisolve_rowwise_upper(U, b)
    b = @benchmark linalg.triangular_solve_upper_rowwise($U, $b)

    println("=== benchmark_trisolve_rowwise ===")
    show(
            stdout,
            MIME("text/plain"),
            b )
    println("\n\n")
end

function benchmark_trisolve_colwise_upper(U, b)
    b = @benchmark linalg.triangular_solve_upper_colwise($U, $b)

    println("=== benchmark_trisolve_colwise ===")
    show(
            stdout,
            MIME("text/plain"),
            b )
    println("\n\n")
end

function benchmark_trisolve_upper()
    N = 30_000
    U = UpperTriangular( rand(N, N) )
    b = rand(N)

    benchmark_trisolve_default_upper(U, b)
    benchmark_trisolve_colwise_upper(U, b)
    benchmark_trisolve_rowwise_upper(U, b)

end

function benchmark_trisolve_default_lower(U, b)
    b = @benchmark $U \ $b

    println("=== benchmark_trisolve_default ===")
    show(
            stdout,
            MIME("text/plain"),
            b )
    println("\n\n")
end

function benchmark_trisolve_rowwise_lower(U, b)
    b = @benchmark linalg.triangular_solve_lower_rowwise($U, $b)

    println("=== benchmark_trisolve_rowwise ===")
    show(
            stdout,
            MIME("text/plain"),
            b )
    println("\n\n")
end

function benchmark_trisolve_colwise_lower(U, b)
    b = @benchmark linalg.triangular_solve_lower_colwise($U, $b)

    println("=== benchmark_trisolve_colwise ===")
    show(
            stdout,
            MIME("text/plain"),
            b )
    println("\n\n")
end

function benchmark_trisolve_lower()
    N = 30_000
    U = LowerTriangular( rand(N, N) )
    b = rand(N)

    benchmark_trisolve_default_lower(U, b)
    benchmark_trisolve_colwise_lower(U, b)
    benchmark_trisolve_rowwise_lower(U, b)

end


if basename(pwd()) == "aoc"
    cd("2015/11")
end
include("../../helpers/helpers.jl")

fromdigits(n; base = 10) = foldl((a, b) -> base * a + b, n, init = 0)

alphabet = "abcdefghjkmnpqrstuvwxyz"
function nextpassword(password::AbstractString)
    n = toencoded(password) + 1
    while !isvalidpass(n)
        n += 1
    end
    fromencoded(n)
end

function toencoded(password::AbstractString)::Int
    fromdigits((findfirst(==(c), alphabet::String) - 1 for c ∈ password), base = 23)
end

function fromencoded(passnum::Integer)::String
    join(getindex.(alphabet::String, reverse(digits(passnum, base = 23)) .+ 1))
end

isvalidpass(password::AbstractString) = isvalidpass(toencoded(password))
isvalidpass(passnum::Integer) = isvalidpass(reverse(digits(passnum, base = 23)))
function isvalidpass(passdigits::AbstractVector{<:Integer})::Bool
    length(passdigits) == 8 &&
        any(a + 1 == b && b + 1 == c for (a, b, c) ∈ eachcons(passdigits, 3)) &&
        count(a == b for (a, b) ∈ Iterators.unique(eachcons(passdigits, 2))) > 1
end

# @time nextpassword("cqjxjnds")
#   0.064901 seconds (306.64 k allocations: 37.737 MiB)
# "cqjxxyzz"

# @time nextpassword(ans)
#   0.275939 seconds (1.19 M allocations: 146.090 MiB, 10.42% gc time)
# "cqkaabcc"


using Symbolics

# this symbolic structure corresponds to a symbolic transfer operator
# (L_n f)(x) = \sum_{y\in T^{-1}(x)}\frac{f(y)}{|T'(y)|^n}
struct SymbL
    n
    f
end

@variables x D(x)

∂ = Differential(x)

Diff(P::SymbL) = [SymbL(P.n+1, ∂(P.f)), SymbL(P.n, P.n*P.f*D)]

function Diff(v::Vector{SymbL})
    w = SymbL[]
    for P in v
        append!(w, Diff(P))
    end
    return w
end

function compute_dfly_k_fi_DDi(k::Int)
    @variables x
    @variables f[0:k](x)
    @variables DD[0:k](x)
    ∂ = Differential(x)

    der_dict = Dict([[D => DD[1] ]; [∂(f[i]) => f[i+1] for i in 1:k]; [∂(DD[i]) => DD[i+1] for i in 1:k]])

    P = SymbL(1, f[1]) #Lf
    v = P
    for i in 1:k
        v = Diff(v)
        for k in 1:length(v)
            l = expand_derivatives(v[k].f, true)
            l = substitute(l, der_dict)
            l = simplify(l; expand = true)
            v[k] = SymbL(v[k].n, l)
        end
    end

    return v
end

using SymbolicUtils

function _optimize_mult(k, n, h::SymbolicUtils.Mul, vals)

    @variables x

    @variables f[0:k](x)
    boolf = [(symb in keys(h.dict)) for symb in f]

    @variables DD[0:k](x)
    boolDD = [(symb in keys(h.dict)) for symb in DD]

    # I start with a simple version, where I use the n on
    # the highest derivative of DD, in further versions
    # I will try to optimize over all possibilities

    i = findlast(boolDD)
    pow = h.dict[DD[i]]

    # This uses the computed bounds to compute
    # ||DDᵢ/(T')^n-1||_{∞}*||DD_i||_{∞}^{pow-1}

    bound = vals[i+1, n]*vals[i+1, 1]^(pow-1)

    for j in 1:i-1
        if boolDD[j]!= false
            pow = h.dict[DD[j]]
            bound *= vals[j+1, 1]*pow
        end
    end

    i = findlast(boolf)
    return h.coeff*bound*f[i]

end

function optimize_coefficients(k, v::Vector{SymbL}, vals)
    out = Num(0)
    for x in v
        n = x.n
        #@info "x" x
        if typeof(x.f.val) <: SymbolicUtils.Mul
            y = x.f.val
            val = _optimize_mult(k, n, y, vals)
            out+= _optimize_mult(k, n, x.f.val, vals)
        elseif typeof(x.f.val) <: SymbolicUtils.Add
            for y in keys(x.f.val.dict)
                val = _optimize_mult(k, n, y, vals)
                out+= x.f.val.dict[y]*val
            end
        end
    end
    return out
end

function substitute_values(k, v::Vector{SymbL}, vals)
    λ = vals[1]
    DDs = vals[2:end]
    @variables x
    @variables f[0:k](x)
    @variables DD[0:k](x)

    subsdict = Dict([DD[i]=>DDs[i] for i in 1:k])

    w = zeros(Num, length(v))

    for k in 1:length(v)
        n = v[k].n
        f = v[k].f
        #w[k] = substitute(f, subsdict)
        #w[k] = λ^(n-1)*w[k]
    end
    return w
end


# This code presents an example to illustrate how HyperbolicCrossApprox can be used

using HyperbolicCrossApprox

function test_hyperbolic_cross_approx()

  d  = 5  # Set the number of dimensions
  k = 7  # Set the level of approximation

  g1, m1 = hyperbolic_cross_grid(chebyshev_nodes,d,k)
  g2, m2 = hyperbolic_cross_grid(chebyshev_extrema,d,k)  # Construct the hyperbolic cross grid and the multi index

  # An arbitrary test function

  function test(grid)

    y_value = (grid[:,1].+1).^0.1.*exp.(grid[:,2]).*log.(grid[:,3].+2).^0.2.*(grid[:,4].+2).^0.8.*(grid[:,5].+7).^0.1

    return y_value

  end

  y1 = test(g1)  # Evaluate the test function on the hyperbolic cross grid
  y2 = test(g2)

  point = [0.75, 0.45, 0.82, -0.15, -0.95]  # Choose a point to evaluate the approximated function

  # Chebyshev nodes

  # One way of computing the weights and evaluating the approximated function

  w1       = hyperbolic_cross_weights(y1,g1,m1)    # Compute the hyperbolic cross weights
  y1_hat   = hyperbolic_cross_evaluate(w1,point,m1)  # Evaluate the approximated function

  w2       = hyperbolic_cross_weights(y2,g2,m2)    # Compute the hyperbolic cross weights
  y2_hat   = hyperbolic_cross_evaluate(w2,point,m2)  # Evaluate the approximated function

  #= A second way of computing the weights and evaluating the approximated function that
  computes the interpolation matrix just once. =#

  interp_mat = hyperbolic_cross_inverse_interpolation_matrix(g1,m1)  # Compute the interpolation matrix
  w1b = hyperbolic_cross_weights(y1,interp_mat)             # Compute the hyperbolic cross weights
  y1b_hat = hyperbolic_cross_evaluate(w1b,point,m1)  # Evaluate the approximated function

  # Evaluate the exact function at point

  y_actual = test(point')

  # Now consider the ansiotropic case

  k = 7
  n = [15, 11, 11, 11, 15]
  g3, m3 = hyperbolic_cross_grid(chebyshev_nodes,d,k,n)  # Construct the hyperbolic cross grid and the multi index
  y3 = test(g3)
  w3 = hyperbolic_cross_weights(y3,g3,m3)                   # Compute the hyperbolic cross weights
  y3_hat_ansio = hyperbolic_cross_evaluate(w3,point,m3)       # Evaluate the approximated function
  w3_th = hyperbolic_cross_weights_threaded(y3,g3,m3)                   # Compute the hyperbolic cross weights

  g4, m4 =  hyperbolic_cross_grid(chebyshev_extrema,d,k)
  y4 = test(g4)
  w4 = hyperbolic_cross_weights(y4,g4,m4)
  y4_hat_ansio = hyperbolic_cross_evaluate(w4,point,m4)
  w4_th = hyperbolic_cross_weights_threaded(y4,g4,m4)

  return y_actual, y1_hat, y2_hat, y1b_hat, y3_hat_ansio, y4_hat_ansio

end

test_hyperbolic_cross_approx()


using Flux, Statistics
using Flux.Data: DataLoader
using Flux: onehotbatch, onecold, logitcrossentropy, throttle, @epochs
using Base.Iterators: repeated
using Parameters: @with_kw
using CUDAapi
using MLDatasets
if has_cuda()		# Check if CUDA is available
    @info "CUDA is on"
    import CuArrays		# If CUDA is available, import CuArrays
    CuArrays.allowscalar(false)
end

@with_kw mutable struct Args
    η::Float64 = 3e-4       # learning rate
    batchsize::Int = 1024   # batch size
    epochs::Int = 10        # number of epochs
    device::Function = gpu  # set as gpu, if gpu available
end

function getdata(args)
    # Loading Dataset
    xtrain, ytrain = MLDatasets.MNIST.traindata(Float32)
    xtest, ytest = MLDatasets.MNIST.testdata(Float32)

    # Reshape Data for flatten the each image into linear array
    xtrain = Flux.flatten(xtrain)
    xtest = Flux.flatten(xtest)

    # One-hot-encode the labels
    ytrain, ytest = onehotbatch(ytrain, 0:9), onehotbatch(ytest, 0:9)

    # Batching
    train_data = DataLoader(xtrain, ytrain, batchsize=args.batchsize, shuffle=true)
    test_data = DataLoader(xtest, ytest, batchsize=args.batchsize)

    return train_data, test_data
end

function build_model(; imgsize=(28,28,1), nclasses=10)
    return Chain(
 	    Dense(prod(imgsize), 32, relu),
            Dense(32, nclasses))
end

function loss_all(dataloader, model)
    l = 0f0
    for (x,y) in dataloader
        l += logitcrossentropy(model(x), y)
    end
    l/length(dataloader)
end

function accuracy(data_loader, model)
    acc = 0
    for (x,y) in data_loader
        acc += sum(onecold(cpu(model(x))) .== onecold(cpu(y)))*1 / size(x,2)
    end
    acc/length(data_loader)
end

function train(; kws...)
    # Initializing Model parameters
    args = Args(; kws...)

    # Load Data
    train_data,test_data = getdata(args)

    # Construct model
    m = build_model()
    train_data = args.device.(train_data)
    test_data = args.device.(test_data)
    m = args.device(m)
    loss(x,y) = logitcrossentropy(m(x), y)

    ## Training
    evalcb = () -> @show(loss_all(train_data, m))
    opt = ADAM(args.η)

    @epochs args.epochs Flux.train!(loss, params(m), train_data, opt, cb = evalcb)

    @show accuracy(train_data, m)

    @show accuracy(test_data, m)
end

cd(@__DIR__)
train()


# This is a Julia implementation of Empirical Bayes model selection and estimation
# in sparse high-dimensional Gaussian linear regression models as described in the
# paper:
#    Martin, Mess, and Walker, "Empirical Bayes posterior concentration in sparse
#    high-dimensional linear models," arXiv:1406:7718
#

# We need a lasso solution to start from so use GLMNet.
using GLMNet

# Get the lasso solution from a cross-validation run. Since it is only for starting
# the MH chain in empirical bayes below we only run a 5-fold CV.
function lasso_cv(x, y)
  cv = glmnetcv(x, y)
  cv.path.betas[:, indmin(cv.meanloss)]
end

# Prior for complexity/size of a model.
function dcomplex(x, n, p, a, b, calclog = true)
  o = -x * (log(b) + a * log(p)) + log(float(x .<= n))
  return (calclog ? o : exp(o))
end

# Empirical Bayes model selection and estimation Run Metropolis-Hastings chain to sample
# from marginal posterior.
#
#   S, s, i, beta = empirical_bayes_linreg(x, y; options...)
#
# Inputs:
#  x = predictor variables, n*p, with n samples of p variables
#  y = response variables, n*1, with n samples of response
#
# Outputs:
#
# Options:
#  sigma = error variance, if nothing then it is estimated from data
#  alpha = likelihood fraction (0 < alpha < 1)
#  gamma = conditional prior parameter (0 <= gamma, should be close to 0)
#  numSamples = number of Monte Carlo samples
#  burnIn = fraction of additional burn in samples taken first but not saved
#  sampleCoefs = true iff samples of the coefficients should be returned
#  logF = function to calc log of prior for the model size
#
function empirical_bayes_linreg(X, y;
  sigma2 = nothing,
  alpha = 0.999,
  gamma = 0.001,
  numSamples = 10000,
  burnIn = 0.25,
  logF = dcomplex
)

  # Check inputs and prep.
  n, p = size(X)
  @assert size(y, 1) == n
  @assert size(y, 2) == 1

  numBurnin = iround(burnIn * numSamples)

  # We will start the chain from the cross-validated Lasso solution so get that.
  lasso_coefs = lasso_cv(X, y)

  if sigma2 == nothing
    z = y - X * lasso_coefs
    sigma2 = sum(z .^ 2) / max(1, n - sum(S))
  end

  twosigma2 = 2 * sigma2
  v = gamma + alpha / sigma2
  logvhalf = log(v) / 2

  # We will use an array of active coefficients to represent a model, S (selected).
  S = find(lasso_coefs .!= 0.0)

  # Get the log priors so we need not recalc them all the time
  logpriors = map((len) -> logF(len) - lchoose(p, len), 0:n)

  # Save the models and which model was used at each step
  models = Any[]        # Each model is unique here and saved as (model, length, betahat, sse, U)
  modelhash = Dict{Vector{Int64}, Int64}() # Map from a model to its index in models array
  sampledmodels = zeros(Int64, numSamples) # Index of model sampled in each step of the chain

  addmodel(model) = begin
    if haskey(modelhash, model)
      entry = models[modelhash[model]]
    else
      # Do linear regression on predictor variables selected by the model
      betas, residuals, sse = linreg_for_subset_of_columns(X, y, model)

      # Create entry for models array
      i = 1 + length(models) # will be at next free index
      modelhash[model] = i
      s = length(model)
      logposterior = logpriors[s] - alpha * sse / twosigma2 - s * logvhalf
      entry = (i, s, model, betas, sse, logposterior)

      # Save entry in models array
      push!(models, entry)
    end
    return entry
  end

  # Add our first model, the one from the Lasso.
  i, s, S, coefs, sse, logposterior = addmodel(S)

  # Now take the Monte Carlo samples
  for j in 1:(numBurnin + numSamples)

    # Get a new model proposal based on random sampling from current model
    Snew = random_proposal_model(S, n, p)

    # Add the model after calculating its implications/variables.
    inew, snew, Snew, coefs, sse, logposteriornew = addmodel(Snew)

    println("model ", j, ": ", snew, ", ", sse, ", ", logposteriornew, "\n")

    # Metropolis-Hastings accept step: Randomly accept proposal dependent on
    # its posterior compared to the posterior of the current model.
    if rand() <= exp(logposteriornew - logposterior)
      i = inew
      S = Snew
      logposterior = logposteriornew
    end

    # Save sample if we are past the burnin
    if j > numBurnin
      sampledmodels[j - numBurnin] = i
    end

  end

  return models, sampledmodels

end

bigfloat_to_float(bf, digits = 7) = int(bf*(10^digits))/(10^digits)
lchoose(n, m) = bigfloat_to_float(log(binomial(BigInt(n), BigInt(m))))

function linreg_for_subset_of_columns(X, y, model)
  xselected = X[:, model]
  betas = linreg(xselected, y)
  residuals = y .- (betas[1] + xselected * betas[2:end])
  sse = sum( residuals.^2 )
  return betas, residuals, sse
end

# Helper functions for proposal function
delete_at(a, i) = [a[1:(i-1)], a[(i+1):end]]
sample(ary) = ary[rand(1:length(ary))]
add_new_from_range(a, maxval, minval = 1) = push!(copy(a), sample(collect(setdiff(Set(minval:maxval), Set(a)))))

# Proposal function that randomly samples a new model close to given one.
function random_proposal_model(model, maxlen, numvars)
  s = length(model)
  if s == maxlen # If we are at the max length we can only take away variables
    return delete_at(model, rand(1:s))
  elseif s == 1 # If we are at length 1 we can only add
    return sort(add_new_from_range(model, numvars))
  else
    if rand() <= 0.5
      return delete_at(model, rand(1:s))
    else
      return sort(add_new_from_range(model, numvars))
    end
  end
end

# To sample coefficients given a model with a restricted set of
# predictor variables we must first calculate the U as defined
# in the paper.
function calc_U(xselected, model)
  xtxselected = xselected' * xselected
  V = pinv(xtxselected)
  U = chol(V)
  return U
end

sample_coefficients(s, U, sqrtv, betas) = betas + sqrtv * U * randn(s)


# PROJECT_SOC    Project a point onto the second-order cone.
#
#   Suppose v is an (n+1)-dimensional vector, so v = [t;v0], where t is a
#   scalar and v0 is n-dimensional. Then project_soc(v) is the projection of
#   (t,v0) onto the second-order cone; the result is also (n+1)-dimensional.

function project_soc(v)
    # assume v = (t, v0)
    nv = norm(v[2:end])
    x  = nan(size(v))

    if nv <= -v[1]
        x = zeros(size(v))
    elseif nv <= v[1]
        x = v
    else
        r = 0.5 * (1 + v[1] / nv)
        x[1] = r * nv
        x[2:end] = r * v[2:end]
    end

    return x
end


@test simplify(:x/:x,Div)==1

E=@equ E=sqrt(p^2*c^2+m^2*c^4)
vars=@equs p=sqrt(2)*1e6/c m=0.5e6/c^2
r=E&vars[2]
@test r.rhs==Equations.Sqrt(:p*:p*:c*:c+2.5e11)


function filter_coeffs(a::Vector{Float64}, b::Vector{Float64}, orders::Vector{Int64}, data::Array{Float64})

    dinv = (b - a)./(orders.-1)
    d = length(a)
    if d == 1
        coeffs = filter_coeffs_1d(dinv,data)
    elseif d == 2
        coeffs = filter_coeffs_2d(dinv,data)
    elseif d == 3
        coeffs = filter_coeffs_3d(dinv,data)
    elseif d == 4
        coeffs = filter_coeffs_4d(dinv,data)
    else
        error()
    end
    return coeffs
end

function ff(a::Vector{Float64}, b::Vector{Float64}, orders::Vector{Int64}, data::Array{Float64})

    dinv = (b - a)./(orders.-1)
    d = length(a)
    if d == 1
        coeffs = filter_coeffs_1d(dinv,data)
    elseif d == 2
        coeffs = filter_coeffs_2d(dinv,data)
    elseif d == 3
        coeffs = filter_coeffs_3d(dinv,data)
    elseif d == 4
        coeffs = filter_coeffs_4d(dinv,data)
    else
        error()
    end
    return coeffs
end

function find_coefs_1d(delta_inv, M, data)

    # args: double, int, double[:], double[:

    N = M + 2

    data = data[:]

    if length(data) == N
        rhs = data
    else
        rhs = [0; data; 0]
    end

    basis = [1.0/6.0, 2.0/3.0, 1.0/6.0]

    vals = repeat( basis, outer=[M])
    co_x = repeat( collect(2:M+1), inner=[3])
    co_y = repeat( collect(1:3) , outer=[M]) + co_x .- 2

    db = 4
    initial = [1.0,-2.0,1.0,0.0]*delta_inv*delta_inv
    final = [0.0,1.0,-2.0,1.0]*delta_inv*delta_inv

    vals = [initial; vals; final]
    co_x = [ones(Int,db); co_x; ones(Int,db)*(M+2)]
    co_y = [1:db; co_y; M+3-db:M+2]

    spmat = sparse(co_x, co_y, vals)

    sol = spmat \ rhs

    return sol

end

function filter_coeffs_1d(dinv, data)

  M = size(data,1)
  N = M+2

  coefs = find_coefs_1d(dinv[1], M, data)

  return coefs

end

function filter_coeffs_2d(dinv, data)

    Mx = size(data,1)
    My = size(data,2)

    Nx = Mx+2
    Ny = My+2

    coefs = zeros(Nx,Ny)

    # First, solve in the X-direction
    for iy in 1:My
        coefs[:,iy+1] = find_coefs_1d(dinv[1], Mx, data[:,iy])
    end

    # Now, solve in the Y-direction
    for ix in 1:Nx
        coefs[ix,:] = find_coefs_1d(dinv[2], My, coefs[ix,:])
    end

    return coefs

end

function filter_coeffs_3d(dinv, data)

    Mx = size(data,1)
    My = size(data,2)
    Mz = size(data,3)

    Nx = Mx+2
    Ny = My+2
    Nz = Mz+2

    coefs = zeros(Nx,Ny,Nz)

    # First, solve in the X-direction
    for iy in 1:My
        for iz in 1:Mz
            coefs[:,iy+1,iz+1] = find_coefs_1d(dinv[1], Mx, data[:,iy,iz])
        end
    end

    # Now, solve in the Y-direction
    for ix in 1:Nx
        for iz in 1:Mz
            coefs[ix,:,iz+1] = find_coefs_1d(dinv[2], My, coefs[ix,:,iz+1])
        end
    end

    # Now, solve in the Z-direction
    for ix in 1:Nx
        for iy in 1:Ny
            coefs[ix,iy,:] = find_coefs_1d(dinv[3], Mz, coefs[ix,iy,:])
        end
    end

    return coefs

end

function filter_coeffs_4d(dinv, data)

    Mx = size(data,1)
    My = size(data,2)
    Mz = size(data,3)
    Mz4 = size(data,4)

    Nx = Mx+2
    Ny = My+2
    Nz = Mz+2
    Nz4 = Mz4+2

    coefs = zeros(Nx,Ny,Nz,Nz4)

    # First, solve in the X-direction
    for iy in 1:My
        for iz in 1:Mz
            for iz4 in 1:Mz4
                coefs[:,iy+1,iz+1,iz4+1] = find_coefs_1d(dinv[1], Mx, data[:,iy,iz,iz4])
            end
        end
    end

    # Now, solve in the Y-direction
    for ix in 1:Nx
        for iz in 1:Mz
            for iz4 in 1:Mz4
                coefs[ix,:,iz+1,iz4+1] = find_coefs_1d(dinv[2], My, coefs[ix,:,iz+1,iz4+1])
            end
        end
    end

    # Now, solve in the Z-direction
    for ix in 1:Nx
        for iy in 1:Ny
            for iz4 in 1:Mz4
                coefs[ix,iy,:,iz4+1] = find_coefs_1d(dinv[3], Mz, coefs[ix,iy,:,iz4+1])
            end
        end
    end
    # Now, solve in the Z4-direction
    for ix in 1:Nx
        for iy in 1:Ny
            for iz in 1:Mz
                coefs[ix,iy,iz,:] = find_coefs_1d(dinv[4], Mz, coefs[ix,iy,iz,:])
            end
        end
    end

    return coefs

end


function getTableauSLRK(s, o, tsym, q, p, q̃, p̃, ω, d=Nothing)
    TableauSLRK(tsym, o, s, q, p, q̃, p̃, ω, d)
end

function getTableauSLRKLobIII(s, q, p)
    o = 2s-2
    tsym = Symbol("SLRKLobIII")
    getTableauSLRK(s, o, tsym, q, p, q, p, get_lobatto_ω_matrix(s), get_lobatto_d_vector(s))
end

function getTableauSLRKLobIIIAB(s)
    q = getCoefficientsLobIIIA(s)
    p = getCoefficientsLobIIIB(s)
    getTableauSLRKLobIII(s, q, p)
end

function getTableauSLRKLobIIIC(s)
    q = getCoefficientsLobIII(s)
    p = getCoefficientsLobIIIC(s)
    getTableauSLRKLobIII(s, q, p)
end

function getTableauSLRKLobIIID(s)
    l = getCoefficientsLobIIID(s)
    getTableauSLRKLobIII(s, l, l)
end

function getTableauSLRKLobIIIE(s)
    l = getCoefficientsLobIIIE(s)
    getTableauSLRKLobIII(s, l, l)
end


# based on http://int8.io/automatic-differentiation-machine-learning-julia/

using MNIST # if not installed try Pkg.add("MNIST")
using AutoDiffSource # if not installed try Pkg.add("AutoDiffSource")
using PyPlot # if not installed try Pkg.add("PyPlot")

sigmoid(x) = 1 / (1 + exp(x))

function autoencoder(We1, We2, Wd, b1, b2, input)
    firstLayer = sigmoid.(We1 * input + b1)
    encodedInput = sigmoid.(We2 * firstLayer + b2)
    reconstructedInput = sigmoid.(Wd * encodedInput)
    return reconstructedInput
end

@δ function autoencoderError(We1, We2, Wd, b1, b2, input)
    reconstructedInput = autoencoder(We1, We2, Wd, b1, b2, input)
    return sum((input - reconstructedInput).^2)
end

@assert checkdiff(autoencoderError, δautoencoderError, randn(3,3), randn(3,3), rand(3,3), randn(3), randn(3), randn(3))

function initializeNetworkParams(inputSize, layer1Size, layer2Size)
    We1 =  1f-1 * randn(Float32, layer1Size, inputSize)
    b1 = zeros(Float32, layer1Size)
    We2 =  1f-1 * randn(Float32, layer2Size, layer1Size)
    b2 = zeros(Float32, layer2Size)
    Wd = 1f-1 * randn(Float32, inputSize, layer2Size)
    return (We1, We2, b1, b2, Wd)
end

function show_digits(testing, We1, We2, b1, b2, Wd)
    clf()
    total_error = 0
    for l = 1:12
        input = testing[:, rand(1:size(testing, 2))]
        reconstructedInput = autoencoder(We1, We2, Wd, b1, b2, input)
        subplot(4, 6, l*2-1)
        pcolor(rotl90(reshape(input, 28, 28)'); cmap="Greys")
        subplot(4, 6, l*2)
        pcolor(rotl90(reshape(reconstructedInput, 28, 28)'); cmap="Greys")
        total_error += sum((input - reconstructedInput).^2)
    end
    tight_layout()
    total_error / 12
end

function trainAutoencoder(epochs, training, testing, We1, We2, b1, b2, Wd, alpha)
    for k in 1:epochs
        total_error = 0.
        for i in 1:size(training, 2)
            input = training[:,i]
            val, ∇autoencoderError = δautoencoderError_const6(We1, We2, Wd, b1, b2, input)
            total_error += val
            if mod(i, 10_000) == 0
                test_error = show_digits(testing, We1, We2, b1, b2, Wd)
                @printf("epoch=%d iter=%d train_error=%.2f test_error=%.2f\n", k, i, total_error/10_000, test_error)
                total_error = 0.
            end
            ∂We1, ∂We2, ∂Wd, ∂b1, ∂b2 = ∇autoencoderError()
            We1 -= alpha * ∂We1
            We2 -= alpha * ∂We2
            Wd  -= alpha * ∂Wd
            b1  -= alpha * ∂b1
            b2  -= alpha * ∂b2
        end
    end
    return (We1, We2, b1, b2, Wd)
end

# read input MNIST data
training = convert(Matrix{Float32}, MNIST.traindata()[1] / 255)
testing = convert(Matrix{Float32}, MNIST.testdata()[1] / 255)

# 784 -> 300 -> 100 -> 784 with weights normally distributed (with small variance)
We1, We2, b1, b2, Wd = initializeNetworkParams(784, 300, 100)

# 4 epochs with alpha = 0.02
@time We1, We2, b1, b2, Wd = trainAutoencoder(4, training, testing, We1, We2, b1, b2, Wd, 2f-2)


using Plots, DelimitedFiles
using ProgressMeter
using Random, Distributions, StatsBase
Random.seed!(21);

mutable struct ParticleGroup
    locations;
    weights;
end

struct NoiseDists
    actual;
    estimated;
    perturb;
end

function elevAboveSeaLevel(x)
    sin_term = sin(0.01 * x + 3);
    cos_term = cos((0.01 * x - 2) * (0.005 * x - 5));
    prefix = (100000 * x) / (1 + (x + 500) ^ 2);
    elev = prefix * (cos_term - sin_term);
    return elev;
end

function get_measurement(x, distribution)
    msmt = elevAboveSeaLevel(x);
    msmt += rand(distribution);
    return msmt;
end

function get_likelihood(x, msmt, distribution)
    ytrue = elevAboveSeaLevel(x);
    residual = msmt - ytrue;
    prob = pdf(distribution, residual);
    return prob;
end

function init_particles(x0, x1, Np)
    scale = rand(Uniform(), Np);
    locations = x0 .+ (x1 - x0) .* scale;
    return ParticleGroup(locations, ones(Np));
end

function init_histories(Nt, Np)
    xtrue = zeros(Nt);
    x = zeros(Nt, Np);
    return (xtrue, x);
end

function update_weights!(particles, xtrue, noise)
    msmt = get_measurement(xtrue, noise.actual);
    particles.weights = get_likelihood.(
        particles.locations,
        msmt,
        noise.estimated
    );
end

function update_locations!(particles)
    wsample!(
        particles.locations,
        particles.weights,
        particles.locations,
        replace=true
    );
end

function move!(particles, noise, dt, velocity)
    particles.locations .+= dt * velocity;
    particles.locations .+= rand.(noise.perturb);
end

function forward!(particles, xtrue, noise, dt, velocity)
    update_weights!(particles, xtrue, noise);
    update_locations!(particles);
    move!(particles, noise, dt, velocity);
end

function run_filter(x0, x1, xtrue, Np, Nt, dt, velocity, σ_true, σ_est, σ_pert)
    particles = init_particles(x0, x1, Np);
    xtrue_history, x_history = init_histories(Nt, Np);
    noise = NoiseDists(map((σ) -> Normal(0, σ), [σ_true, σ_est, σ_pert])...);
    @showprogress 1 "Running pfilter..." for t = 1 : Nt
        forward!(particles, xtrue, noise, dt, velocity);
        xtrue += dt * velocity;
        xtrue_history[t] = xtrue;
        x_history[t, :] = particles.locations;
    end
    return (xtrue_history, x_history);
end

function make_marker(fpath, scale=5)
    verts = scale .* readdlm(fpath);
    x, y = verts[:, 1], verts[:, 2];
    return [Shape(x, y)];
end

function make_mountain(x0, x1)
    x = collect(x0 : 1 : x1);
    y = elevAboveSeaLevel.(x);
    return (x, y);
end

function make_animation(x, y, x0, y0, x1, y1, x_history, xtrue_history, aeroplane, Nt)
    p = Progress(Nt, 1, "Generating animation...")
    anim = @animate for t = 1 : Nt
        xest_t = x_history[t, :];
        yest_t = elevAboveSeaLevel.(xest_t);
        plot(x, y, xlims=(x0, x1), ylims=(y0, y1), legend=false);
        scatter!(xest_t, yest_t, markersize=1, color="darkblue");
        scatter!([xtrue_history[t] - 50], [y1 - 50], shape=aeroplane, ms=20);
        next!(p)
    end
    return anim;
end

function main()
    Nt, Np = 200, 5000;
    dt, velocity = 0.01, 200;
    x0, x1, xtrue = 0, 1000, 100;
    σ_true, σ_est, σ_pert  = 50, 40, 0.1 * dt * velocity;

    args = (x0, x1, xtrue, Np, Nt, dt, velocity, σ_true, σ_est, σ_pert)
    xtrue_history, x_history = run_filter(args...);

    x, y = make_mountain(x0, x1);
    y0, y1 = quantile(y[:], [0, 1]) .+ [-10, +75];
    aeroplane = make_marker("../data/airplane_verts.txt")

    anim = make_animation(x, y, x0, y0, x1, y1, x_history, xtrue_history, aeroplane, Nt);
    gif(anim, "../data/demo.gif", fps=20);
end

main();


_interpolate_helper = (x0, y0, x1, y1, x) -> ( y0 * (x1 - x) + y1 * (x - x0) ) / (x1 - x0)

function interpolate(x0, y0, x1; left_copy::Bool=false, right_copy::Bool=false)

    # assuming x0, x1 are both monotonically increasing

    idx0 = 1  # old coordinate
    idx1 = 1  # new coordinate

    N0 = length(x0)

    y1 = copy(x1)
    N1 = length(x1)

    while true

        if idx1 > N1
            break
        end

        lx = x0[idx0  ]
        ly = y0[idx0  ]

        rx = x0[idx0+1]
        ry = y0[idx0+1]

        ix = x1[idx1]

        if (idx0 == 1) && (ix < lx)

            y1[idx1] = (left_copy) ? ly : NaN
            idx1 += 1

        elseif (idx0 == N0-1) && (ix > rx)

            y1[idx1] = (right_copy) ? ry : NaN
            idx1 += 1

        elseif lx <= ix <= rx

            y1[idx1] = _interpolate_helper(lx, ly, rx, ry, ix)
            idx1 += 1

        else

            idx0 += 1

        end

    end

    return y1
end


using IntervalWavelets
using Test

@testset "Scaling function basis" begin
    @testset "Non-default reconstruction intervals" begin
        J = 3
        R = 3
        l = DyadicRational(-1, 1)
        r = DyadicRational(1, 1)
        B = interval_scaling_function_basis(2, J, R; left_boundary = l, right_boundary = r)

        x = reconstruct(B, ones(2^J))[1]

        # This only works because the interval has length 1
        @test x == all_dyadic_rationals(l, r, R)
    end

    @testset "Support of basis function" begin
        l = DyadicRational(-1, 1)
        r = DyadicRational(1, 0)
        B = interval_scaling_function_basis(2, 3, 4; left_boundary = l, right_boundary = r)

        # Left boundary functions
        @test B[1][1][1] == l
        @test B[2][1][1] == l

        # Right boundary functions
        @test B[7][1][end] == r
        @test B[8][1][end] == r
    end

    @testset "Reconstruct from unit vectors" begin
        p = 2
        J = 3
        R = 4
        B = interval_scaling_function_basis(p, J, R)

        x = all_dyadic_rationals(B.left_boundary, B.right_boundary, R)
        L = boundary_scaling_functions(LEFT, p, R)
        for k in 1:2
            coef = zeros(2^J)
            coef[k] = 1

            y = 2.0^(J/2) * L[k - 1].(2^J * x)
            xx, z = reconstruct(B, coef)

            @test x == xx
            @test y ≈ z atol = 1e-8
        end

        phi = interior_scaling_function(interior_filter(p), R)
        for k in 3:6
            coef = zeros(2^J)
            coef[k] = 1

            translation = k - 1
            y = 2.0^(J/2) * phi.(2^J * x .- translation)
            xx, z = reconstruct(B, coef)

            @test x == xx
            @test y ≈ z atol = 1e-8
        end

        right = boundary_scaling_functions(RIGHT, p, R)
        for k in 7:8
            coef = zeros(2^J)
            coef[k] = 1

            y = 2.0^(J/2) * right[8 - k].(2^J * (x .- B.right_boundary))
            xx, z = reconstruct(B, coef)

            @test x == xx
            @test y ≈ z atol = 1e-8
        end
    end

    @testset "Reconstruct constant function 1D" begin
        B = interval_scaling_function_basis(2, 3, 4)

        # Coefficients found by solving [ B[j](x_i) ] * coef = ones()
        coef = [
            0.128004734,
            0.354064418,
            0.353553390,
            0.353553390,
            0.353553390,
            0.353553390,
            0.385317706,
            0.458021222
        ]

        y = reconstruct(B, coef)[2]

        @test y ≈ ones(17) atol = 1e-8
    end

    @testset "Reconstruct constant function 2D" begin
        B = interval_scaling_function_basis(2, 3, 4)

        # Coefficients reused from 1D
        coef1 = [
            0.128004734,
            0.354064418,
            0.353553390,
            0.353553390,
            0.353553390,
            0.353553390,
            0.385317706,
            0.458021222
        ]
        coef = coef1 * coef1'

        y = reconstruct(B, coef)

        @test y ≈ ones(17, 17) atol = 1e-7
    end
end


using FEMSparse, SparseArrays, Test

K = sparse(Float64[1 0 1 1;
                   0 1 0 1;
                   1 0 1 0;
                   1 1 0 1];)

f = zeros(4)
fill!(K, 0.0)

dofs1 = [1, 3]
dofs2 = [2, 4]
dofs3 = [1, 4]
Ke = ones(2, 2)
fe = ones(2)
assembler = FEMSparse.start_assemble(K, f)
for dofs in (dofs1, dofs2, dofs3)
    FEMSparse.assemble_local!(assembler, dofs, Ke, fe)
end

@test Matrix(K) ≈  [2.0  0.0  1.0  1.0;
                    0.0  1.0  0.0  1.0;
                    1.0  0.0  1.0  0.0;
                    1.0  1.0  0.0  2.0]

@test f == [2.0, 1.0, 1.0, 2.0]


using InfinitesimalGenerators

# all these examples are generators with only real eigenvalues so I should choose better ones

##  Ornstein–Uhlenbeck
κx = 0.1
σ = 0.02
x = range(- 10 * sqrt(σ^2 /(2 * κx)), stop = 10 * sqrt(σ^2 /(2 * κx)), length = 1000)
μx = -κx .* x
σx = σ .* ones(length(x))

## stationnary distribution
@time g = stationary_distribution(DiffusionProcess(x, μx, σx))
#   0.002711 seconds (248 allocations: 713.797 KiB)

## Feynman-Kac
ψ = x.^2
t = range(0, stop = 1000, step = 1/10)
@time u = feynman_kac(generator(DiffusionProcess(x, μx, σx)); t = t, ψ = ψ)[:, end]
#   0.019786 seconds (3.09 k allocations: 31.120 MiB, 14.88% gc time)
g'u ≈ g'ψ

## test left and right eigenvector
κx = 0.1
σ = 0.02
x = range(- 3 * sqrt(σ^2 /(2 * κx)), stop = 3 * sqrt(σ^2 /(2 * κx)), length = 500)
μx = -κx .* x
σx = σ .* ones(length(x))
μM = -0.01 .+ x
σM = 0.1 .* ones(length(x))
ρ = 1.0
M = AdditiveFunctionalDiffusion(DiffusionProcess(x, μx, σx), μM, σM; ρ = ρ)
@time ζ = tail_index(M)
#  0.22s
@time l, η, r = cgf(M, eigenvector = :both)(ζ)
#  0.06s


using ArbNumerics, Plots
prec = setprecision(BigFloat, 256)

#a = BigFloat(0.506073569036822351319599371053047956980141736828203749380990114218225638827)
#b = BigFloat(0.02328852830307032054478158044023918735669943648088852646123182739831022528)
#c = BigFloat(0.121205692738975111744666848150620569782497212127938371936404761693002104361)

#function T(x)
#    if x <= (0.3)
#        return (a+cbrt((x-(1/8))))*exp(-x) + b
#    end
#    return c*(10*x*exp((-10*x)/3))^19 + b
#end
##
#epsilonc = BigFloat(0.7)
#tauc = BigFloat(0.8)

#function T(x)
#    t = x-(epsilonc /(2*pi))*sin(2*pi*x)+tauc
#    if t < 1
#        return t
#    else
#        return t-1
#    end
#end

a = BigFloat(3.93)
function T(x)
   t = a*x*(1-x)
    if t < 1
        return t
    else
        return t -1
    end
end

T(0.2)
#typeof(T(0.2))
##

Size = 256
noise_size = 0.05

P, gamma = NoiseFourier.Fourier1D.assemble_matrix(T, Size; x_0 = 0, x_1 = 1, T = BigFloat)
##
gamma

##
D = NoiseFourier.Fourier1D.noise_matrix(noise_size, Size)
M = D*P
typeof(M)
##
maxco = 0
for i in 1:Size
    if D[i,i]> maxco
        maxco = D[i,i]
    end
end

function Gamma(sigma,n)
    setprecision(256)
    return BigFloat((exp((-n^2*sigma^2)/2))/(sigma*n*sqrt(2*pi)))
end

#Approximation Error
gamma1 = gamma * maxco + Size*Gamma(noise_size, 2048)
typeof(gamma1)
##

#Discretization Error
prec = setprecision(BigFloat, 64)
P1, asd = NoiseFourier.Fourier1D.assemble_matrix(T, Size; x_0 = 0, x_1 = 1, T = Float64)
##
D1 = NoiseFourier.Fourier1D.noise_matrix(noise_size, Size)
M1 = D1*P1
v2, vinf = NoiseFourier.Fourier1D.rigorous_norm(M1, k=500)
##
#print(asd)
print(v2)
##

function discr_error(v)
    setprecision(256)
    Ci = BigFloat(0.0)
    t = size(v)[1]
    for i in 1:t-1
        Ci += v[i]
    end
    return (1/(1-v[t])) * Ci * (1 + 1/(noise_size*sqrt(2*pi)) + Gamma(noise_size,Size)) * Gamma(noise_size, Size)
end

##
discr_error(v2)
##

#Power method
prec = setprecision(BigFloat, 256)
v = zeros(2*Size+1)
v[1] = BigFloat(1.0)
for i in 1:1000
    @info(i)
    v = M * v
    v /= norm(v,2)
end
lambda = abs(adjoint(v)*M*v)
##
norm(M*v -v,2)
##
#Eigen error
epsilon = BigFloat(norm(M*v -v,2))
##
eigenerr = epsilon + gamma1/2

##
#Plot
function idx_to_freq(j,k)
    # k is the size of the matrix
    if j>=0 && j <=k ÷ 2+1
        return j-1
    end
    if j >k ÷ 2+1
        return j-k-1
    end
end

function measure(x,v)
    t = real(v[1])
    for i in 2:Size
        a = 2*real(v[i])
        b = 2*imag(v[i])
        k = idx_to_freq(i,Size)
        t += a*cos(2*pi*x*k) - b*sin(2*pi*x*k)
    end
    return t
end

p = 50000
x = range(0,1, length = p)
y = zeros(p)
for i in 1:p
    y[i] = measure(x[i],v)
end
plot(x,y,ylims=(0,5),fmt = :png)#,xticks=25:5:75)

##
#Total error
#err = discr_error(v2) + gamma1 + eigenerr
##
print(gamma1)

##
print(discr_error(v2))
##
print(eigenerr)


function fermat(phifunc_xy, forcefunc_xy, rs, tgα, N=100; reltol=1e-21, abstol=1e-21, ext=1.0)
    const C2 = C*C
    function f(x,yvec)
        (y, v) = yvec
        y_prime = v
        v_prime = 0.0

        phi = phifunc_xy(x,y)/C/C
        dphidx, dphidy = forcefunc_xy(x,y)
        dphidx /= C2
        dphidy /= C2
        v2 = v*v

        v_prime = 2*(dphidx*(v2*v+v)-dphidy*(v2+1)^2) / (1-2*phi)
        [y_prime, v_prime]
    end
    _x = linspace(0.0,1,div(N,2)).^2
    _l = [-reverse(_x)*rs ; _x*rs.*ext]
    x, u = ode45(f, [0.0; tgα], _l; reltol=reltol, abstol=abstol)
    y = map(u -> u[1], u);
    dy = map(u -> u[2], u);
    x,y
end

function fermat_y(forcefunc_y, rs, tgα, N=100; reltol=1e-21, abstol=1e-21, ext=1.0)
    const C2 = C*C
    function f(x,yvec)
        (y, v) = yvec
        y_prime = v
        v_prime = 0.0

        v_prime = -2*forcefunc_y(x,y)/C2
        [y_prime, v_prime]
    end
    _x = linspace(0.0,1,div(N,2)).^2
    _l = [-reverse(_x)*rs ; _x*rs.*ext]
    x, u = ode45(f, [0.0; tgα], _l; reltol=reltol, abstol=abstol)
    y = map(u -> u[1], u);
    dy = map(u -> u[2], u);
    x,y
end


function lbinom(n::T, s::T) where T <: Integer
    if n < 60
        log(binomial(n,s))
    else
        logabsbinomial(n,s)[1]
    end
end

# A simple implementation of a DFT to avoid introducing a dependency
# on an external FFT package just for this one distribution
# adapted from Distributions.jl
function _dft!(y::Vector{T}, x::Vector{T}) where T
    n = length(x)
    y .= zero(T)
    @inbounds for j = 0:n-1, k = 0:n-1
        y[k+1] += x[j+1] * cis(-π * T(2 * mod(j * k, n)) / n)
    end
end

#adopted from DSP.jl
function _filt!(out::AbstractArray{T,1}, b::AbstractArray{T,1}, x::AbstractArray{T,1}, si::AbstractArray{T,1}) where T <: Real
    silen = length(si)
    @inbounds for i in 1:length(x)
        xi = x[i]
        val = muladd(xi, b[1], si[1])
        for j in 1:(silen-1)
            si[j] = muladd(xi, b[j+1], si[j+1])
        end
        si[silen] = b[silen+1]*xi
        out[i] = val
    end
end

function _filt_reg!(out::AbstractArray{T,1}, b::AbstractArray{T,1}, x::AbstractArray{T,1}, si::AbstractArray{T,1}, k1::D, k2::D) where {T <: Real, D <: Integer}
    silen = length(si)
    @inbounds for j in 1:(silen-1)
        adj1 = exp(lbinom(k1,j))
        si[j] = x[1]*b[j+1]*adj1
    end
    adj1 = exp(lbinom(k1,silen))
    si[silen] = b[silen+1]*x[1]*adj1
    out[1] = one(T)

    @inbounds for i in 2:length(x)
        xi = x[i]
        adj1 = exp(lbinom(k2,i-1)-lbinom(k1+k2,i-1))
        adj2 = (i-1) / (k1+k2-i+2)
        val = muladd(xi, b[1]*adj1, si[1]*adj2)
        for j in 1:(silen-1)
            adj1 = exp(lbinom(k1,j)+lbinom(k2,i-1)-lbinom(k1+k2,i-1))
            si[j] = muladd(xi, b[j+1]*adj1, si[j+1]*adj2)
        end
        adj1 = exp(lbinom(k1,silen)+lbinom(k2,i-1)-lbinom(k1+k2,i-1))
        si[silen] = b[silen+1]*xi*adj1
        out[i] = val
    end
end


# want:
# - truew -- true width of Rchannel in m
#
# have
# - ppm: pix per m at surface
#

"""
    pixel2meter(pixels, exi::ExpImgs)

This gives the pixel to meter conversion for the plane in which the
R-channel lies.
"""
function pixel2meter(pixels, exi::ExpImgs)
    r1 = pixels/pixels_per_m(exi)
    r1_to_R(r1, exi)
end

pixels_per_m(exi::ExpImgs) = exi.p2m.ppm/exi.thin_num

"""
    r1_to_R(r1, p2m::Pixel2Meter)
    r1_to_R(r1, exi::ExpImgs)

Transforms a distance as measured at the surface of the block into a
distance inside the ice block.

Notes:
- see Yuri's thesis
- green is about 500nm wavelength
- water refractive index for green at 0C is about 1.339
  http://optics.sgu.ru/_media/optics/staff/bashkatov/bashkatov_spie_03_5068_393.pdf
- perspex is about 1.48
"""
function r1_to_R(r1, exi::ExpImgs)
    p2m = exi.p2m
    r1_to_R(r1, p2m.h1, p2m.h2, p2m.h3, p2m.n1, p2m.n2, p2m.n3)
end
function r1_to_R(r1, h1, h2, h3, n1=1.0, n2=[1.339,1.48][1], n3=1.31)
    α = atan(r1/h1)
    β = asin(sin(α)*n1/n2)
    γ = asin(n2/n3*sin(β))
    Δr2 = tan(β)*h2
    r3 = (tan(γ)*h3 + r1 + Δr2) / (1+(tan(γ))^2)
    return r3/cos(γ)
end


using LinearAlgebra

"""
This file contains a list of cost functions that are ready to be used in optimisations
I'm going to try and keep some standard inputs that make sense

Needs citation from the Schuster lab paper
"""

"""
Density matrix fidelity
"""
function C1(KT, KN)
    D = size(KT)[1]
    1 - abs2(tr(KT' * KN) / D)
    # 1 - real(tr(KT' * KN))/D
end

"""
Target-state infidelity
"""
function C2(ψT, ψN)
    1 - abs2(ψT' * ψN)
end

"""
Control amplitudes
"""
function C3(u)
    sum(abs2.(u))
end

"""
Control variations
"""
function C4(u)
    (K, J) = size(u)
    sum(abs2.(diff(u, dims = 2)))
end

"""
Occupation of forbidden states
"""
function C5(ψF, ψj)
    # since ψj is the intermediate states we can assume its an array
    sum(abs2(ψF' * ψ) for ψ in ψj)
end

"""
Evolution time (target gate)
"""
function C6(KT, KJ, N, D)
    1 - 1 / N * sum(abs2(tr(KT' * Kj) / D) for Kj in KJ)
end

"""
Evolution time (target state)
"""
function C7(ψT, ψJ, N)
    1 - 1 / N * sum(abs2(tr(ψT * ψj)) for ψj in ψJ)
end

"""
Stores information, relative weights and functions to compute the penalty functions
"""
struct PenaltyFunctionals
    weights::Any # relative weights of each
    functions::Any # array of penalty functionals
end

"""
Cost functions for the GRAPE algorithm
"""

"""
Unitary synthesis figure of merit, from Khaneja et. al. paper (proper citation needed)
"""
function fom_func(system_type::UnitaryGate, t, k, state, costate, props)::Float64
    @views rael(tr(costate[t, k]' * state[t, k]) * tr(state[t, k]' * costate[t, k]))
end

function fom_func(
    system_type::Union{StateTransfer,CoherenceTransfer},
    t,
    k,
    state,
    costate,
    props,
)::Float64
    # recall that target is always the last entry of L
    # and that we have in U[end] the propagated forward target state
    @views real(C1(costate[t, k], state[t, k]))
end

function fom_func(system_type::UnitaryGate, t, state, costate, props)::Float64
    @views real(tr(state[t]' * costate[t]) * tr(state[t]' * costate[t]))
end

function fom_func(
    system_type::Union{StateTransfer,CoherenceTransfer},
    t,
    state,
    costate,
    props,
)::Float64
    @views real(C1(costate[t], state[t]))
end


# -----------------------------------------------------------
#
# Constructing the matrix elements of the derivative
# operator in the 1-D DG Basis
#
# -----------------------------------------------------------

# Efficiency criticality: LOW
# Computations only performed once

# Accuracy criticality: HIGH
# Critical for accurate PDE evolution

# -----------------------------------------------------
# 1-D Derivatives
# -----------------------------------------------------

function dLegendreP(k,x)
	k<=K_max || throw(DomainError())
	return array2poly(symbolic_diff(leg_coeffs[k+1]),x)
end

function dh(k,mode,x)
	mode<=k || throw(DomainError())
	return array2poly(symbolic_diff((dg_coeffs[k])[mode]),x)
end

function dleg{T<:Real}(mode::Int, x::T)
	return sqrt(2.0)*dLegendreP(mode-1, 2*x-1) *2
end

function dbasis{T<:Real}(level::Int, cell::Int, mode::Int, x::T)
	return dleg(mode, (1<<level)*x - (cell-1)) * (2.0)^(level/2) * (1<<level)
end

function dbasis(level::Int, cell::Int, mode::Int)
	return x->dbasis(level, cell, mode, x)
end

# -----------------------------------------------------
# Shifted and scaled derivatives: v
# -----------------------------------------------------

function dv(k::Int, level::Int, cell::Int, mode::Int, x::Real)
	if level==0 # At the base level, we are not discontinuous, and we simply
				# use Legendre polynomials up to degree k-1 as a basis
		return 2*dLegendreP(mode-1,2*x-1)*sqrt(2.0)
	else
		return (1<<(level))*dh(k, mode, (1<<level)*x - (2*cell-1)) * (2.0)^(level/2)
		# Otherwise we use an appropriately shifted, scaled, and normalized
		# DG function
	end
end

function dv(k::Int, level::Int, cell::Int, mode::Int)
	return (xs::Real -> dv(k,level,cell,mode,xs))
end

# -----------------------------------------------------
# Numerical Inner Product
# -----------------------------------------------------

# TODO: There is a way to do this all symbolically, with no use for numerics
function inner_product1D(f::Function, g::Function, lvl::Int, cell::Int;
								rel_tol = REL_TOL, abs_tol = ABS_TOL, max_evals=MAX_EVALS)
	xmin = (cell-1)/(1<<max(0, lvl-1))
	xmax = (cell)/(1<<max(0, lvl-1))
	h = (x-> f(x)*g(x))
	(val, err) = hquadrature(h, xmin, xmax; reltol=rel_tol, abstol=abs_tol, maxevals=max_evals)
	return val
end

# -----------------------------------------------------
# Taking Derivative of Array Representing Polynomial
# -----------------------------------------------------

function symbolic_diff{T<:Real}(v::Array{T})
	n=length(v)
	k=div(n,2)
	ans = zeros(T,n)
	for i in 1:n
		if i<k
			ans[i] = i*v[i+1]
		elseif i > k && i<2k
			ans[i] = (i-k) * v[i+1]
		else
			ans[i]=0
		end
	end
	return ans
end

# -----------------------------------------------------
# < f_1 | D | f_2 > matrix elements
# -----------------------------------------------------

# Matrix element of legendre basis on [-1, 1]
function legendreDlegendre(mode1::Int, mode2::Int)
	return inner_product(leg_coeffs[mode1], symbolic_diff(leg_coeffs[mode2]))
end

# Matrix element of position basis elements
function legvDv(level, cell1, mode1, cell2, mode2;
				rel_tol = REL_TOL, abs_tol=ABS_TOL, max_evals=MAX_EVALS)
	if cell1 == cell2
		return (1<<(level+1))*legendreDlegendre(mode1, mode2)
	end
	return 0.0
end

# Matrix element of DG basis on [-1, 1]
function hDh(k::Int, mode1::Int, mode2::Int)
	return inner_product(dg_coeffs[k][mode1], symbolic_diff(dg_coeffs[k][mode2]))
end

# Matrix element of hierarchical basis elements
function vDv(k::Int, lvl1::Int, cell1::Int, mode1::Int, lvl2::Int, cell2::Int, mode2::Int;
					rel_tol = REL_TOL, abs_tol = ABS_TOL, max_evals=MAX_EVALS)
	if lvl1 == lvl2
		if lvl1 == 0
			return 2*legendreDlegendre(mode1, mode2)
		end
		if cell1 == cell2
			return hDh(k, mode1, mode2)*(1<<max(0, lvl1))
		end
		return 0.0
	end
	if lvl1 < lvl2
		if lvl1 == 0
			return inner_product1D(v(k,0,1,mode1), dv(k,lvl2,cell2,mode2), lvl2, cell2;
									rel_tol = rel_tol, abs_tol = abs_tol, max_evals=max_evals)
		end
		if (1<<(lvl2-lvl1))*cell1 >= cell2 && (1<<(lvl2-lvl1))*(cell1-1) < cell2
			return inner_product1D(v(k,lvl1,cell1,mode1), dv(k,lvl2,cell2,mode2), lvl2, cell2;
									rel_tol = rel_tol, abs_tol = abs_tol, max_evals=max_evals)
		end
		return 0.0
	end
	return 0.0

end

# -----------------------------------------------------
# < f_1 | D | f_2 > for a specific f_2 over all f_1
# -----------------------------------------------------

function diff_basis_DG(k::Int, level::Int, cell::Int, mode::Int)
	dcoeffs = Array{Real}((level+1, k))
	p = cell
	for l in level:-1:0
		for f_n in 1:k
			dcoeffs[l+1,f_n]=vDv(k, l, p, f_n, level, cell, mode)
		end
		p = Int(ceil(p/2))
	end
	return dcoeffs
end


export compensated_sum, recursive_sum, blocked_sum, fab_sum

# Kahan sum
function compensated_sum(A)
    T = eltype(A)
    sum = c = zero(T)
    for a in A
        y = a - c
        t = sum + y
        c = (t - sum) - y
        sum = t
    end
    return sum
end

function recursive_sum(A)
    T = eltype(A)
    sum = zero(T)
    for a in A
        sum += a
    end
    sum
end

function blocked_sum(A, b::Integer=128)
    n = length(A)
    m = (n+b-1) ÷ b
    S = [recursive_sum(view(A,i*b+1:min(i*b+b,n))) for i = 0:m-1]
    return sum(S)
end

function fab_sum(A, b::Integer=128)
    n = length(A)
    m = (n+b-1) ÷ b
    S = [recursive_sum(view(A,i*b+1:min(i*b+b,n))) for i = 0:m-1]
    return compensated_sum(S)
end


require("bm3d")

using Images, ImageView
using NumericExtensions
using BM3D

function getPsnr(img::Matrix{Float64},imgClean::Matrix{Float64})
	return 20.0*log10(255.0*sqrt(length(imgClean)/sumsqdiff(imgClean,img)))
end

# Convert test image to grayscale
img = imread("lena.tiff")
tmp = mean(convert(Array{Float64},img.data),1)
tmp = reshape(tmp,512,512)'
img = convert(Image,tmp)
img.properties["limits"] = (0.0,255.0)

# Noisy image
imgn = deepcopy(img)
sig = 15.0
imgn.data += sig*randn(512,512)

# Perform BM3D denoising
@time imgBasic = bm3d_thr(imgn.data,sig)
@time imgOut = bm3d_wie(imgn.data,imgBasic,sig)

imgBasic = convert(Image,imgBasic)
imgBasic.properties["limits"] = (0.0,255.0)

imgOut = convert(Image,imgOut)
imgOut.properties["limits"] = (0.0,255.0)

errNoisy = getPsnr(imgn.data,img.data)
errBasic = getPsnr(img.data,imgBasic.data)
errFinal = getPsnr(img.data,imgOut.data)

println("PSNR noisy: ", errNoisy, " dB")
println("PSNR basic: ", errBasic, " dB")
println("PSNR denoised: ", errFinal, " dB")

#display([imgn imgBasic])


# Multi-threading in Julia
# Multi-threading will be native to Julia in the next release (v0.5.x). It is currently under active development.
# This tutorial gives you a quick taste of how to use multi-threading in Julia.

# First load the threading code that's in the Threads module.

using Base.Threads

@inline function cndf2(in::Array{Float64,1})
    out = 0.5 .+ 0.5 .* erf(0.707106781 .* in)
    return out
end

function blackscholes_serial(sptprice::Float64,
                           strike::Array{Float64,1},
                           rate::Float64,
                           volatility::Float64,
                           time::Float64)
    logterm = log10(sptprice ./ strike)
    powterm = .5 .* volatility .* volatility
    den = volatility .* sqrt(time)
    d1 = (((rate .+ powterm) .* time) .+ logterm) ./ den
    d2 = d1 .- den
    NofXd1 = cndf2(d1)
    NofXd2 = cndf2(d2)
    futureValue = strike .* exp(- rate .* time)
    c1 = futureValue .* NofXd2
    call = sptprice .* NofXd1 .- c1
    put  = call .- futureValue .+ sptprice
end

# This next parallel rewrite does two simple things:
# It devectorizes the entire loop. We're now looping through arrays instead of dealing with them in blocks like we did previously. This is devectorized code while the above serial block is vectorized code.

function blackscholes_devec(sptprice::Float64,
				strike::Vector{Float64},
				rate::Float64,
				volatility::Float64,
				time::Float64)
    sqt = sqrt(time)
    put = similar(strike)
    for i = 1:size(strike, 1)
        logterm = log10(sptprice / strike[i])
        powterm = 0.5 * volatility * volatility
        den = volatility * sqt
        d1 = (((rate + powterm) * time) + logterm) / den
        d2 = d1 - den
        NofXd1 = 0.5 + 0.5 * erf(0.707106781 * d1)
        NofXd2 = 0.5 + 0.5 * erf(0.707106781 * d2)
        futureValue = strike[i] * exp(-rate * time)
        c1 = futureValue * NofXd2
        call_ = sptprice * NofXd1 - c1
        put[i] = call_ - futureValue + sptprice
    end
    put
end

# Affixes the `@threads` macro in front of the `for` to tell Julia that this is a multi-threaded block.

function blackscholes_parallel(sptprice::Float64,
				strike::Vector{Float64},
				rate::Float64,
				volatility::Float64,
				time::Float64)
    sqt = sqrt(time)
    put = similar(strike)
    @threads for i = 1:size(strike, 1)
        logterm = log10(sptprice / strike[i])
        powterm = 0.5 * volatility * volatility
        den = volatility * sqt
        d1 = (((rate + powterm) * time) + logterm) / den
        d2 = d1 - den
        NofXd1 = 0.5 + 0.5 * erf(0.707106781 * d1)
        NofXd2 = 0.5 + 0.5 * erf(0.707106781 * d2)
        futureValue = strike[i] * exp(-rate * time)
        c1 = futureValue * NofXd2
        call_ = sptprice * NofXd1 - c1
        put[i] = call_ - futureValue + sptprice
    end
    put
end

function run(iterations)
    sptprice   = 42.0
    initStrike = Float64[ 40.0 + (i / iterations) for i = 1:iterations ]
    rate       = 0.5
    volatility = 0.2
    time       = 0.5

    tic()
    put1 = blackscholes_serial(sptprice, initStrike, rate, volatility, time)
    t1 = toq()
    println("Serial checksum: ", sum(put1))
    tic()
    put2 = blackscholes_devec(sptprice, initStrike, rate, volatility, time)
    t2 = toq()
    println("Parallel checksum: ", sum(put2))
    tic()
    put3 = blackscholes_parallel(sptprice, initStrike, rate, volatility, time)
    t3 = toq()
    println("Parallel checksum: ", sum(put3))
    return t1, t2, t3
end

function driver()
    srand(0)
    tic()
    iterations = 10^6
    blackscholes_serial(0., Float64[], 0., 0., 0.)
    blackscholes_devec(0., Float64[], 0., 0., 0.)
    blackscholes_parallel(0., Float64[], 0., 0., 0.)
    println("SELFPRIMED ", toq())
    tserial, tdevec, tparallel = run(iterations)
    println("Time taken for serial = $tserial")
    println("Time taken for devec = $tdevec")
    println("Time taken for parallel = $tparallel")
    println("Serial rate = ", iterations / tserial, " opts/sec")
    println("Devec rate = ", iterations / tdevec, " opts/sec")
    println("Parallel rate = ", iterations / tparallel, " opts/sec")
end
driver()


# path grath
g = DAG(:A => :B, :B => :C)
@test is_d_separated(g, [1], [3], [2])
@test !is_d_separated(g, [1], [3], Int[])

# fork graph
g = DAG(:A => :B, :A => :C)
@test is_d_separated(g, [:B], [:C], [:A])
@test !is_d_separated(g, [:B], [:C], Symbol[])

# collider graph
g = DAG(:A => :B, :C => :B)
@test is_d_separated(g, [:A], [:C], Symbol[])
@test !is_d_separated(g, [:A], [:C], [:B])

# asia graph
g = DAG(
    :asia => :tuberculosis,
    :smoking => :cancer,
    :smoking => :bronchitis,
    :tuberculosis => :either,
    :cancer => :either,
    :either => :xray,
    :either => :dyspnea,
    :bronchitis => :dyspnea
)
# this test is wrong - dyspnea is in isolated component, but xray is on the main
@test is_d_separated(g,
    [:asia, :smoking],
    [:dyspnea, :xray],
    [:bronchitis, :either]
)
@test is_d_separated(g,
    [:tuberculosis, :cancer],
    [:bronchitis],
    [:smoking, :xray]
)

# test ignore_edges argument
g = DAG(:A => :B, :A => :C, :C => :B)
@test !is_d_separated(g, [:A], [:B], [:C])
@test is_d_separated(g, [:A], [:B], [:C], ignore_edges=[:A => :B])