@testset "Result" begin
    @testset "Result+PathResult" begin
        R = solve(equations(heart()), save_all_paths=true, show_progress=false)
        @test R isa Result
        @test nat_infinity(R) ≤ 572
        @test nfinite(R) == 4
        @test length(collect(R)) == 576
        @test finite(R) isa Vector{<:PathResult}

        @test length(finite(R, only_nonsingular=false)) == 4
        @test length(finite(R, only_nonsingular=true)) == 4
        @test length(finite(R, only_singular=true)) == 0
        @test 572 - length(failed(R)) == nat_infinity(R)
        @test length(real(R, tol=1e-6)) == 2
        @test nreal(R, tol=1e-6) == 2
        @test length(real_solutions(R)) == 2
        @test_deprecated realsolutions(R)
        @test length(at_infinity(R)) ≤ 572
        @test length(results(R, only_real=true, real_tol=1e-8)) == 2
        @test length(results(R, only_nonsingular=true, singular_tol=1e9)) == 4
        @test length(finite(results(R, only_real=true))) == 2
        @test nresults(R, only_nonsingular=true, singular_tol=1e9) == 4
        @test length(results(R, onlyfinite=false)) == 576
        @test nresults(R, onlyfinite=false) == 576
        @test nnonsingular(R) == 4
        @test length(nonsingular(R)) == 4
        @test length(singular(R)) == 0
        @test length(singular(real(R))) == 0

        @test_nowarn mapresults(solution, R)
        @test_nowarn mapresults(start_solution, R)
        @test_nowarn mapresults(residual, R)
        @test_nowarn mapresults(is_singular, R)
        @test count(mapresults(is_real, R)) == 2
        # test fallback
        @test count(results(is_real, R)) == 2

        @test length(solutions(R)) == 4
        @test solutions(R) isa Vector{Vector{ComplexF64}}
        @test real_solutions(R, real_tol=1e-8) isa Vector{Vector{Float64}}
        @test length(real_solutions(R, real_tol=1e-8)) == 2

        @test_nowarn string(R)
        @test_nowarn string(R[end])
        @test_nowarn show(IOContext(devnull, :compact=>true), R)
        @test_nowarn show(IOContext(devnull, :compact=>true), R[end])

        test_treeviews(R)

        @polyvar x y z
        R = solve([(x-3)^3,(y-2)], affine_tracking=true)
        @test R isa Result
        test_treeviews(R)
        @test nnonsingular(R) == 0
        @test nsingular(R) == 1
        @test nsingular(R, counting_multiplicities=true) == 3
        @test_nowarn sprint(show, R)

        R = solve([(x-3)^3,(y-2)], affine_tracking=true, system_scaling=nothing)
        @test nnonsingular(R) == 0
        @test nsingular(R) == 1
        @test nsingular(R, counting_multiplicities=true) == 3
        @test_nowarn sprint(show, R)

        @polyvar x y z
        R = solve([(x-3z),(y-2z)])
        @test R isa Result{<:ProjectiveVectors.PVector}
        @test_nowarn string(R)
        test_treeviews(R)
        @test nnonsingular(R) == 1

        @polyvar x y z
        R = solve([(x-3z)^3,(y-2z)])
        @test R isa Result{<:ProjectiveVectors.PVector}
        test_treeviews(R)
        @test nnonsingular(R) == 0
        @test nsingular(R) == 1
        @test nsingular(R, counting_multiplicities=true) == 3

        @polyvar x y z
        V = x^2 + y^2 - z^2
        L = randn(1,3) * [x, y, z]
        R = solve([V; L])
        @test_nowarn sprint(show, R)
    end

    @testset "singular result" begin
        @polyvar x
        f = (x-3)^3*(x-2)
        R = solve([f])
        @test length(solutions(R)) == 2
        @test sort(results(multiplicity, R)) == [1,3]

        @polyvar x y
        f = (x-3y)^3*(x-2y)
        R = solve([f])
        @test nsolutions(R) == 2
        @test sort(results(multiplicity, R)) == [1,3]
    end

    @testset "multiplicities" begin
        @polyvar x y z
        f = (x-1*y)*(x-1.01*y)
        g = x - y - z
        S = solve([f,g], threading=false)
        @test length(multiplicities(S, tol=1e-1)) == 1
        @test length(multiplicities(S, tol=1e-5)) == 0
        @test length(multiplicities(S.pathresults, tol=1e-1)) == 1
        @test length(multiplicities(S.pathresults, tol=1e-5)) == 0

        @polyvar x z
        f = (x-1)*(x-1.01)
        g = x - 1 - z
        S = solve([f,g], threading=false)
        @test length(multiplicities(S, tol=1e-1)) == 1
        @test length(multiplicities(S, tol=1e-5)) == 0
        @test length(multiplicities(S.pathresults, tol=1e-1)) == 1
        @test length(multiplicities(S.pathresults, tol=1e-5)) == 0
        @test norm(solution(S[1]) - solution(S[2])) < 1e-1
    end
end


using Test
using LinearAlgebra
using GenericLinearAlgebra
using GenericLinearAlgebra: qrBlocked!

@testset "The QR decomposition" begin
    @testset "Problem dimension ($m,$n) with block size $bz" for
        (m, n) in (( 10,  5), ( 10,  10), ( 5,  10),
                   (100, 50), (100, 100), (50, 100)),
            bz in (1, 2, 3, 4, 7, 8, 9, 15, 16, 17, 31, 32, 33)

        A = randn(m, n)
        Aqr = qrBlocked!(copy(A), bz)
        AqrQ = Aqr[Tuple{:QBlocked}]
        if m >= n
            @test (AqrQ'A)[1:min(m,n),:] ≈ Aqr[Tuple{:R}]
        else # For type stbility getindex(,Tuple{:R}) throw when the output is trapezoid
            @test (AqrQ'A) ≈ triu(Aqr.factors)
        end
        @test AqrQ'*(AqrQ*A) ≈ A
    end

    @testset "Error paths" begin
        @test_throws DimensionMismatch LinearAlgebra.reflectorApply!(zeros(5, 5), zeros(4), 1.0)
        @test_throws ArgumentError     qrBlocked!(randn(5, 10))[Tuple{:R}]
    end
end


export inpolygon

"""
Return whether the given point (px, py) is inside or on the edge of the
polygon defined by vertices (polyx, polyy). Uses Hormann-Agathos
"Point in Polygon" algorithm to support arbitrary polygons.
Implementation inspired by 'isinside' in Luxor
(see https://github.com/JuliaGraphics/Luxor.jl/blob/master/src/polygons.jl)
"""
function inpolygon(px, py, polyx, polyy)
    if !isfinite(px) || !isfinite(py)
        return false
    end

    c = false
    @inbounds for idx in 1:length(polyx)
        q1x = polyx[idx]
        q1y = polyy[idx]

        nextidx = 1 + mod(idx, length(polyx))
        q2x = polyx[nextidx]
        q2y = polyy[nextidx]

        if q1x == px && q1y == py
            return true # on vertex
        end
        if q2y == py
            if q2x == px
                return true # on vertex
            elseif q1y == py && (q2x > px) == (q1x < px)
                return true # on edge
            end
        end
        if (q1y < py) != (q2y < py) # crossing
            if q1x >= px
                if q2x > px
                    c = !c
                else
                    det = det3p(q1x, q1y, q2x, q2y, px, py)
                    if isapprox(0, det)
                        return true # on edge
                    elseif (det > 0) == (q2y > q1y)
                        c = !c
                    end
                end
            elseif q2x > px
                det = det3p(q1x, q1y, q2x, q2y, px, py)
                if isapprox(0, det)
                    return true # on edge
                elseif (det > 0) == (q2y > q1y)
                    c = !c
                end
            end
        end
    end
    return c
end

function det3p(q1x, q1y, q2x, q2y, px, py)
    (q1x - px) * (q2y - py) - (q2x - px) * (q1y - py)
end


using Optimize
using NLPModels

using Plots
# pyplot()
plotly()

n_min = 1
n_max = 1

# Math Prog Base collection
include("MPBProblems-1D.jl")
test_probs = mpb_probs_1D

using ScalarSolvers
solvers = [TR_Sec, TR_SecA, bissect_sec, ARC_SecA, bissect_nwt, ARC_Sec]
labels = ["TR_Sec"; "TR_SecA"; "bissect_sec"; "ARC_SecA"; "bissect_nwt"; "ARC_Sec"]

options = [Dict{Symbol,Any}(:verbose=>false)
           Dict{Symbol,Any}(:verbose=>false)
           Dict{Symbol,Any}(:verbose=>false)
           Dict{Symbol,Any}(:verbose=>false)
           Dict{Symbol,Any}(:verbose=>false)
           Dict{Symbol,Any}(:verbose=>false)
           ]

include("compare_solvers.jl")

# s1, P1, t1, Pt1 = compare_solvers_with_options2(solvers, options, labels, test_probs, n_min, n_max, printskip = false)
P2 = compare_solvers_with_options2(solvers, options, labels, test_probs, n_min, n_max, printskip = false)[2]


using SubsetSelection, DataFrames, LinearAlgebra

function split_data(n, holdout)
    batch = ceil(Int, n*holdout)
    perm = shuffle(collect(1:n))
    train = perm[1:(n-batch)]
    val = perm[(n-batch+1): n]
    return train, val
end

function error(ℓ::Regression, Y, X, indices, w)
  Z = Y .- X[:,indices]*w
  return dot(Z,Z)/size(Z,1)
end

function error(ℓ::Classification, Y, X, indices, w)
    df = DataFrame(Output = (Y.>0), Probability = 1. ./ (1 .+ exp.(X[:,indices]*w)))
    sort!(df, :Probability)
    # sort!(df, cols = :Probability)
    # df = sort(df, cols = :Probability)
    df[!,:Above]=(1:length(Y)) .- cumsum(df[:,:Output])

    nP = length(findall(o->o>0, Y)); nN = length(findall(o->o<0, Y));

    AUC = 1-sum(df[:,:Above].*df[:,:Output])/nP/nN
    return 1-AUC
end

function error2(ℓ::Classification, Y, X, indices, w)
    Y_truth = Y .> 0
    Y_pred = X[:,indices]*w .> 0

    MR = sum(1 .- Y_truth.*Y_pred .- (1 .- Y_truth).*(1 .- Y_pred)) / size(Y,1)
    return MR
end

function grad_primal(ℓ::LossFunction, Y, X, w, γ)
  n,k = size(X)
  g = zeros(k)
  for j in 1:k
    g[j] += w[j] / γ
  end
  for i in 1:n
    g += grad_loss(ℓ, Y[i], X[i,:], w)
  end
  return g
end

function grad_loss(ℓ::OLS, y, x, w)
  return -(y-dot(x,w))*x
end
function grad_loss(ℓ::L1SVM, y, x, w)
  return -y*(1-y*dot(x,w) > 0)*x
end
function grad_loss(ℓ::LogReg, y, x, w)
  z = exp.(y*dot(x,w))
  return -y/(1+z)*x
end

function value_primal(ℓ::LossFunction, Y, X, w, γ)
  n,k = size(X)
  v = dot(w,w)/2/γ
  for i in 1:n
    v += value_loss(ℓ, Y[i], X[i,:], w)
  end
  return v
end

function value_loss(ℓ::OLS, y, x, w)
  return .5*(y-dot(x,w))^2
end
function value_loss(ℓ::L1SVM, y, x, w)
  return max(0, 1-y*dot(x,w))
end
function value_loss(ℓ::LogReg, y, x, w)
  z = exp.(-y*dot(x,w))
  return log(1+z)
end


#Solves the predictor corrector euler scheme - pg327 of Bruti-Liberati 2007
#Strong order 0.5, with some degree of implicitness

#Prototyping predictor corrector code
struct PCEproto <: Solver end
struct PCE <: Solver end

"""
optimized predictor corrector integrate
ab! : bias & diffusion function
x0 : Initial state
t : times to use for time evolution
noise : Noise object
callback : a function that runs on t, x, W
"""
function integrate(a!::Function, b!::Function, x0::AbstractVector{T}, t::AbstractVector{<:Real},
    noise::Noise, pce::PCEproto, callback::Function; θ::Float64=0.5, η::Float64 = 0., bbprime!::Function = (t,x,bbp)-> nothing) where {T<:Number}

    n = length(x0)
    bbp = zeros(T, n)

    function abar!(t, x, ff)
        a!(t, x, ff)
        bbprime!(t, x, bbp)
        ff .= ff - η*bbp
        return nothing
    end
    W = W0(noise)
    ΔW = copy(W)
    x = copy(x0)

    f = Vector{T}(n)
    g = Matrix{T}(n, noise.m)

    xbar = copy(x0)
    fbar = copy(f)
    gbar = copy(g)

    callback(t[1], x, W)
    for i in 1:length(t)-1
        Δt = t[i+1]-t[i]
        randn!(noise, ΔW)
        ΔW .*= sqrt(Δt)

        abar!(t[i], x, f)
        b!(t[i], x, g)

        xbar .= copy(x)
        xbar .+= f*Δt + g*ΔW

        abar!(t[i+1], xbar, fbar)
        b!(t[i+1], xbar, gbar)

        x .+= (θ*fbar + (1-θ)*f)*Δt + (η*gbar + (1-η)*g)*ΔW
        W .+= ΔW
        callback(t, x, W)
    end
end

function integrate(a!::Function, b!::Function, x0::AbstractVector{T}, t::AbstractVector{<:Real},
    noise::Noise, pce::PCE, callback::Function; θ::Float64=0.5, η::Float64 = 0., bbprime!::Function = (t,x,bbp)-> nothing) where {T<:Number}

    n = length(x0)
    bbp = zeros(T, n)

    function abar!(t, x, ff)
        a!(t, x, ff)
        bbprime!(t, x, bbp)
        bbp .*= -η
        ff .+=  bbp
        return nothing
    end

    W = W0(noise)
    ΔW = copy(W)
    x = copy(x0)

    fΔt = Vector{T}(n)
    g = Matrix{T}(n, noise.m)
    gΔW = Vector{T}(n)

    xbar = copy(x)

    #save the first data point
    callback(t[1], x, W)

    for i in 1:length(t)-1
        Δt = t[i+1]-t[i]
        randn!(noise, ΔW)
        ΔW .*= sqrt(Δt)

        abar!(t[i], x, fΔt)
        b!(t[i], x, g)

        fΔt .*= Δt
        A_mul_B!(gΔW, g, ΔW)

        xbar = copy(x)
        xbar .+= fΔt
        xbar .+= gΔW

        fΔt .*= (1-θ)
        gΔW .*= (1-η)

        x .+= fΔt
        x .+= gΔW

        abar!(t[i+1], xbar, fΔt)
        b!(t[i+1], xbar, g)

        fΔt .*= (Δt*θ) #fΔbar*θ
        A_mul_B!(gΔW, g, ΔW)
        gΔW .*= η #gbar*η
        x .+= fΔt
        x .+= gΔW

        W .+= ΔW
        callback(t, x, W)
    end
    return nothing
end


module DiscFESpacesTests

using Test
using Gridap

D = 2
dom = fill(4,D)
model = CartesianDiscreteModel(partition=tuple(dom...))

order = 1
reffe = PDiscRefFE(Float64,D,order)

fespace = DiscFESpace(reffe,model)

ufun(x) = x[1] + x[2]

trian = Triangulation(model)

nfree = 48
ndiri = 0

trian = Triangulation(model)
quad = CellQuadrature(trian,degree=2)

bh = FEBasis(fespace)
uh = zero(fespace)

cellmat = integrate(inner(bh,bh),trian,quad)
cellvec = integrate(inner(bh,uh),trian,quad)

test_fe_space(fespace, nfree, ndiri, cellmat, cellvec, ufun)

@test celldofids(fespace).data == 1:nfree

uh = interpolate(fespace,ufun)

e = ufun - uh

el2 = sqrt(sum(integrate(inner(e,e),trian,quad)))
@test el2 < 1.0e-10

#ufun(x) = sin(x[1]) * cos(x[2])
#
#uh = interpolate(fespace,ufun)
#
#writevtk(trian,"trian",nref=4,cellfields=["uh"=>uh])

end # module


#=

  Seseman problem in Julia + ConstraintSolver.jl

  Description of the problem:

  n is the length of a border
  There are (n-2)^2 "holes", i.e.
  there are n^2 - (n-2)^2 variables to find out.

  The simplest problem, n = 3 (n x n matrix)
  which is represented by the following matrix:

   a b c
   d   e
   f g h

  Where the following constraints must hold:

    a + b + c = border_sum
    a + d + f = border_sum
    c + e + h = border_sum
    f + g + h = border_sum
    a + b + c + d + e + f = total_sum

  For a (Swedish) discussion of this problem, see
  "Sesemans matematiska klosterproblem samt lite Constraint Logic Programming"
  http://www.hakank.org/webblogg/archives/001084.html
  and
  Seseman's Convent Problem: http://www.hakank.org/seseman/seseman.cgi
  (using ECLiPSe CLP code)

  It was also is commented in the (Swedish) blog post
  "Constraint Programming: Minizinc, Gecode/flatzinc och ECLiPSe/minizinc"
  http://www.hakank.org/webblogg/archives/001209.html

  Model created by Hakan Kjellerstrand, hakank@gmail.com
  See also my Julia page: http://www.hakank.org/julia/

=#
using ConstraintSolver, JuMP
using Cbc, GLPK
const CS = ConstraintSolver

include("constraints_utils.jl")

#
# first_num = 1:
# * without symmetry breaking: 85 solutions
# * with symmetry breaking: 4 solutions
#
# first_num = 0:
# * without symmetry breaking: 231 solutions
# * with symmetry breaking: 7 solutions#
#
function seseman(row_sum=9,total=24,first_num=1;all_solutions=false,print_solutions=true,symmetry_breaking=false)

    cbc_optimizer = optimizer_with_attributes(Cbc.Optimizer, "logLevel" => 0)
    glpk_optimizer = optimizer_with_attributes(GLPK.Optimizer)

    model = Model(optimizer_with_attributes(CS.Optimizer,   "all_solutions"=> all_solutions,
                                                            # "all_optimal_solutions"=>true,
                                                            "logging"=>[],

                                                            # "traverse_strategy"=>:BFS,
                                                            "traverse_strategy"=>:DFS,
                                                            # "traverse_strategy"=>:DBFS,

                                                            # "branch_split"=>:Smallest,
                                                            # "branch_split"=>:Biggest,
                                                            "branch_split"=>:InHalf,

                                                            # "simplify"=>false,
                                                            "simplify"=>true,

                                                            "time_limit"=>14,
                                                            # "lp_optimizer" => cbc_optimizer,
                                                            # "lp_optimizer" => glpk_optimizer,
                                        ))

    println("row_sum:$row_sum total:$total first_num:$first_num symmetry_breaking:$symmetry_breaking")

    @variable(model, first_num <= x[1:8] <= 9, Int)
    a,b,c,d,e,f,g,h = x

    @constraint(model, sum(x) == total)

    segments = [[a,b,c],
                [a,d,f],
                [c,e,h],
                [f,g,h]]
    for segment in segments
        @constraint(model, sum(segment) == row_sum)
    end

    if symmetry_breaking
        syms = [[a,h],[b,d],[d,e],[e,g]]
        for s in syms
            @constraint(model, s[1] <= s[2])
        end
    end

    # Solve the problem
    # println("solve")
    optimize!(model)

    status = JuMP.termination_status(model)
    # println("status:$status")
    if status == MOI.OPTIMAL
        num_sols = MOI.get(model, MOI.ResultCount())
        println("num_sols:$num_sols\n")
        # num_sols2 = JuMP.result_count(model) # same result
        # println("num_sols2:$num_sols2\n")

        if print_solutions
            for sol in 1:num_sols
                println("solution #$sol")
                xx = convert.(Integer,JuMP.value.(x,result=sol))
                println("$(xx[1])  $(xx[2])  $(xx[3])")
                println("$(xx[4])     $(xx[5])")
                println("$(xx[6])  $(xx[7])  $(xx[8])")
            end
        end
    end
end

# First num: 1
seseman(9,24,1;all_solutions=true,print_solutions=true,symmetry_breaking=true)

# First num: 0
# seseman(9,24,0;all_solutions=true,symmetry_breaking=false)

println("\nChecking different configurations")
for first_num in [0,1], symmetry_breaking in [true,false]
    seseman(9,24,first_num;all_solutions=true,print_solutions=false,symmetry_breaking=symmetry_breaking)
end


#=
ACCURATE FLOATING-POINT SUMMATION PART I: FAITHFUL ROUNDING
Siegfried M. Rump, Takeshi Ogita, and Shin'ichi Oishi
in SIAM J. Sci. Comput. Volume 31, Issue 1, pp. 189-224 (2008)

ERROR ESTIMATION OF FLOATING-POINT SUMMATION AND DOT PRODUCT
Siegfried M. Rump
in BIT, May 18, 2011
=#

import Base: precision, ldexp

# export epsilon, eta, ufp, ulp, sufp, sulp

for F in (:Float64, :Float32, :Float16)
   @eval begin
       epsilon(::Type{$F}) = ldexp(one($F), -precision($F))     # dist from 1 to the next smaller float
       epsilon(x::($F)) = flipsign(abs(x)-prevfloat(abs(x)),x)  # dist from x to next lesser mag float
       eta(::Type{$F}) = nextfloat(zero($F))                    # smallest nonzero subnormal
       ufp(x::($F)) = ldexp(one($F), floor(Int, log2(abs(x))))  # unit first place
       ulp(x::($F)) = ldexp(one($F), floor(Int, log2(abs(x))) - precision($F)) # unit last place
       sufp(x::($F)) = flipsign(ufp(x),x)                       # signed unit first place
       sulp(x::($F)) = flipsign(ulp(x),x)                       # signed unit last place
   end
end


using LowLevelParticleFilters, ForwardDiff, Distributions
const LLPF = LowLevelParticleFilters
eye(n) = Matrix{Float64}(I,n,n)
n = 2 # Dinemsion of state
m = 2 # Dinemsion of input
p = 2 # Dinemsion of measurements

# Define random linenar state-space system
A = SMatrix{n,n}([0.99 0.1; 0 0.2])
B = @SMatrix randn(n,m)
C = SMatrix{p,p}(eye(p))
# C = SMatrix{p,n}([1 1])

dynamics(x,u,t) = A*x .+ B*u
measurement(x,u,t) = C*x

T = 800 # Number of time steps

d0 = MvNormal(randn(n),2.0)   # Initial state Distribution
du = MvNormal(2,1) # Control input distribution
kf = KalmanFilter(A, B, C, 0, 0.001eye(n), eye(p), d0)
ekf = LLPF.ExtendedKalmanFilter(kf, dynamics, measurement)
x,u,y = LLPF.simulate(kf,T,du)

xf,xt,R,Rt,ll = forward_trajectory(kf, u, y)
xf2,xt2,R2,Rt2,ll2 = forward_trajectory(ekf, u, y)

# When the dynamics is linear, these should be the same
@test reduce(hcat, xf) ≈ reduce(hcat, xf2)
@test reduce(hcat, xt) ≈ reduce(hcat, xt2)
@test reduce(hcat, R)  ≈ reduce(hcat, R2)
@test reduce(hcat, Rt) ≈ reduce(hcat, Rt2)
@test ll ≈ ll2

## add nonlinear dynamics
dynamics2(x,u,t) = A*x - 0.01*abs.(x) .+ B*u
ekf = LLPF.ExtendedKalmanFilter(kf, dynamics2, measurement)
x,u,y = LLPF.simulate(ekf,T,du)

xf,xt,R,Rt,ll = forward_trajectory(kf, u, y)
xf2,xt2,R2,Rt2,ll2 = forward_trajectory(ekf, u, y)

@test norm(reduce(hcat, x .- xf)) > norm(reduce(hcat, x .- xf2))
@test norm(reduce(hcat, x .- xt)) > norm(reduce(hcat, x .- xt2))
@test ll < ll2

# plot(reduce(hcat, x)', layout=2, lab="True")
# plot!(reduce(hcat, xf)', lab="Kf")
# plot!(reduce(hcat, xf2)', lab="EKF")


function ispower2(x::Integer)::Bool
    x>0 && (x & (x-1))==0
end
arr = [1, 2, 3, 4, 8, 10]
for x in arr
    println("$(x) is $(ispower2(x) ? "" : "not ")a power of 2.")
end


# genGauss
@testset "genGauss" begin
    # One-dimensional test
    aG = ones(3);
    mG = zeros(1,3);
    mG[1,:] = [0.0, 0.7, -0.3];
    AG = zeros(1,1,3);
    AG[1,1,:] = [100 80 120];
    g = FPRS.genGauss(aG, mG, AG);
    @test abs.(sum(g) - 2.8286690853954575) < 67.0 * eps()

    # Two-dimensional test
    # Generate 3 Gaussians
    aG = ones(3);
    mG = zeros(2,3);
    mG[1,:] = [0.0, 0.7, -0.3];
    mG[2,:] = [0.0, -0.6, -0.06];
    AG = zeros(2,2,3);
    AG[:,:,1] = [100 0.05; 0 100];
    AG[:,:,2] = [100 0.15; 0 100];
    AG[:,:,3] = [100 -0.3; 0.4 100];
    g = FPRS.genGauss(aG, mG, AG);
    @test abs.(sum(g) - 102.63572536664947) < 67.0^2 * eps()

    xmin = [-1.0 -2.0];
    xmax = [2.0   1.0];
    nPts = [127 127];
    g = FPRS.genGauss(aG, mG, AG, nPts = nPts, xmin=xmin, xmax=xmax);
    @test abs.(sum(g) - 166.25310747322118) < 127.0^2 * eps()

    # Test errors
    aG = ones(2)
    @test_throws DimensionMismatch FPRS.genGauss(aG, mG, AG);
    aG = ones(3)
    AG = zeros(2,3,3)
    @test_throws DimensionMismatch FPRS.genGauss(aG, mG, AG);

    # Three-dimensional test
    aG = ones(3);
    mG = zeros(3,3);
    mG[1,:] = [0.0, 0.7, -0.3];
    mG[2,:] = [0.0, -0.6, -0.06];
    mG[3,:] = [0.3, -0.5, -0.2];
    AG = zeros(3,3,3);
    AG[:,:,1] = [100 0.05 50; 0   100 20; 0  0  100];
    AG[:,:,2] = [100 0.15 10; 0   100 10; 0  20 100];
    AG[:,:,3] = [100 -0.3 10; 0.4 100 50; 50 0  100];
    g = FPRS.genGauss(aG, mG, AG);
    @test abs.(sum(g) - 627.7968551738583) < 67.0^3 * eps()
end

# genRandGauss
@testset "genRandGauss" begin
    # 1D
    g = FPRS.genRandGauss(nGauss = 50, nPts=[512], AGmin=100, AGmax=1000, mGrad=0.5, randSeed=0, suppShape="box")
    @test abs.(sum(g) - 63.77852000489939) < 512 * eps()

    # 2D
    g = FPRS.genRandGauss(nGauss = 10, nPts=[51,51], mGrad=0.5, randSeed=0, suppShape="ball")
    @test abs.(sum(g) - 45.34679104451378) < 51^2 * eps()

    # 3D
    g = FPRS.genRandGauss(nGauss = 10, nPts=[31,31,21], randSeed=0, suppShape="ball")
    @test abs.(sum(g) - 17.801826001001068) < 31^2 * 21 * eps()

    # Reverse-engineered. (?) Why not ArgumentError
    @test_throws MethodError FPRS.genGauss(suppShape = 1);
end


### A Pluto.jl notebook ###
# v0.14.0

using Markdown
using InteractiveUtils

# This Pluto notebook uses @bind for interactivity. When running this notebook outside of Pluto, the following 'mock version' of @bind gives bound variables a default value (instead of an error).
macro bind(def, element)
    quote
        local el = $(esc(element))
        global $(esc(def)) = Core.applicable(Base.get, el) ? Base.get(el) : missing
        el
    end
end

# ╔═╡ 0db6ee04-81b7-11eb-330c-11b578b72c90
html"""

Section 1.9

 Taking Advantage of Structure

Lecture Video

"""

# ╔═╡ ca1a1072-81b6-11eb-1fee-e7df687cc314
PlutoUI.TableOfContents(aside = true)

# ╔═╡ b0ba5b8c-f5d1-11ea-1304-3f0e47f935fe
md"# Examples of structure"

# ╔═╡ 864e1180-f693-11ea-080e-a7d5aabc9ca5
begin
	import Pkg
    Pkg.activate(mktempdir())
    Pkg.add([
		Pkg.PackageSpec(name="ImageIO", version="0.5"),
		Pkg.PackageSpec(name="ImageShow", version="0.2"),
		Pkg.PackageSpec(name="FileIO", version="1.6"),
		Pkg.PackageSpec(name="PNGFiles", version="0.3.6"),
		Pkg.PackageSpec(name="Colors", version="0.12"),
		Pkg.PackageSpec(name="ColorVectorSpace", version="0.8"),
        Pkg.PackageSpec(name="ColorSchemes", version="3"),

        Pkg.PackageSpec(name="PlutoUI", version="0.7"),
    ])
	using Colors, ColorVectorSpace, ImageShow, FileIO
	using ImageShow.ImageCore
	using ColorSchemes

	using InteractiveUtils, PlutoUI
	using LinearAlgebra, SparseArrays, Statistics
end

# ╔═╡ 261c4df2-f5d2-11ea-2c72-7d4b09c46098
md"""
# One-hot vectors
This example comes from machine learning.
"""

# ╔═╡ 3cada3a0-81cc-11eb-04c8-bde26d36a84e
md"""
A **one-hot** vector has a single "hot" element, i.e. a single 1 in a sea of zeros. For example:
"""

# ╔═╡ fe2028ba-f6dc-11ea-0228-938a81a91ace
myonehatvector = [0, 1, 0, 0, 0, 0]

# ╔═╡ 8d2c6910-f5d4-11ea-1928-1baf09815687
md"""How much "information" (numbers) do you need to represent a one-hot vector? Is it $n$ or is it two?
"""

# ╔═╡ 54649792-81cc-11eb-1038-9161a4037acf
md"""
(There are also "1-cold" vectors:)
"""

# ╔═╡ 0a902426-f6dd-11ea-0ae4-fb0c47863fe7
# also one "cold"
1 .- myonehatvector

# ╔═╡ 4794e860-81b7-11eb-2c91-8561c20f308a
md"""
## Julia: `structs` (creating a new type in Julia)
"""

# ╔═╡ 67827da8-81cc-11eb-300e-278104d2d958
md"""
We can create our own new types in Julia. Let's create a new type to represent one-hot vectors. It will be a subtype of `AbstractVector`, meaning that it *behaves like* a vector.
"""

# ╔═╡ 4624cd26-f5d3-11ea-1cf8-7d6555eae5fa
struct OneHot <: AbstractVector{Int}
	n::Int
	k::Int
end

# ╔═╡ 9bdabef8-81cc-11eb-14a1-67a9a7d968c0
md"""
We need to specify how long the vector is:
"""

# ╔═╡ 397ac764-f5fe-11ea-20cc-8d7cab19d410
Base.size(x::OneHot) = (x.n, )

# ╔═╡ a22dcd2c-81cc-11eb-1252-13ace134192d
md"""
and how to extract the $i$th component:
"""

# ╔═╡ 82c7046c-f5d3-11ea-04e2-ef7c0f4db5da
Base.getindex(x::OneHot, i::Int) = Int(x.k == i)

# ╔═╡ b024c318-81cc-11eb-018c-e1f7830ff51b
md"""
(Note that `x.k == i` returns a Boolean value, `true` or `false`, which we are then converting to an `Int`.)
"""

# ╔═╡ 93bfe3ac-f756-11ea-20fb-8f7d586b42f3
myonehotvector = OneHot(6,2)

# ╔═╡ 175039aa-f758-11ea-251a-5db57d7c4b32
myonehotvector[3]

# ╔═╡ c2a4b0a2-81cc-11eb-37a7-db601a6ddfdf
myonehotvector[2]

# ╔═╡ c5ed7d3e-81cc-11eb-3386-15b72db8155d
md"""
This behaves as if it were the original vector, but we are storing only 2 integers.
This is an example of taking advantage of structure.
"""

# ╔═╡ e2e354a8-81b7-11eb-311a-35151063c2a7
md"""
## Julia: dump and Dump
"""

# ╔═╡ dc5a96ba-81cc-11eb-3189-25920df48afa
md"""
`dump` shows the internal data stored inside a given object:
"""

# ╔═╡ af0d3c22-f756-11ea-37d6-11b630d2314a
with_terminal() do
	dump(myonehotvector)
end

# ╔═╡ 06e3a842-26b8-4417-9cf5-8a083ccdb264
md"""
Since `dump` writes to a terminal, you can't use it directly inside Pluto, which is why we wrap it inside a `with_terminal` block. You can also use the function `Dump` from `PlutoUI`:
"""

# ╔═╡ 91172a3e-81b7-11eb-0953-9f5e0207f863
Dump(myonehotvector)

# ╔═╡ 4bbf3f58-f788-11ea-0d24-6b0fb070829e
myonehotvector

# ╔═╡ fe70d104-81b7-11eb-14d0-eb5237d8ea6c
md"""
### Visualizing a one-hot vector
"""

# ╔═╡ ef8f44b2-f5fc-11ea-1e4d-bd873cd39d6c
md"""
n=$(@bind nn Slider(1:20, show_value=true))
"""

# ╔═╡ fd9211c0-f5fc-11ea-1745-7f2dae88af9e
md"""
k=$(@bind kk Slider(1:nn, default=1, show_value=true))
"""

# ╔═╡ f1154df8-f693-11ea-3b16-f32835fcc470
x = OneHot(nn, kk)

# ╔═╡ 982590d4-f5ff-11ea-3802-73292c75ad6c
show_image(x)

# ╔═╡ 81c35324-f5d4-11ea-2338-9f982d38732c
md"# Diagonal matrices"

# ╔═╡ 2cfda0dc-f5d5-11ea-16c4-b5a33b90e37f
md"Another example is diagonal matrices. Here's how you might see them in high school:"

# ╔═╡ 150432d4-f5d5-11ea-32b2-19a2a91d9637
denseD = [5 0 0
	    0 6 0
	    0 0 -10]

# ╔═╡ 44215aa4-f695-11ea-260e-b564c6fbcd4a
md"Julia has a better way of representing them:"

# ╔═╡ 21328d1c-f5d5-11ea-288e-4171ad35326d
D = Diagonal(denseD)

# ╔═╡ 75761cc0-81cd-11eb-1186-7d47debd68ca
md"""
It even displays nicely, with dots instead of zeros.

We can also create a diagonal matrix from the values on the diagonal:
"""

# ╔═╡ 6bd8a886-f758-11ea-2587-870a3fa9d710
Diagonal([5, 6, -10])

# ╔═╡ 4c533ac6-f695-11ea-3724-b955eaaeee49
md"How much information is stored for each representation? We can use Julia's `dump` function to find out:"

# ╔═╡ 466901ea-f5d5-11ea-1db5-abf82c96eabf
Dump(denseD)

# ╔═╡ b38c4aae-f5d5-11ea-39b6-7b0c7d529019
Dump(D)

# ╔═╡ 93e04ed8-81cd-11eb-214a-a761ef8c406f
md"""
We see that `Diagonal` stores only the diagonal entries, not the zeros!
"""

# ╔═╡ e90c55fc-f5d5-11ea-10f1-470ff772985d
md"""We should always look for *structure* where it exists!"""

# ╔═╡ 19775c3c-f5d6-11ea-15c2-89618e654a1e
md"# Sparse matrices"

# ╔═╡ 653792a8-f695-11ea-1ae0-43761c502583
md"A *sparse* matrix is a matrix that has many zeros, and is hence worth storing in a *sparse* representation:"

# ╔═╡ 79c94d2a-f75a-11ea-031d-09d70d229e15
denseM = [0 0 9; 0 0 0;12 0 4]

# ╔═╡ 10bc5d50-81b9-11eb-2ac7-354a6c6c826b
md"""
The above displays a sparse matrix in so-called `(i, j, value)` form.  We could
store sparse matrices in this way:
"""

# ╔═╡ 77d6a952-81ba-11eb-24e3-cb6510a59455
M = sparse(denseM)

# ╔═╡ 1f3ba55a-81b9-11eb-001f-593b9d8639ca
md"""
Although it looks like it's stored like this, in fact the actual storage format is different. In the Julia `SparseArrays.jl` package, the storage format
is **compressed sparse column** (CSC) format,
which is generally considered favorable for arithmetic, matrix-vector
	products and column slicing.  Of course, for specific matrices, other
		formats might be better.

* `nzval` contains the nonzero matrix entries
* `rowval` is the "i" or row entry for the corresponding value in nzval
  * length(rowval) == length(nzval)
* `colptr[j]` points into nzval and tells you the first nonzero in or after column j
* The last entry of colptr points beyond the end of nzval to indicate no more columns.
  * length(colptr) == number of columns + 1
"""

# ╔═╡ 3d4a702e-f75a-11ea-031c-333d591fc442
Dump(sparse(M))

# ╔═╡ 80ff4010-81bb-11eb-374e-215a57defb0b
md"""
 An example where CSC may not be a great choice is the following. The reason is that  `colptr` must have an entry in each column:
"""

# ╔═╡ 5de72b7c-f5d6-11ea-1b6f-35b830b5fb34
M2 = sparse([1, 2, 10^6], [4, 9, 10^6], [7, 8, 9])

# ╔═╡ 8b60629e-f5d6-11ea-27c8-d934460d3a57
with_terminal() do
	dump(M2)
end

# ╔═╡ 2fd7e52e-f5d7-11ea-3b5a-1f338e2451e0
M3 = [1 0 2 0 10; 0 3 4 0 9; 0 0 0 5 8; 0 0 0 0 7]

# ╔═╡ 2e87d4fe-81bc-11eb-0d16-b988bcedcc73
M4 = M3 .* 0

# ╔═╡ cde79f38-f5d6-11ea-3297-0b5b240f7b9e
Dump(sparse(M4))

# ╔═╡ aa09c008-f5d8-11ea-1bdc-b51ee6eb2478
sparse(M4)

# ╔═╡ 62a6ec62-f5d9-11ea-071e-ed33c5dea0cd
md"""# Random vectors
"""

# ╔═╡ 7f63daf6-f695-11ea-0b80-8702a83103a4
md"How much structure is there in a *random* vector?"

# ╔═╡ 67274c3c-f5d9-11ea-3475-c9d228e3bd5a
v = rand(1:9, 1_000_000)

# ╔═╡ 765c6552-f5d9-11ea-29d3-bfe7b4b04612
md"""You might guess that there is "no structure". But you can actually think of randomness itself as a structure.

For example, take the mean and standard deviation -- some would say that's the structure.

"""

# ╔═╡ 126fb3ea-f5da-11ea-2f7d-0b3259a296ce
mean(v), std(v), 5, sqrt(10 * 2/3)

# ╔═╡ ed0b2358-81ce-11eb-3339-93abcc06fd91
md"""
If you repeat the calculation, to 3 or 4 digits the mean and standard deviation don't change, and are approximately equal to the theoretical values on the right.
"""

# ╔═╡ 24ce92fa-81cf-11eb-30f0-b1e357d79d53
md"""
We can also count how many times each digit occurs in the data set:
"""

# ╔═╡ 2d4500e0-81cf-11eb-1699-d310074fddf5
[sum(v .== i) for i in 1:9]

# ╔═╡ 3546ff30-81cf-11eb-3afc-05c5db61366f
md"""
We see that each number occurs roughly the same number of times.
"""

# ╔═╡ 9b9e2c2a-f5da-11ea-369b-b513b196515b
md"Statisticians (and professors who've just graded exams) might say that under certain circumstances the mean and the variance give you the necessary structure, and the rest can be thrown away."

# ╔═╡ e68b98ea-f5da-11ea-1a9d-db45e4f80241
m = sum(v) / length(v)  # mean

# ╔═╡ f20ccac4-f5da-11ea-0e69-413b5e49f423
σ² = sum( (v .- m) .^ 2 ) / (length(v) - 1)

# ╔═╡ 12a2e96c-f5db-11ea-1c3e-494ae7446886
σ = sqrt(σ²)

# ╔═╡ 22487ce2-f5db-11ea-32e9-6f70ab2c0353
std(v)

# ╔═╡ 389ae62e-f5db-11ea-1557-c3adbbee0e5c
md"Sometimes the summary statistics are all you want. (But sometimes not.)"

# ╔═╡ 0c2b6408-f5d9-11ea-2b7f-7fece2eecc1f
md"# Multiplication tables"

# ╔═╡ 5d767290-f5dd-11ea-2189-81198fd216ce
outer(v, w) = [x * y for x ∈ v, y ∈ w]  # just a multiplication table

# ╔═╡ 587790ce-f6de-11ea-12d9-fde2a17ae314
outer(1:10, 1:10)

# ╔═╡ a39e8256-f6de-11ea-3170-c923b56609da
md"Did you memorize this in third grade?"

# ╔═╡ 8c84edd0-f6de-11ea-2180-61c6b81aac3b
@bind k Slider(1:14, show_value=true)

# ╔═╡ 22b73baa-f6df-11ea-197f-bbb4bd1a7ef5
outer(1:k, 1:k)

# ╔═╡ b2332814-f6e6-11ea-1c7d-556c7d4687f1
outer([2,4,6], [10,100,1000])

# ╔═╡ 9ab7a72e-81cf-11eb-2b78-073ff51cae58
md"""
A multiplication table is clearly a structure, but it's not sparse -- there are no zeros. Nonetheless you need much less information to reconstruct the matrix.
"""

# ╔═╡ fd8dd108-f6df-11ea-2f7c-3d99d054ac15
md"In the context of 1:k times 1:k, just one number k is needed."

# ╔═╡ 165788b2-f601-11ea-3e69-cdbbb6558e54
md"If you look at the following matrix? Does it have any structure? It's certainly more hidden."

# ╔═╡ 22941bb8-f601-11ea-1d6e-0d955297bc2e
outer( rand(3), rand(4) )  # but it's just a multiplication table

# ╔═╡ c33bf00e-81cf-11eb-1e1a-e5a879a45093
md"""
You might guess by visualizing the matrix that it is a multiplication table:
"""

# ╔═╡ 2f75df7e-f601-11ea-2fc2-aff4f335af33
show_image( outer( rand(10), rand(10) ))

# ╔═╡ 7ff664f0-f74b-11ea-0d2d-b53f19e4f4bf
md"We can factor out a multiplication table, if it's there:"

# ╔═╡ a0611eaa-81bc-11eb-1d23-c12ab14138b1
md"""
### Julia: Exceptions are thrown (generated) using `error`

An exception is anything that can interrupt a program, e.g. invalid input data.
"""

# ╔═╡ a4728944-f74b-11ea-03c3-9123908c1f8e
function factor( mult_table )
	v = mult_table[:, 1]
	w = mult_table[1, :]

	if v[1] ≠ 0
		w /= v[1]
	end

	# Good code has a check:
	if outer(v, w) ≈ mult_table
	   return v, w
	else
		error("Input is not a multiplication table")
	end
end

# ╔═╡ 05c35402-f752-11ea-2708-59cf5ef74fb4
factor( outer([1, 2, 3], [2, 2, 2] ) )

# ╔═╡ 8c11b19e-81bc-11eb-184b-bf6ffefe29de
md"""
A random 2x2 matrix is not a multiplication table. Most matrices are not given by multiplication tables.
"""

# ╔═╡ 8baaa994-f752-11ea-18d9-b3d0a6b9f7d9
factor( rand(2,2) )

# ╔═╡ d92bece4-f754-11ea-0242-99f198bb5b7b
md" Let's add two (or more) multiplication tables:"

# ╔═╡ e740999c-f754-11ea-2089-4b7a9aec6030
A = sum( outer(rand(3),rand(3)) for i=1:2 )

# ╔═╡ 0a79a7b4-f755-11ea-1b2d-21173567b257
md"Is it possible, given the matrix, to find the structure? E.g. to show that a matrix is a sum of outer products (multiplication table)."

# ╔═╡ 5adb98c2-f6e0-11ea-1fde-53b0fd6639c3
md"The answer is yes: The **Singular-Value Decomposition** (SVD) from algebra can find the structure!"

# ╔═╡ 487d6f9c-81d0-11eb-3bb0-336a4beb9b38
md"""
Let's take the SVD and calculate the sum of two outer products:
"""

# ╔═╡ 5a493052-f601-11ea-2f5f-f940412905f2
begin
	U, Σ, V = svd(A)

    outer( U[:, 1], V[:, 1] * Σ[1] ) + outer( U[:, 2], V[:, 2] * Σ[2] )
end

# ╔═╡ 55b76aee-81d0-11eb-0bcc-413f5bd14360
md"""
We see that we reconstruct the original matrix!"
"""

# ╔═╡ 709bf30a-f755-11ea-2e82-bd511e598c77
B = rand(3,3)

# ╔═╡ 782532b0-f755-11ea-1385-cd1a28c4b9d5
begin
	UU, ΣΣ, VV = svd( B )
    outer( UU[:,1], VV[:,1] * ΣΣ[1] ) + outer( UU[:,2], VV[:,2] * ΣΣ[2] )
end

# ╔═╡ 5bc4ab0a-f755-11ea-0fad-4987ad9fc02f
md"and it can approximate too!"

# ╔═╡ a5d637ea-f5de-11ea-3b70-877e876bc9c9
flag = outer([1,1,1,2,2,2,1,1,1], [1,1,1,1,1,1,1,1,1])

# ╔═╡ 21bbb60a-f5df-11ea-2c1b-dd716a657df8
cs = distinguishable_colors(100)

# ╔═╡ 2668e100-f5df-11ea-12b0-073a578a5edb
cs[flag]

# ╔═╡ e8d727f2-f5de-11ea-1456-f72602e81e0d
cs[flag + flag']

# ╔═╡ f5fcdeea-f75c-11ea-1fc3-731f0ef1ad14
outer([1,1,1,2,2,2,1,1,1], [1,1,1,1,1,1,1,1,1]) + outer([1,1,1,1,1,1,1,1,1], [1,1,1,2,2,2,1,1,1])

# ╔═╡ 0373fbf6-f75d-11ea-2a9e-cbb714d69cf4
cs[outer([1,1,1,2,2,2,1,1,1], [1,1,1,1,1,1,1,1,1]) + outer([1,1,1,1,1,1,1,1,1], [1,1,1,2,2,2,1,1,1])]

# ╔═╡ ebd72fb8-f5e0-11ea-0630-573337dff753
md"""
# Singular Value Decomposition (SVD): A tool to find structure
"""

# ╔═╡ b6478e1a-f5f6-11ea-3b92-6d4f067285f4
tree_url = "https://user-images.githubusercontent.com/6933510/110924885-d7f1b200-8322-11eb-9df7-7abf29c8db7d.png"

# ╔═╡ f2c11f88-f5f8-11ea-3e02-c1d4fa22031e
image = load(download(tree_url))

# ╔═╡ 29062f7a-f5f9-11ea-2682-1374e7694e32
picture = Float64.(channelview(image));

# ╔═╡ 5471fd30-f6e2-11ea-2cd7-7bd48c42db99
size(picture)

# ╔═╡ 6156fd1e-f5f9-11ea-06a9-211c7ab813a4
pr, pg, pb = eachslice(picture, dims=1)

# ╔═╡ a9766e68-f5f9-11ea-0019-6f9d02050521
[RGB.(pr, 0, 0) RGB.(0, pg, 0) RGB.(0, 0, pb)]

# ╔═╡ 0c0ee362-f5f9-11ea-0f75-2d2810c88d65
begin
	Ur, Σr, Vr = svd(pr)
	Ug, Σg, Vg = svd(pg)
	Ub, Σb, Vb = svd(pb)
end;

# ╔═╡ b95ce51a-f632-11ea-3a64-f7c218b9b3c9
@bind n Slider(1:200, show_value=true)

# ╔═╡ 7ba6e6a6-f5fa-11ea-2bcd-616d5a3c898b
RGB.(sum(outer(Ur[:,i], Vr[:,i]) .* Σr[i] for i in 1:n),
	 sum(outer(Ug[:,i], Vg[:,i]) .* Σg[i] for i in 1:n),
	 sum(outer(Ub[:,i], Vb[:,i]) .* Σb[i] for i in 1:n))

# ╔═╡ 8df84fcc-f5d5-11ea-312f-bf2a3b3ce2ce
md"# Appendix"

# ╔═╡ 0edd7cca-834f-11eb-0232-ff0850027f76
md"## Syntax Learned"

# ╔═╡ 69be8194-81b7-11eb-0452-0bc8b9f22286
md"""
Syntax to be learned:

* A `struct` is a great way to embody structure.
* `dump` and `Dump`: to see what's inside a data structure.
* `Diagonal`, `sparse`
* `error` (throws an exception)
* `svd` (Singular Value Decomposition)
"""

# ╔═╡ 1c462f68-834f-11eb-1447-85848814769b
[ dump, Diagonal, error, svd]

# ╔═╡ 5813e1b2-f5ff-11ea-2849-a1def74fc065
begin
	show_image(M) = get.([ColorSchemes.rainbow], M ./ maximum(M))
	show_image(x::AbstractVector) = show_image(x')
end

# ╔═╡ Cell order:
# ╟─0db6ee04-81b7-11eb-330c-11b578b72c90
# ╟─ca1a1072-81b6-11eb-1fee-e7df687cc314
# ╟─b0ba5b8c-f5d1-11ea-1304-3f0e47f935fe
# ╠═864e1180-f693-11ea-080e-a7d5aabc9ca5
# ╟─261c4df2-f5d2-11ea-2c72-7d4b09c46098
# ╟─3cada3a0-81cc-11eb-04c8-bde26d36a84e
# ╠═fe2028ba-f6dc-11ea-0228-938a81a91ace
# ╟─8d2c6910-f5d4-11ea-1928-1baf09815687
# ╟─54649792-81cc-11eb-1038-9161a4037acf
# ╠═0a902426-f6dd-11ea-0ae4-fb0c47863fe7
# ╟─4794e860-81b7-11eb-2c91-8561c20f308a
# ╟─67827da8-81cc-11eb-300e-278104d2d958
# ╠═4624cd26-f5d3-11ea-1cf8-7d6555eae5fa
# ╟─9bdabef8-81cc-11eb-14a1-67a9a7d968c0
# ╠═397ac764-f5fe-11ea-20cc-8d7cab19d410
# ╟─a22dcd2c-81cc-11eb-1252-13ace134192d
# ╠═82c7046c-f5d3-11ea-04e2-ef7c0f4db5da
# ╟─b024c318-81cc-11eb-018c-e1f7830ff51b
# ╠═93bfe3ac-f756-11ea-20fb-8f7d586b42f3
# ╠═175039aa-f758-11ea-251a-5db57d7c4b32
# ╠═c2a4b0a2-81cc-11eb-37a7-db601a6ddfdf
# ╟─c5ed7d3e-81cc-11eb-3386-15b72db8155d
# ╟─e2e354a8-81b7-11eb-311a-35151063c2a7
# ╟─dc5a96ba-81cc-11eb-3189-25920df48afa
# ╠═af0d3c22-f756-11ea-37d6-11b630d2314a
# ╟─06e3a842-26b8-4417-9cf5-8a083ccdb264
# ╠═91172a3e-81b7-11eb-0953-9f5e0207f863
# ╠═4bbf3f58-f788-11ea-0d24-6b0fb070829e
# ╟─fe70d104-81b7-11eb-14d0-eb5237d8ea6c
# ╟─ef8f44b2-f5fc-11ea-1e4d-bd873cd39d6c
# ╟─fd9211c0-f5fc-11ea-1745-7f2dae88af9e
# ╠═f1154df8-f693-11ea-3b16-f32835fcc470
# ╠═982590d4-f5ff-11ea-3802-73292c75ad6c
# ╟─81c35324-f5d4-11ea-2338-9f982d38732c
# ╟─2cfda0dc-f5d5-11ea-16c4-b5a33b90e37f
# ╠═150432d4-f5d5-11ea-32b2-19a2a91d9637
# ╟─44215aa4-f695-11ea-260e-b564c6fbcd4a
# ╠═21328d1c-f5d5-11ea-288e-4171ad35326d
# ╟─75761cc0-81cd-11eb-1186-7d47debd68ca
# ╟─6bd8a886-f758-11ea-2587-870a3fa9d710
# ╟─4c533ac6-f695-11ea-3724-b955eaaeee49
# ╠═466901ea-f5d5-11ea-1db5-abf82c96eabf
# ╠═b38c4aae-f5d5-11ea-39b6-7b0c7d529019
# ╟─93e04ed8-81cd-11eb-214a-a761ef8c406f
# ╟─e90c55fc-f5d5-11ea-10f1-470ff772985d
# ╟─19775c3c-f5d6-11ea-15c2-89618e654a1e
# ╟─653792a8-f695-11ea-1ae0-43761c502583
# ╠═79c94d2a-f75a-11ea-031d-09d70d229e15
# ╟─10bc5d50-81b9-11eb-2ac7-354a6c6c826b
# ╠═77d6a952-81ba-11eb-24e3-cb6510a59455
# ╟─1f3ba55a-81b9-11eb-001f-593b9d8639ca
# ╠═3d4a702e-f75a-11ea-031c-333d591fc442
# ╟─80ff4010-81bb-11eb-374e-215a57defb0b
# ╠═5de72b7c-f5d6-11ea-1b6f-35b830b5fb34
# ╠═8b60629e-f5d6-11ea-27c8-d934460d3a57
# ╠═2fd7e52e-f5d7-11ea-3b5a-1f338e2451e0
# ╠═2e87d4fe-81bc-11eb-0d16-b988bcedcc73
# ╠═cde79f38-f5d6-11ea-3297-0b5b240f7b9e
# ╠═aa09c008-f5d8-11ea-1bdc-b51ee6eb2478
# ╟─62a6ec62-f5d9-11ea-071e-ed33c5dea0cd
# ╟─7f63daf6-f695-11ea-0b80-8702a83103a4
# ╠═67274c3c-f5d9-11ea-3475-c9d228e3bd5a
# ╟─765c6552-f5d9-11ea-29d3-bfe7b4b04612
# ╠═126fb3ea-f5da-11ea-2f7d-0b3259a296ce
# ╟─ed0b2358-81ce-11eb-3339-93abcc06fd91
# ╟─24ce92fa-81cf-11eb-30f0-b1e357d79d53
# ╠═2d4500e0-81cf-11eb-1699-d310074fddf5
# ╟─3546ff30-81cf-11eb-3afc-05c5db61366f
# ╟─9b9e2c2a-f5da-11ea-369b-b513b196515b
# ╠═e68b98ea-f5da-11ea-1a9d-db45e4f80241
# ╠═f20ccac4-f5da-11ea-0e69-413b5e49f423
# ╠═12a2e96c-f5db-11ea-1c3e-494ae7446886
# ╠═22487ce2-f5db-11ea-32e9-6f70ab2c0353
# ╟─389ae62e-f5db-11ea-1557-c3adbbee0e5c
# ╟─0c2b6408-f5d9-11ea-2b7f-7fece2eecc1f
# ╠═5d767290-f5dd-11ea-2189-81198fd216ce
# ╠═587790ce-f6de-11ea-12d9-fde2a17ae314
# ╟─a39e8256-f6de-11ea-3170-c923b56609da
# ╠═8c84edd0-f6de-11ea-2180-61c6b81aac3b
# ╠═22b73baa-f6df-11ea-197f-bbb4bd1a7ef5
# ╠═b2332814-f6e6-11ea-1c7d-556c7d4687f1
# ╟─9ab7a72e-81cf-11eb-2b78-073ff51cae58
# ╟─fd8dd108-f6df-11ea-2f7c-3d99d054ac15
# ╟─165788b2-f601-11ea-3e69-cdbbb6558e54
# ╠═22941bb8-f601-11ea-1d6e-0d955297bc2e
# ╟─c33bf00e-81cf-11eb-1e1a-e5a879a45093
# ╠═2f75df7e-f601-11ea-2fc2-aff4f335af33
# ╟─7ff664f0-f74b-11ea-0d2d-b53f19e4f4bf
# ╟─a0611eaa-81bc-11eb-1d23-c12ab14138b1
# ╠═a4728944-f74b-11ea-03c3-9123908c1f8e
# ╠═05c35402-f752-11ea-2708-59cf5ef74fb4
# ╟─8c11b19e-81bc-11eb-184b-bf6ffefe29de
# ╠═8baaa994-f752-11ea-18d9-b3d0a6b9f7d9
# ╟─d92bece4-f754-11ea-0242-99f198bb5b7b
# ╠═e740999c-f754-11ea-2089-4b7a9aec6030
# ╟─0a79a7b4-f755-11ea-1b2d-21173567b257
# ╟─5adb98c2-f6e0-11ea-1fde-53b0fd6639c3
# ╟─487d6f9c-81d0-11eb-3bb0-336a4beb9b38
# ╟─5a493052-f601-11ea-2f5f-f940412905f2
# ╟─55b76aee-81d0-11eb-0bcc-413f5bd14360
# ╠═709bf30a-f755-11ea-2e82-bd511e598c77
# ╠═782532b0-f755-11ea-1385-cd1a28c4b9d5
# ╟─5bc4ab0a-f755-11ea-0fad-4987ad9fc02f
# ╠═a5d637ea-f5de-11ea-3b70-877e876bc9c9
# ╠═21bbb60a-f5df-11ea-2c1b-dd716a657df8
# ╠═2668e100-f5df-11ea-12b0-073a578a5edb
# ╠═e8d727f2-f5de-11ea-1456-f72602e81e0d
# ╠═f5fcdeea-f75c-11ea-1fc3-731f0ef1ad14
# ╠═0373fbf6-f75d-11ea-2a9e-cbb714d69cf4
# ╟─ebd72fb8-f5e0-11ea-0630-573337dff753
# ╠═b6478e1a-f5f6-11ea-3b92-6d4f067285f4
# ╠═f2c11f88-f5f8-11ea-3e02-c1d4fa22031e
# ╠═29062f7a-f5f9-11ea-2682-1374e7694e32
# ╠═5471fd30-f6e2-11ea-2cd7-7bd48c42db99
# ╠═6156fd1e-f5f9-11ea-06a9-211c7ab813a4
# ╠═a9766e68-f5f9-11ea-0019-6f9d02050521
# ╠═0c0ee362-f5f9-11ea-0f75-2d2810c88d65
# ╠═b95ce51a-f632-11ea-3a64-f7c218b9b3c9
# ╠═7ba6e6a6-f5fa-11ea-2bcd-616d5a3c898b
# ╟─8df84fcc-f5d5-11ea-312f-bf2a3b3ce2ce
# ╟─0edd7cca-834f-11eb-0232-ff0850027f76
# ╟─69be8194-81b7-11eb-0452-0bc8b9f22286
# ╠═1c462f68-834f-11eb-1447-85848814769b
# ╟─5813e1b2-f5ff-11ea-2849-a1def74fc065


using LinearAlgebra
@testset "indexing" begin
    A1 = rand(10)
    A2 = rand(10,10)
    B1 = Variable(A1)
    B2 = Variable(A2)

    ind = Array{Any}(undef, 4)
    ind[1] = 3
    ind[2] = 2:4
    ind[3] = zeros(Bool,10)
    ind[3][3:4] .= true
    ind[4] = [3;4;5]
    init(sess)

    for i = 1:4
        @test A1[ind[i]] ≈ run(sess, B1[ind[i]])
        @test A2[ind[i], :] ≈ run(sess, B2[ind[i], :])
        @test A2[:, ind[i]] ≈ run(sess, B2[:, ind[i]])
    end

    for i = 1:4
        for j = 1:4
            @test A2[ind[i], ind[j]] ≈ run(sess, B2[ind[i], ind[j]])
        end
    end
end

@testset "Variables" begin
    # @test_nowarn placeholder(Float64, shape=[nothing,20], name="myvar")
    # @test_nowarn Variable(rand(10,10), dtype=Float64, name="myvar")
    # @test_nowarn constant(rand(10,10), dtype=Float64, name="myvar")
    # @test_nowarn get_variable("W", shape=[10,20], dtype=Float64)
    # @test_nowarn placeholder(Float64, shape=[nothing,20])
    # @test_nowarn Variable(rand(10,10), dtype=Float64)
    # @test_nowarn constant(rand(10,10), dtype=Float64)
    @test get_dtype(constant(rand(10,10))) == Float64
    @test get_dtype(Variable(rand(Int64,10,10))) == Int64
end

@testset "tensor" begin
    v = Variable(0.5)
    g = [1;v;0]
    G = [1 v 0; v 1 0; 0 0 (1-v)/2]
    gv = tensor(g;sparse=true)
    g_gv = gradients(2sum(gv), v)
    Gv = tensor(G;sparse=true)
    G_gv = gradients(sum(Gv), v)
    init(sess)
    @test run(sess, g_gv) ≈ 2.0
    @test run(sess, G_gv) ≈ 1.5
end

@testset "Hessian" begin
    x = Variable(randn(), dtype = Float32)
    y = Variable(randn(), dtype = Float32)
    g = stack([x,y])
    f = g[1]^2 + 2*g[1]*g[2] + 3*g[2]^2 + 4*g[1] + 5*g[2] + 6
    hess = hessian(f, g)
    gv = rand(Float32,2)
    hess_v =  hessian_vector(f, g, gv)
    val = [[2.;2.] [2.;6.]] * gv
    init(sess)
    @test run(sess, hess) ≈ [[2.;2.] [2.;6.]]
    @test run(sess, hess_v)≈val
end

@testset "Jacobian" begin
    xs = constant(rand(10))
    A = rand(20,10)
    ys = A*xs
    jac = gradients(ys, xs)
    @test run(sess, jac)≈A
end

@testset "gradients_v" begin
    xs = constant(1.0)
    ys = stack([xs,xs,xs,xs])
    gv = gradients(ys, xs)
    @test run(sess, gv)≈[1.0;1.0;1.0;1.0]
end

@testset "size and length" begin
    a = Variable(1.0)
    b = Variable(rand(10))
    c = Variable(rand(10,20))
    @test size(a)==()
    @test size(b)==(10,)
    @test size(b,1)==10
    @test size(c) == (10,20)
    @test size(c,1)==10
    @test size(c,2)==20
    @test length(a)==1
    @test length(b)==10
    @test length(c)==200
end

@testset "copy" begin
    o = constant(rand(10,10))
    v = copy(o)
    init(sess)
    @test run(sess, v)≈run(sess, o)
end

@testset "getindex" begin
     i = constant(2)
     a = constant([1.;2.;3.;4.])
     @test run(sess, a[i])≈2.0
end

@testset "convert_to_tensor" begin
    a = rand(10)
    @test run(sess, convert_to_tensor(a))≈a
    a = 5.0
    @test run(sess, convert_to_tensor(a))≈a
    a = constant(rand(10))
    @test run(sess, convert_to_tensor(a))≈run(sess, a)
    @test ismissing(convert_to_tensor(missing))
    @test isnothing(convert_to_tensor(nothing))
end

@testset "cell" begin
    r = cell([[1.],[2.,3.]])
    @test run(sess, r[1])≈[1.0]
    @test run(sess, r[2])≈[2.,3.]
end

@testset "special matrices" begin
    A = rand(10,10)
    @test run(sess, sym(A)) ≈ 0.5 * (A + A')
    @test all(eigvals(run(sess, spd(A))) .> 0.0)
end


using Benchmark

################################################################################
# Test how matrix operations performance compared with elementwise operations
# when @inbounds annotation is turned on.
#
# | Row | Function         | Average     | Relative | Replications |
# |-----|------------------|-------------|----------|--------------|
# | 1   | "softmax_matrix" | 0.00168359  | 2.63545  | 50           |
# | 2   | "softmax_elem"   | 0.000638823 | 1.0      | 50           |
################################################################################

function softmax_matrix(input, the_output)
  output = copy(input)
  output .-= maximum(output, 3) # subtract max along channel dimension
  output = exp(output)
  output ./= sum(output, 3) # normalize along channel dimension
  copy!(the_output, output)
end

function softmax_elem(input, output)
  width, height, channels, num = size(input)

  for w = 1:width
    for h = 1:height
      for n = 1:num
        maxval = -Inf
        @simd for c = 1:channels
          @inbounds maxval = max(maxval, input[w,h,c,n])
        end
        @simd for c = 1:channels
          @inbounds output[w,h,c,n] = exp(input[w,h,c,n]-maxval)
        end
        the_sum = 0.0
        @simd for c = 1:channels
          @inbounds the_sum += output[w,h,c,n]
        end
        @simd for c = 1:channels
          @inbounds output[w,h,c,n] /= the_sum
        end
      end
    end
  end
end

input = rand(5, 5, 10, 128)
o1 = rand(size(input))
o2 = rand(size(input))
softmax_matrix() = softmax_matrix(input, o1)
softmax_elem() = softmax_elem(input, o2)

softmax_matrix()
softmax_elem()
@assert all(abs(o1-o2) .< 1e-10)

df = compare([softmax_matrix, softmax_elem], 50)
println("$df")


# ---
# title: 753. Cracking the Safe
# id: problem753
# author: Tian Jun
# date: 2020-10-31
# difficulty: Hard
# categories: Math, Depth-first Search
# link: <https://leetcode.com/problems/cracking-the-safe/description/>
# hidden: true
# ---
#
# There is a box protected by a password. The password is a sequence of `n`
# digits where each digit can be one of the first `k` digits `0, 1, ..., k-1`.
#
# While entering a password, the last `n` digits entered will automatically be
# matched against the correct password.
#
# For example, assuming the correct password is `"345"`, if you type `"012345"`,
# the box will open because the correct password matches the suffix of the
# entered password.
#
# Return any password of **minimum length** that is guaranteed to open the box
# at some point of entering it.
#
#
#
# **Example 1:**
#
#
#
#     Input: n = 1, k = 2
#     Output: "01"
#     **Note:** "10" will be accepted too.
#
#
# **Example 2:**
#
#
#
#     Input: n = 2, k = 2
#     Output: "00110"
#     **Note:** "01100", "10011", "11001" will be accepted too.
#
#
#
#
# **Note:**
#
#   1. `n` will be in the range `[1, 4]`.
#   2. `k` will be in the range `[1, 10]`.
#   3. `k^n` will be at most `4096`.
#
#
#
#
## @lc code=start
using LeetCode

## add your code here:
## @lc code=end


import LinearAlgebra: Schur, GeneralizedSchur

export Near, SmallestMagnitude, LargestMagnitude,
       LargestRealPart, SmallestRealPart,
       LargestImaginaryPart, SmallestImaginaryPart,
       schur_permutation, schur_sort!

abstract type Target end

# For finding eigenvalues near a target
struct Near{T} <: Target
  τ::Complex{T}
end

# For finding eigenvalues with the smallest magnitude
struct SmallestMagnitude{T} <: Target
  τ::Complex{T}
end

# For finding eigenvalues with the largest magnitude
struct LargestMagnitude{T} <: Target
  τ::Complex{T}
end

# For finding eigenvalues with the largest real part
struct LargestRealPart{T} <: Target
  τ::Complex{T}
end

# For finding eigenvalues with the smallest real part
struct SmallestRealPart{T} <: Target
  τ::Complex{T}
end

# For finding eigenvalues with the largest imaginary part
struct LargestImaginaryPart{T} <: Target
  τ::Complex{T}
end

# For finding eigenvalues with the smallest imaginary part
struct SmallestImaginaryPart{T} <: Target
  τ::Complex{T}
end

eigval(F::GeneralizedSchur, idx::Int) = F.alpha[idx] / F.beta[idx]

schur_permutation(target::Near, F::GeneralizedSchur, k::OrdinalRange) =
  partialsortperm(1 : length(F.alpha), k, by = idx -> abs(eigval(F, idx) - target.τ))

schur_permutation(target::LargestMagnitude, F::GeneralizedSchur, k::OrdinalRange) =
  partialsortperm(1 : length(F.alpha), k, by = idx -> abs(eigval(F, idx)), rev = true)

schur_permutation(target::SmallestMagnitude, F::GeneralizedSchur, k::OrdinalRange) =
  partialsortperm(1 : length(F.alpha), k, by = idx -> abs(eigval(F, idx)))

schur_permutation(target::LargestRealPart, F::GeneralizedSchur, k::OrdinalRange) =
  partialsortperm(1 : length(F.alpha), k, by = idx -> real(eigval(F, idx)), rev = true)

schur_permutation(target::SmallestRealPart, F::GeneralizedSchur, k::OrdinalRange) =
  partialsortperm(1 : length(F.alpha), k, by = idx -> real(eigval(F, idx)))

schur_permutation(target::LargestImaginaryPart, F::GeneralizedSchur, k::OrdinalRange) =
  partialsortperm(1 : length(F.alpha), k, by = idx -> imag(eigval(F, idx)), rev = true)

schur_permutation(target::SmallestImaginaryPart, F::GeneralizedSchur, k::OrdinalRange) =
  partialsortperm(1 : length(F.alpha), k, by = idx -> imag(eigval(F, idx)))

function schur_sort!(target::Target, F::GeneralizedSchur, k::OrdinalRange)
  best = vec(schur_permutation(target, F, k))
  perm = falses(length(F.alpha))
  perm[best] .= true
  ordschur!(F, perm)
end

schur_sort!(target::Target, F::GeneralizedSchur, k::Int) =
    schur_sort!(target, F, k:k)


module InternalForces

using DiffEqOperators: CenteredDifference

export moment, shear, torsion, bimoment, calculateDerivativeOperators

#############################################################
# Fornberg algorithm

# This implements the Fornberg (1988) algorithm (https://doi.org/10.1090/S0025-5718-1988-0935077-0)
# to obtain Finite Difference weights over arbitrary points to arbitrary order

function calculate_weights(order::Int, x0::T, x::AbstractVector) where T<:Real
    #=
        order: The derivative order for which we need the coefficients
        x0   : The point in the array 'x' for which we need the coefficients
        x    : A dummy array with relative coordinates, eg. central differences
               need coordinates centred at 0 while those at boundaries need
               coordinates starting from 0 to the end point
        The approximation order of the stencil is automatically determined from
        the number of requested stencil points.
    =#
    N = length(x)
    @assert order < N "Not enough points for the requested order."
    M = order
    c1 = one(T)
    c4 = x[1] - x0
    C = zeros(T, N, M+1)
    C[1,1] = 1
    @inbounds for i in 1 : N-1
        i1 = i + 1
        mn = min(i, M)
        c2 = one(T)
        c5 = c4
        c4 = x[i1] - x0
        for j in 0 : i-1
            j1 = j + 1
            c3 = x[i1] - x[j1]
            c2 *= c3
            if j == i-1
                for s in mn : -1 : 1
                    s1 = s + 1
                    C[i1,s1] = c1*(s*C[i,s] - c5*C[i,s1]) / c2
                end
                C[i1,1] = -c1*c5*C[i,1] / c2
           end
            for s in mn : -1 : 1
                s1 = s + 1
                C[j1,s1] = (c4*C[j1,s1] - s*C[j1,s]) / c3
            end
            C[j1,1] = c4 * C[j1,1] / c3
        end
        c1 = c2
    end
    #=
        This is to fix the problem of numerical instability which occurs when the sum of the stencil_coefficients is not
        exactly 0.
        https://scicomp.stackexchange.com/questions/11249/numerical-derivative-and-finite-difference-coefficients-any-update-of-the-fornb
        Stack Overflow answer on this issue.
        http://epubs.siam.org/doi/pdf/10.1137/S0036144596322507 - Modified Fornberg Algorithm
    =#
    _C = C[:,end]
    _C[div(N,2)+1] -= sum(_C)
    return _C
end

function operatorjumps(z, dm, A, order)

    diffdm = diff(dm)
    jumps = findall(x-> x > 0.0, diffdm)

    for i in eachindex(jumps)

        #left pointing single sided stencil, left of jump
        x = z[jumps[i]-4-1:jumps[i]-1]
        x0 = z[jumps[i]-1]
        coeffs=calculate_weights(order, x0, x)
        A[jumps[i]-1, 1:end] .= 0.0
        A[jumps[i]-1, jumps[i]-4-1:jumps[i]-1] = coeffs

        #left pointing single sided stencil, at jump
        x = z[jumps[i]-4:jumps[i]]
        x0 = z[jumps[i]]
        coeffs=calculate_weights(order, x0, x)
        A[jumps[i], 1:end] .= 0.0
        A[jumps[i], jumps[i]-4:jumps[i]] = coeffs

        # right pointing single sided stencil, right of jump
        x = z[jumps[i]+1:jumps[i]+1 + 4]
        x0 = z[jumps[i]+1]
        coeffs=calculate_weights(order, x0, x)
        A[jumps[i]+1, 1:end] .= 0.0
        A[jumps[i]+1, jumps[i]+1:jumps[i]+4+1] = coeffs

        # right pointing single sided stencil, two right of jump
        x = z[jumps[i]+2:jumps[i]+2 + 4]
        x0 = z[jumps[i]+2]
        coeffs=calculate_weights(order, x0, x)
        A[jumps[i]+2, 1:end] .= 0.0
        A[jumps[i]+2, jumps[i]+2:jumps[i]+4+2] = coeffs

    end

    return A

end

function calculateDerivativeOperators(z, dm)

    NumberOfNodes=length(z)
    dz=diff(z)
    dz=[dz[1]; dz; dz[end]]
    dL=dz

    #first derivative

    NthDerivative = 1
    DerivativeOrder = 2
    Az = CenteredDifference(NthDerivative, DerivativeOrder, dL, NumberOfNodes)
    Az=Array(Az)
    Az=Az[:,2:end-1]  #remove ghost nodes
    Az[1,:]=zeros(NumberOfNodes)
    Az[2,:]=zeros(NumberOfNodes)
    Az[(end-1),:]=zeros(NumberOfNodes)
    Az[end,:]=zeros(NumberOfNodes)

    #update boundary condition with single sided stencils
    order=1

    x=z[1:5]
    x0=x[1]
    coeffs=calculate_weights(order, x0, x)
    Az[1,1:5]=coeffs

    x=z[1:5]
    x0=x[2]
    coeffs=calculate_weights(order, x0, x)
    Az[2,1:5]=coeffs

    x=z[end-4:end]
    x0=x[end-1]
    coeffs=calculate_weights(order, x0, x)
    Az[end-1,end-4:end]=coeffs

    x=z[end-4:end]
    x0=x[end]
    coeffs=calculate_weights(order, x0, x)
    Az[end,end-4:end]=coeffs

    #place singled sided stencils at discontinuities
    Az = operatorjumps(z, dm, Az, order)

    #second derivative

    NthDerivative = 2
    DerivativeOrder = 2
    Azz = CenteredDifference(NthDerivative, DerivativeOrder, dL, NumberOfNodes)
    Azz=Array(Azz)
    Azz=Azz[:,2:end-1]
    Azz[1,:]=zeros(NumberOfNodes)
    Azz[2,:]=zeros(NumberOfNodes)
    Azz[(end-1),:]=zeros(NumberOfNodes)
    Azz[end,:]=zeros(NumberOfNodes)

    order=2   #update stencil to consider boundary conditions without ghost nodes

    x=z[1:5]
    x0=x[1]
    coeffs=calculate_weights(order, x0, x)
    Azz[1,1:5]=coeffs

    x=z[1:5]
    x0=x[2]
    coeffs=calculate_weights(order, x0, x)
    Azz[2,1:5]=coeffs

    x=z[end-4:end]
    x0=x[end-1]
    coeffs=calculate_weights(order, x0, x)
    Azz[end-1,end-4:end]=coeffs

    x=z[end-4:end]
    x0=x[end]
    coeffs=calculate_weights(order, x0, x)
    Azz[end,end-4:end]=coeffs

    #place singled sided stencils at discontinuities
    Azz = operatorjumps(z, dm, Azz, order)

    #third derivative

    NthDerivative = 3
    DerivativeOrder = 2
    Azzz = CenteredDifference(NthDerivative, DerivativeOrder, dL, NumberOfNodes)
    Azzz=Array(Azzz)
    Azzz=Azzz[:,2:end-1]
    Azzz[1,:]=zeros(NumberOfNodes)
    Azzz[2,:]=zeros(NumberOfNodes)
    Azzz[(end-1),:]=zeros(NumberOfNodes)
    Azzz[end,:]=zeros(NumberOfNodes)

    order=3

    x=z[1:5]
    x0=x[1]
    coeffs=calculate_weights(order, x0, x)
    Azzz[1,1:5]=coeffs

    x=z[1:5]
    x0=x[2]
    coeffs=calculate_weights(order, x0, x)
    Azzz[2,1:5]=coeffs

    x=z[end-4:end]
    x0=x[end-1]
    coeffs=calculate_weights(order, x0, x)
    Azzz[end-1,end-4:end]=coeffs

    x=z[end-4:end]
    x0=x[end]
    coeffs=calculate_weights(order, x0, x)
    Azzz[end,end-4:end]=coeffs

    #place singled sided stencils at discontinuities
    Azzz = operatorjumps(z, dm, Azzz, order)

    return Az, Azz, Azzz

end

function moment(z, dm, Δ, E, I)

    Az, Azz, Azzz = calculateDerivativeOperators(z, dm)

    M = E .* I .* Azz * Δ

    return M

end

function shear(z, dm, Δ, E, I)

    Az, Azz, Azzz = calculateDerivativeOperators(z, dm)

    V = E .* I .* Azzz * Δ

    return V

end

function torsion(z, dm, ϕ, E, G, J, Cw)

    Az, Azz, Azzz = calculateDerivativeOperators(z, dm)

    #T=GJ*dϕ/dz - ECw*dϕ3/dz3
    T=G.*J.*Az*ϕ .- E.*Cw.*Azzz*ϕ

    return T

end

function bimoment(z, dm, ϕ, E, Cw)

    Az, Azz, Azzz = calculateDerivativeOperators(z, dm)

    #B=ECwϕ''
    B=E.*Cw.*Azz*ϕ

    return B

end

end #module


function train!(method::KernelInterpolation)
    X = method.trainingData.Xs
    Fs = method.trainingData.Fs
    k = method.kernel

    N, D = size(X)

    A = ones(N+1, N+1)

    y = zeros(N+1)
    y[1:N] = Fs

    for i = 1:N
        for j = 1:N
            A[i, j] = kernel(k, view(X, i,:), view(X, j,:))
        end

        A[i, i] += method.λ
    end

    A[N+1, N+1] = 0.0
    method.kernelMatrix = A
    method.kernelMatrixInv = inv(A)

    # y = Ab
    # b = A⁻¹ ⋅ y

    b = method.kernelMatrixInv * y

    method.coeffs = b
    method
end

train(method::KernelInterpolation) = train!(method)

function evaluate(x::Real, method::KernelInterpolation)
    if length(method.coeffs) == 0
        @info("Training method...")
        train!(method)
    end

    evaluate(Float64[x], method)
end

function evaluate(x, method::KernelInterpolation)
    if length(method.coeffs) == 0
        @info("Training method...")
        train!(method)
    end

    X = method.trainingData.Xs
    k = method.kernel

    s = 0.0
    for i = 1:size(X,1)
        s += kernel(k,x, view(X, i,:)) * method.coeffs[i]
    end

    s + method.coeffs[end]
end

function evaluate(X::Array{Float64, 2}, method::KernelInterpolation)
    if length(method.coeffs) == 0
        @info("Training method...")
        train!(method)
    end

    Float64[ evaluate(view(X, i,:), method) for i=1:size(X,1) ]
end


import Base: +, -, *, /, \, exp, log, ≈, inv, conj
import LinearAlgebra: norm2

export
    Rotation,
    UnitQuaternion,
    MRP,
    RPY,
    RodriguesParam,
    ExponentialMap,
    VectorPart,
    MRPMap,
    CayleyMap,
    IdentityMap,
    ReNorm

export
    differential_rotation,
    retraction_map,
    scalar,
    vector,
    logm,
    expm,
    kinematics,
    rotmat,
    Lmult,
    Rmult,
    Vmat,
    Hmat,
    Tmat,
    skew,
    vee,
    ⊕,
    ⊖

function skew(v::AbstractVector)
    @assert length(v) == 3
    @SMatrix [0   -v[3]  v[2];
              v[3] 0    -v[1];
             -v[2] v[1]  0]
end

function vee(S::AbstractMatrix)
    return @SVector [S[3,2], S[1,3], S[2,1]]
end

abstract type Rotation end

abstract type DifferentialRotation end
abstract type VectorPart <: DifferentialRotation end
abstract type ExponentialMap <: DifferentialRotation end
abstract type MRPMap <: DifferentialRotation end
abstract type CayleyMap <: DifferentialRotation end
abstract type IdentityMap <: DifferentialRotation end
abstract type ReNorm <: DifferentialRotation end

# Scalings
@inline scaling(::Type{ExponentialMap}) = 0.5
@inline scaling(::Type{VectorPart}) = 1.0
@inline scaling(::Type{CayleyMap}) = 1.0
@inline scaling(::Type{MRPMap}) = 2.0

# Retraction Maps
(::Type{ExponentialMap})(ϕ) = expm(ϕ/scaling(ExponentialMap))

function (::Type{VectorPart})(v)
    μ = 1/scaling(VectorPart)
    UnitQuaternion{VectorPart}(sqrt(1-μ^2*v'v), μ*v[1], μ*v[2], μ*v[3])
end

function (::Type{CayleyMap})(g)
    g /= scaling(CayleyMap)
    M = 1/sqrt(1+g'g)
    UnitQuaternion{CayleyMap}(M, M*g[1], M*g[2], M*g[3])
end

function (::Type{MRPMap})(p)
    p /= scaling(MRPMap)
    n2 = p'p
    M = 2/(1+n2)
    UnitQuaternion{MRPMap}((1-n2)/(1+n2), M*p[1], M*p[2], M*p[3])
end

(::Type{IdentityMap})(q) = UnitQuaternion{IdentityMap}(q[1], q[2], q[3], q[4])

# Retraction Map Jacobians
function jacobian(::Type{ExponentialMap},ϕ, eps=1e-5)
    μ = 1/scaling(ExponentialMap)
    θ = norm(ϕ)
    cθ = cos(μ*θ/2)
    sincθ = sinc(μ*θ/2π)
    if θ < eps
        0.5*μ*[-0.5*μ*sincθ*ϕ'; sincθ*I + (cθ - sincθ)*ϕ*ϕ']
    else
        0.5*μ*[-0.5*μ*sincθ*ϕ'; sincθ*I + (cθ - sincθ)*ϕ*ϕ'/(ϕ'ϕ)]
    end
end

function jacobian(::Type{VectorPart}, v)
    μ = 1/scaling(VectorPart)
    μ2 = μ*μ
    M = -μ2/sqrt(1-μ2*v'v)
    @SMatrix [v[1]*M v[2]*M v[3]*M;
              μ 0 0;
              0 μ 0;
              0 0 μ]
end

function jacobian(::Type{CayleyMap}, g)
    μ = 1/scaling(CayleyMap)
    μ2 = μ*μ
    n = 1+μ2*g'g
    ni = 1/n
    μ*[-μ*g'; -μ2*g*g' + I*n]*ni*sqrt(ni)
end

function jacobian(::Type{MRPMap}, p)
    μ = 1/scaling(MRPMap)
    μ2 = μ*μ
    n = 1+μ2*p'p
    2*[-2*μ2*p'; I*μ*n - 2*μ*μ2*p*p']/n^2
end

jacobian(::Type{IdentityMap}, q) = I

const DEFAULT_QUATDIFF = VectorPart

""" $(TYPEDEF)
4-parameter attitute representation that is singularity-free. Quaternions with unit norm
represent a double-cover of SO(3). The `UnitQuaternion` does NOT strictly enforce the unit
norm constraint, but certain methods will assume you have a unit quaternion. The
`UnitQuaternion` type is parameterized by the linearization method, which maps quaternions
to the 3D plane tangent to the 4D unit sphere. Follows the Hamilton convention for quaternions.

There are currently 4 methods supported:
* `VectorPart` - uses the vector (or imaginary) part of the quaternion
* `ExponentialMap` - the most common approach, uses the exponential and logarithmic maps
* `CayleyMap` - or Rodrigues parameters (aka Gibbs vectors).
* `MRPMap` - or Modified Rodrigues Parameter, is a sterographic projection of the 4D unit sphere
onto the plane tangent to either the positive or negative real poles.

# Constructors
```julia
UnitQuaternion(s,x,y,z)  # defaults to `VectorPart`
UnitQuaternion{D}(s,x,y,z)
UnitQuaternion{D}(q::SVector{4})
UnitQuaternion{D}(r::SVector{3})  # quaternion with 0 real part
```
"""
struct UnitQuaternion{T,D<:DifferentialRotation} <: Rotation
    s::T
    x::T
    y::T
    z::T
end

UnitQuaternion(s::T,x::T,y::T,z::T) where T = UnitQuaternion{T,DEFAULT_QUATDIFF}(s,x,y,z)
UnitQuaternion(q::SVector{4}) = UnitQuaternion{DEFAULT_QUATDIFF}(q[1],q[2],q[3],q[4])
UnitQuaternion{D}(s::T,x::T,y::T,z::T) where {T,D} = UnitQuaternion{T,D}(s,x,y,z)

UnitQuaternion{D}(q::SVector{4}) where D = UnitQuaternion{D}(q[1],q[2],q[3],q[4])
UnitQuaternion{D}(r::SVector{3}) where D = UnitQuaternion{D}(0.0, r[1],r[2],r[3])
UnitQuaternion{D}(q::UnitQuaternion) where D = UnitQuaternion{D}(q.s, q.x, q.y, q.z)
UnitQuaternion{T,D}(q::R) where {T,D,R <: UnitQuaternion} =
    UnitQuaternion{T,D}(q.s, q.x, q.y, q.z)

UnitQuaternion(r::SVector{3}) = UnitQuaternion{DEFAULT_QUATDIFF}(0.0, r[1],r[2],r[3])
UnitQuaternion(q::UnitQuaternion) = q

(::Type{UnitQuaternion{T,D}})(x::SVector{4,T2}) where {T,T2,D} =
    UnitQuaternion{promote_type(T,T2),D}(x[1], x[2], x[3], x[4])

retraction_map(::UnitQuaternion{T,D}) where {T,D} = D
retraction_map(::Type{UnitQuaternion{T,D}}) where {T,D} = D

Base.rand(::Type{<:UnitQuaternion{T,D}}) where {T,D} =
    normalize(UnitQuaternion{T,D}(randn(T), randn(T), randn(T), randn(T)))

Base.rand(::Type{UnitQuaternion{T}}) where T = Base.rand(UnitQuaternion{T,VectorPart})
Base.rand(::Type{UnitQuaternion}) = Base.rand(UnitQuaternion{Float64,VectorPart})
Base.zero(::Type{Q}) where Q<:UnitQuaternion = I(Q)
Base.zero(q::Q) where Q<:UnitQuaternion = I(Q)

SVector(q::UnitQuaternion{T}) where T = SVector{4,T}(q.s, q.x, q.y, q.z)

scalar(q::UnitQuaternion) = q.s
vector(q::UnitQuaternion{T}) where T = SVector{3,T}(q.x, q.y, q.z)
vecnorm(q::UnitQuaternion) = sqrt(q.x^2 + q.y^2 + q.z^2)

conj(q::UnitQuaternion{T,D}) where {T,D} = UnitQuaternion{T,D}(q.s, -q.x, -q.y, -q.z)
inv(q::UnitQuaternion) = conj(q)

LinearAlgebra.norm2(q::UnitQuaternion) = q.s^2 + q.x^2 + q.y^2 + q.z^2
LinearAlgebra.norm(q::UnitQuaternion) = sqrt(q.s^2 + q.x^2 + q.y^2 + q.z^2)
LinearAlgebra.I(::Type{UnitQuaternion}) = UnitQuaternion(1.0, 0.0, 0.0, 0.0)
LinearAlgebra.I(::Type{Q}) where Q <: UnitQuaternion = UnitQuaternion(1.0, 0.0, 0.0, 0.0)
(::Type{Q})(I::UniformScaling) where Q<:UnitQuaternion = Q(1.0, 0.0, 0.0, 0.0)
(≈)(q::UnitQuaternion, u::UnitQuaternion) = q.s ≈ u.s && q.x ≈ u.x && q.y ≈ u.y && q.z ≈ u.z
(-)(q::UnitQuaternion{T,D}) where {T,D} = UnitQuaternion{T,D}(-q.s, -q.x, -q.y, -q.z)

function LinearAlgebra.normalize(q::UnitQuaternion{T,D}) where {T,D}
    n = norm(q)
    UnitQuaternion{T,D}(q.s/n, q.x/n, q.y/n, q.z/n)
end

function (*)(q::UnitQuaternion{T1,D1}, w::UnitQuaternion{T2,D2}) where {T1,T2,D1,D2}
    T = promote_type(T1, T2)
    D = D2
    UnitQuaternion{T,D}(q.s * w.s - q.x * w.x - q.y * w.y - q.z * w.z,
                        q.s * w.x + q.x * w.s + q.y * w.z - q.z * w.y,
                        q.s * w.y - q.x * w.z + q.y * w.s + q.z * w.x,
                        q.s * w.z + q.x * w.y - q.y * w.x + q.z * w.s)
end

function Base.:*(q::UnitQuaternion{Tq}, r::SVector{3}) where Tq
    qo = (-q.x  * r[1] - q.y * r[2] - q.z * r[3],
           q.s  * r[1] + q.y * r[3] - q.z * r[2],
           q.s  * r[2] - q.x * r[3] + q.z * r[1],
           q.s  * r[3] + q.x * r[2] - q.y * r[1])

   T = promote_type(Tq, eltype(r))

   return similar_type(r, T)(-qo[1] * q.x + qo[2] * q.s - qo[3] * q.z + qo[4] * q.y,
                             -qo[1] * q.y + qo[2] * q.z + qo[3] * q.s - qo[4] * q.x,
                             -qo[1] * q.z - qo[2] * q.y + qo[3] * q.x + qo[4] * q.s)
end

function (*)(q::Q, s::Real) where Q<:UnitQuaternion
    return Q(q.s*s, q.x*s, q.y*s, q.z*s)
end

(\)(q1::UnitQuaternion, q2::UnitQuaternion) = inv(q1)*q2
(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*inv(q2)

(\)(q::UnitQuaternion, r::SVector{3}) = inv(q)*r

function exp(q::UnitQuaternion{T,D}) where {T,D}
    θ = vecnorm(q)
    sθ,cθ = sincos(θ)
    es = exp(q.s)
    M = es*sθ/θ
    UnitQuaternion{T,D}(es*cθ, q.x*M, q.y*M, q.z*M)
end

function expm(ϕ::SVector{3,T}) where T
    θ = norm(ϕ)
    sθ,cθ = sincos(θ/2)
    M = 0.5*sinc(θ/2π)
    UnitQuaternion{T,ExponentialMap}(cθ, ϕ[1]*M, ϕ[2]*M, ϕ[3]*M)
end

# Assumes unit quaternion
function log(q::UnitQuaternion{T,D}, eps=1e-6) where {T,D}
    θ = vecnorm(q)
    if θ > eps
        M = atan(θ, q.s)/θ
    else
        M = (1-(θ^2/(3q.s^2)))/q.s
    end
    UnitQuaternion{T,D}(0.0, q.x*M, q.y*M, q.z*M)
end

# Assumes unit quaternion
function logm(q::UnitQuaternion{T}) where T
    q = log(q)
    SVector{3,T}(2*q.x, 2*q.y, 2*q.z)
end

function rotmat(q::UnitQuaternion)
    s = q.s
    v = vector(q)
    (s^2 - v'v)*I + 2*v*v' + 2*s*skew(v)
end

function kinematics(q::UnitQuaternion{T,D}, ω::SVector{3}) where {T,D}
    SVector(q*UnitQuaternion{T,D}(0.0, 0.5*ω[1], 0.5*ω[2], 0.5*ω[3]))
end

function Base.angle(q::UnitQuaternion)
    min(2*atan(vecnorm(q), q.s), 2*atan(vecnorm(q), -q.s))
end

function (⊕)(q::UnitQuaternion{T,ExponentialMap}, δq::SVector{3}) where T
    q*expm(δq)
end

function (⊕)(q::UnitQuaternion{T,VectorPart}, δq::SVector{3}) where T
    q*UnitQuaternion{VectorPart}(sqrt(1 - δq[1]^2 - δq[2]^2 - δq[3]^2),
        δq[1], δq[2], δq[3])
end

function (⊖)(q::UnitQuaternion{T,D}, q0::UnitQuaternion) where {T,D}
    D(q0\q)
end

function (⊖)(q::UnitQuaternion{T,IdentityMap}, q0::UnitQuaternion) where {T}
    SVector(q) - SVector(q0)
    # return SVector(q0\q)
end

function rotation_between(from::SVector{3}, to::SVector{3})
    # Robustified version of implementation from https://www.gamedev.net/topic/429507-finding-the-quaternion-betwee-two-vectors/#entry3856228
    normprod = sqrt(dot(from, from) * dot(to, to))
    T = typeof(normprod)
    normprod < eps(T) && throw(ArgumentError("Input vectors must be nonzero."))
    w = normprod + dot(from, to)
    # v = abs(w) < 100 * eps(T) ? perpendicular_vector(from) : cross(from, to)
    v = from × to
    @inbounds return normalize(UnitQuaternion(w, v[1], v[2], v[3])) # relies on normalization in constructor
end

"""
Jacobian of q ⊕ ϕ, when ϕ is near zero. Useful for converting Jacobians from R⁴ to R³ and
    correctly account for unit norm constraint. Jacobians for different
    differential quaternion parameterization are the same up to a constant.
"""
function ∇differential(q::UnitQuaternion)
    1.0 * @SMatrix [
        -q.x -q.y -q.z;
         q.s -q.z  q.y;
         q.z  q.s -q.x;
        -q.y  q.x  q.s;
    ]
end

"Lmult(q2)q1 returns a vector equivalent to q2*q1 (quaternion multiplication)"
function Lmult(q::UnitQuaternion)
    @SMatrix [
        q.s -q.x -q.y -q.z;
        q.x  q.s -q.z  q.y;
        q.y  q.z  q.s -q.x;
        q.z -q.y  q.x  q.s;
    ]
end
Lmult(q::SVector{4}) = Lmult(UnitQuaternion(q))

"Rmult(q1)q2 return a vector equivalent to q2*q1 (quaternion multiplication)"
function Rmult(q::UnitQuaternion)
    @SMatrix [
        q.s -q.x -q.y -q.z;
        q.x  q.s  q.z -q.y;
        q.y -q.z  q.s  q.x;
        q.z  q.y -q.x  q.s;
    ]
end
Rmult(q::SVector{4}) = Rmult(UnitQuaternion(q))

"Tmat()q return a vector equivalent to inv(q)"
function Tmat()
    @SMatrix [
        1  0  0  0;
        0 -1  0  0;
        0  0 -1  0;
        0  0  0 -1;
    ]
end

"Vmat(q)q returns the imaginary (vector) part of the quaternion q (equivalent to vector(q))"
function Vmat()
    @SMatrix [
        0 1 0 0;
        0 0 1 0;
        0 0 0 1
    ]
end

function Hmat()
    @SMatrix [
        0 0 0;
        1 0 0;
        0 1 0;
        0 0 1.;
    ]
end

function Hmat(r)
    @assert length(r) == 3
    @SVector [0,r[1],r[2],r[3]]
end

"Jacobian of q*r with respect to the quaternion"
function ∇rotate(q::UnitQuaternion{T,D}, r::SVector{3}) where {T,D}
    rhat = UnitQuaternion{D}(r)
    R = Rmult(q)
    2Vmat()*Rmult(q)'Rmult(rhat)
end

"Jacobian of q2*q1 with respect to q1"
function ∇composition1(q2::UnitQuaternion, q1::UnitQuaternion)
    Lmult(q2)
end

"Jacobian of q2*q1 with respect to q2"
function ∇composition2(q2::UnitQuaternion, q1::UnitQuaternion)
    Rmult(q1)
end

function ∇²differential(q::UnitQuaternion, b::SVector{4})
    b1 = -SVector(q)'b
    Diagonal(@SVector fill(b1,3))
end

############################################################################################
#                             MODIFIED RODRIGUES PARAMETERS                                #
############################################################################################

""" $(TYPEDEF)
Modified Rodrigues Parameter. Is a 3D parameterization of attitude, and is a sterographic
projection of the 4D unit sphere onto the plane tangent to the negative real pole. They
have a singularity at θ = ±180°.

# Constructors
MRP(x, y, z)
MRP(r::SVector{3})
"""
struct MRP{T} <: Rotation
    x::T
    y::T
    z::T
end

MRP(r::SVector{3}) = MRP(r[1], r[2], r[3])
(::Type{<:MRP})(::Type{T},x,y,z) where T = MRP{T}(T(x),T(y),T(z))
MRP{T}(r::SVector{3}) where T = MRP(T(r[1]), T(r[2]), T(r[3]))
MRP{T}(p::MRP) where T = MRP(T(p.x), T(p.y), T(p.z))
function (::Type{<:MRP})(q::UnitQuaternion)
    M = 1/(1+q.s)
    MRP(q.x*M, q.y*M, q.z*M)
end

function UnitQuaternion(p::MRP)
    n = norm2(p)
    s = (1-n)/(1+n)
    M = 2/(1+n)
    UnitQuaternion{MRPMap}(s, p.x*M, p.y*M, p.z*M)
end

function rotmat(p::MRP)
    p = SVector(p)
    P = skew(p)
    R2 = I + 4( (1-p'p)I + 2*P )*P/(1+p'p)^2
end

SVector(p::MRP{T}) where T = SVector{3,T}(p.x, p.y, p.z)

Base.rand(::Type{MRP{T}}) where T = MRP(rand(UnitQuaternion{T}))
Base.rand(::Type{MRP}) = MRP(rand(UnitQuaternion))
Base.zero(::Type{MRP{T}}) where T = MRP(zero(T), zero(T), zero(T))
Base.zero(::Type{MRP}) = MRP(0.0, 0.0, 0.0)

LinearAlgebra.norm(p::MRP) = sqrt(p.x^2 + p.y^2 + p.z^2)
LinearAlgebra.norm2(p::MRP) = p.x^2 + p.y^2 + p.z^2

(≈)(p2::MRP, p1::MRP) = p2.x ≈ p1.x && p2.y ≈ p1.y && p2.z ≈ p1.z

function (*)(p2::MRP, p1::MRP)
    p2, p1 = SVector(p2), SVector(p1)
    MRP(((1-p2'p2)*p1 + (1-p1'p1)*p2 - cross(2p1, p2) ) / (1+p1'p1*p2'p2 - 2p1'p2))
end

(*)(p::MRP, r::SVector) = UnitQuaternion(p)*r
(\)(p::MRP, r::SVector) = UnitQuaternion(p)\r

function (\)(p1::MRP, p2::MRP)
    # fun fact: equivalent to p2 - p1 when either is 0
    p1,p2 = SVector(p1), SVector(p2)
    n1,n2 = p1'p1, p2'p2
    θ = 1/((1+n1)*(1+n2))
    s1,s2 = (1-n1), (1-n2)
    v1 = -2p1
    v2 =  2p2
    s = s1*s2 - v1'v2
    v = s1*v2 + s2*v1 + v1 × v2

    M = θ/(1+θ*s)
    return MRP(v*M)
end

function ∇err(p1::MRP, p2::MRP)
    n1,n2 = norm2(p1),   norm2(p2)
    θ = 1/((1+n1)*(1+n2))
    s1,s2 = (1-n1), (1-n2)
    p1,p2 = SVector(p1), SVector(p2)
    v1 = -2p1
    v2 =  2p2
    s = s1*s2 - v1'v2
    v = s1*v2 + s2*v1 + v1 × v2
    dsdp = -2s1*p2 - 2v1
    dvdp = 2s1*I + -2v1*p2' + 2skew(v1)
    dθdp = -θ^2*(1+n1)*2p2

    M = θ/(1+θ*s)
    dMdp = 1/(1+θ*s)*dθdp - θ/(1+θ*s)^2*(dθdp*s + θ*dsdp)
    return dvdp*M + v*dMdp'
end

function ∇²err(p1::MRP, p2::MRP, b::SVector{3})
    n1,n2 = norm2(p1),   norm2(p2)
    θ = 1/((1+n1)*(1+n2))
    s1,s2 = (1-n1), (1-n2)
    p1,p2 = SVector(p1), SVector(p2)
    v1 = -2p1
    v2 =  2p2
    s = s1*s2 - v1'v2
    v = s1*v2 + s2*v1 + v1 × v2

    dsdp = -2s1*p2 - 2v1  # 3x1
    dsdp2 = -2s1*I  # 3x3

    dvdp = 2s1*b + -2p2*v1'b - 2skew(v1)*b
    dvdp2 = -I*2v1'b

    dθdp = -θ^2*(1+n1)*2p2  # 3x1
    dθdp2 = -2θ*(1+n1)*2p2*dθdp' - θ^2*(1+n1)*2I # 3x3

    M = θ/(1+θ*s)  # scalar
    dMdp = 1/(1+θ*s)*dθdp - θ/(1+θ*s)^2*(dθdp*s + θ*dsdp) # 3x1
    dM2 = θ/(1+θ*s)^2  # scalar
    dM3 = dθdp*s + θ*dsdp  # 3x1
    dM2dp = dθdp'/(1+θ*s)^2 - 2θ/(1+θ*s)^3 * (dθdp*s + θ*dsdp)'
    dM3dp = dθdp2*s + dθdp*dsdp' + dsdp*dθdp' + θ*dsdp2

    dMdp2 = -1/(1+θ*s)^2*dθdp*(dθdp*s + dsdp*θ)' + 1/(1+θ*s)*dθdp2  # good
    dMdp2 -= dM3*dM2dp + dM2*dM3dp

    vb = s1*v2'b + s2*v1'b + b'skew(v1)*v2  # scalar
    vpdp = s1*2b' - 2p2' * (v1'b)  + b'skew(v1)*2 # good
    # vpdp = s1*2b' - 2p2 * (v1'b)

    d1 = M*dvdp2 + dvdp*dMdp'
    d2 = dMdp2*vb + dMdp*vpdp
    return d1 + d2
end

function (/)(p1::MRP, p2::MRP)
    n1,n2 = norm2(p1),   norm2(p2)
    θ = 1/((1+n1)*(1+n2))
    s1,s2 = (1-n1), (1-n2)
    p1,p2 = SVector(p1), SVector(p2)
    v1 =  2p1
    v2 = -2p2
    s = s1*s2 - v1'v2
    v = s1*v2 + s2*v1 + v1 × v2

    M = θ/(1+θ*s)
    MRP(v[1]*M, v[2]*M, v[3]*M)

end

Base.angle(p::MRP) = angle(UnitQuaternion(p))

function kinematics(p::MRP, ω)
    p = SVector(p)
    A = @SMatrix [
        1 + p[1]^2 - p[2]^2 - p[3]^2  2(p[1]*p[2] - p[3])      2(p[1]*p[3] + p[2]);
        2(p[2]*p[1] + p[3])            1-p[1]^2+p[2]^2-p[3]^2   2(p[2]*p[3] - p[1]);
        2(p[3]*p[1] - p[2])            2(p[3]*p[2] + p[1])      1-p[1]^2-p[2]^2+p[3]^2]
    0.25*A*ω
end

(⊕)(p::MRP, δp::SVector{3}) = p*MRP(δph)
(⊖)(p::MRP, p0::MRP) = SVector(p0\p)

function ∇rotate(p::MRP, r)
    p = SVector(p)
    4( (1-p'p)*skew(r)*(4p*p'/(1+p'p) - I) - (4/(1+p'p)*skew(p) + I)*2*skew(p)*r*p'
      - 2*(skew(p)*skew(r) + skew(skew(p)*r)))/(1+p'p)^2
end

function ∇composition2(p2::MRP, p1::MRP)
    p2,p1 = SVector(p2), SVector(p1)
    n1 = p1'p1
    n2 = p2'p2
    D = 1 / (1+n1*n2 - 2p1'p2)
    d1 = (-2p1*p2' + (1-n1)*I - skew(2p1) ) * D
    d2 = -((1-n2)*p1 + (1-n1)*p2 - cross(2p1, p2) ) * D^2 *
        (2p2*n1 - 2p1)'
    return d1 + d2
end

function ∇composition1(p2::MRP, p1::MRP)
    p2,p1 = SVector(p2), SVector(p1)
    n1 = p1'p1
    n2 = p2'p2
    D = 1 / (1+n1*n2 - 2p1'p2)
    d1 = ((1-n2)*I + -2p2*p1' + 2skew(p2) ) * D
    d2 = -((1-n2)*p1 + (1-n1)*p2 - cross(2p1, p2) ) * D^2 *
        (2p1*n2 - 2p2)'
    d1 + d2
end

function ∇differential(p::MRP)
    p = SVector(p)
    n = p'p
    # p = SVector(p)
    # (1-n)I + 2(skew(p) + p*p')
    # @SMatrix [n + 2p.x^2      2(p.x*p.y-p.z)  2(p.x*p.z+p.y);
    #           2(p.y*p.x+p.z)  n + 2p.y^2      2(p.y*p.z-p.x);
    #           2(p.z*p.x-p.y)  2(p.z*p.y+p.x)  n + 2p.z^2]
    #
    # p2 = SVector(p)
    # n2 = p2'p2
    return (1-n)*I + 2(skew(p) + p*p')
end

function ∇²composition1(p2::MRP, p1::MRP, b::SVector{3})
    p2,p1 = SVector(p2), SVector(p1)
    n1 = p1'p1
    n2 = p2'p2
    D = 1 / (1+n1*n2 - 2p1'p2)  # scalar
    dD = -D^2 * (n2*2p1 - 2p2)  # 3x1
    A = -((1-n2)*p1 + (1-n1)*p2 - cross(2p1, p2) )  # 3x1
    dA = -I*(1-n2) + 2p2*p1' - 2skew(p2)  # 3x3
    B = 2(p1*n2 -  p2)'b  # scalar
    dB = 2n2*b  # 3x1
    d1 = -2p2*b' * D + ((1-n2)*b + -2p2*p1'b + 2skew(p2)*b )*dD'
    d2 = dA * D^2 * B +
         A * 2D * dD' * B +
         A * D^2 * dB'
    return d1 + d2
end

function ∇²differential(p2::MRP, b::SVector{3})
    p2 = SVector(p2)
    n2 = p2'p2
    A = -p2  # 3x1
    B = -2p2  # 3x1
    D = 1
    dD = 2p2

    dA = -I*(1-n2) - 2skew(p2)  # 3x3
    dB = 2n2*I  # 3x3

    d1 = (-2p2'b*I*D) - (dA'b * dD')
    d2 = dB * A'b * D^2 +
        (-(1-n2)*b + (skew(2p2))*b)*B' * D^2 +
        B*A'b * 2D * dD'
    d1 + d2'
end

############################################################################################
#                                 Rodrigues Parameters
############################################################################################

struct RodriguesParam{T} <: Rotation
    x::T
    y::T
    z::T
end

RodriguesParam(g::SVector{3,T}) where T = RodriguesParam{T}(g[1], g[2], g[3])
(::Type{<:RodriguesParam})(::Type{T},x,y,z) where T = RodriguesParam{T}(T(x),T(y),T(z))
(::Type{<:RodriguesParam})(p::RodriguesParam) = p
SVector(g::RodriguesParam{T}) where T = SVector{3,T}(g.x, g.y, g.z)

(::Type{<:RodriguesParam})(q::UnitQuaternion{T}) where T = RodriguesParam(q.x/q.s, q.y/q.s, q.z/q.s)

function UnitQuaternion(g::RodriguesParam{T}) where T
    M = 1/sqrt(1+norm2(g))
    UnitQuaternion{T,CayleyMap}(M, M*g.x, M*g.y, M*g.z)
end

Base.rand(::Type{<:RodriguesParam}) = RodriguesParam(rand(UnitQuaternion))
Base.zero(::Type{<:RodriguesParam}) = RodriguesParam(0.0, 0.0, 0.0)

LinearAlgebra.norm(g::RodriguesParam) = sqrt(g.x^2 + g.y^2 + q.z^2)
LinearAlgebra.norm2(g::RodriguesParam) = g.x^2 + g.y^2 + g.z^2

function (≈)(g2::RodriguesParam, g1::RodriguesParam)
    g2.x ≈ g1.x && g2.y ≈ g1.y && g2.z ≈ g1.z
end

function (*)(g2::RodriguesParam, g1::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)
    RodriguesParam((g2+g1 + g2 × g1)/(1-g2'g1))
end

(*)(g::RodriguesParam, r::SVector{3}) = UnitQuaternion(g)*r
(\)(g::RodriguesParam, r::SVector{3}) = inv(UnitQuaternion(g))*r

"Same as inv(g1)*g2"
function (\)(g1::RodriguesParam, g2::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)
    RodriguesParam((g2-g1 + g2 × g1)/(1+g1'g2))
end

"Same as g2*inv(g1)"
function (/)(g1::RodriguesParam, g2::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)
    RodriguesParam((g1-g2 + g2 × g1)/(1+g1'g2))
end

function kinematics(g::RodriguesParam, ω::SVector{3})
    g = SVector(g)
    0.5*(I + skew(g) + g*g')*ω
end

(⊕)(g::RodriguesParam, δg::SVector{3}) = g*RodriguesParam(δg)
(⊖)(g::RodriguesParam, g0::RodriguesParam) = SVector(g0\g)

Base.angle(g::RodriguesParam) = angle(UnitQuaternion(g))

function rotmat(g::RodriguesParam)
    ghat = skew(SVector(g))
    I + 2*ghat*(ghat + I)/(1+norm2(g))
end

function ∇rotate(g0::RodriguesParam, r)
    g = SVector(g0)
    ghat = skew(g)
    n1 = 1/(1 + g'g)
    gxr = cross(g,r) + r
    d1 = ghat*gxr * -2*n1^2 * g'
    d2 = -(ghat*skew(r) + skew(gxr))*n1
    return 2d1 + 2d2
end

function ∇composition1(g2::RodriguesParam, g1::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)

    N = g2 + g1 + g2 × g1
    D = 1/(1 - g2'g1)
    (I + skew(g2) + D*N*g2')*D
end

function ∇²composition1(g2::RodriguesParam, g1::RodriguesParam, b::SVector{3})
    g2 = SVector(g2)
    g1 = SVector(g1)

    N = g2 + g1 + g2 × g1  # 3x1
    D = 1/(1 - g2'g1)  # scalar
    dN = I + skew(g2)
    dD = D^2*g2'
    return g2*b'*(N*(2*D*dD) + D^2*dN) + (I - skew(g2))*b*dD
end

function ∇composition2(g2::RodriguesParam, g1::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)

    N = g2 + g1 + g2 × g1
    D = 1/(1 - g2'g1)
    (I - skew(g1) + D*N*g1')*D
end

function ∇differential(g::RodriguesParam)
    g = SVector(g)
    (I + skew(g) + g*g')
end

function ∇²differential(g::RodriguesParam, b::SVector{3})
    g = SVector(g)
    return g*b'*(2g*g' + I + skew(g)) + (I - skew(g))*b*g'
end

############################################################################################
#                             Roll, Pitch, Yaw Euler Angles
############################################################################################

""" $(TYPEDEF)
Roll-pitch-yaw Euler angles.
"""
struct RPY{T} <: Rotation
    ϕ::T  # roll
    θ::T  # pitch
    ψ::T  # yaw
end

RPY(e::SVector{3,T}) where T = RPY{T}(e[1], e[2], e[3])
RPY(R::SMatrix{3,3,T}) where T =  RPY(rotmat_to_rpy(R))
RPY(q::UnitQuaternion) = RPY(rotmat(q))
RPY(p::MRP) = RPY(rotmat(p))
(::Type{<:RPY})(e::RPY) = e
(::Type{<:RPY})(::Type{T},x,y,z) where T = RPY{T}(T(x),T(y),T(z))
function RPY(ϕ::T1,θ::T2,ψ::T3) where {T1,T2,T3}
    T = promote_type(T1,T2)
    T = promote_type(T,T3)
    RPY(T(ϕ), T(θ), T(ψ))
end

SVector(e::RPY{T}) where T = SVector{3,T}(e.ϕ, e.θ, e.ψ)

Base.rand(::Type{RPY{T}}) where T = RPY(rand(UnitQuaternion{T}))
Base.rand(::Type{RPY}) = RPY(rand(UnitQuaternion))
Base.zero(::Type{RPY{T}}) where T = RPY(zero(T), zero(T), zero(T))
Base.zero(::Type{RPY}) = RPY(0.0, 0.0, 0.0)

roll(e::RPY) = e.ϕ
pitch(e::RPY) = e.θ
yaw(e::RPY) = e.ψ

Base.angle(e::RPY) = angle(UnitQuaternion(e))

(≈)(e1::RPY, e2::RPY) = rotmat(e1) ≈ rotmat(e2)

@inline rotmat(e::RPY) = rotmat(e.ϕ, e.θ, e.ψ)

function rotmat(ϕ, θ, ψ)
    # Equivalent to RotX(e[1])*RotY(e[2])*RotZ(e[3])
    if !isfinite(ϕ) || !isfinite(θ) || !isfinite(ψ)
        return @SMatrix zeros(3,3)
    end
    sϕ,cϕ = sincos(ϕ)
    sθ,cθ = sincos(θ)
    sψ,cψ = sincos(ψ)
    # A = @SMatrix [
    #     cθ*cψ          -cθ*sψ              sθ;
    #     sϕ*sθ*cψ+cϕ*sψ -sϕ*sθ*sψ + cϕ*cψ  -cθ*sϕ;
    #    -cϕ*sθ*cψ+sϕ*sψ  cϕ*sθ*sψ + sϕ*cψ   cθ*cϕ
    # ]
    A = @SMatrix [
        cψ*cθ - sϕ*sψ*sθ   -cϕ*sψ  cψ*sθ + cθ*sϕ*sψ;
        cθ*sψ + cψ*sϕ*sθ    cϕ*cψ  sψ*sθ - cψ*cθ*sϕ;
        -cϕ*sθ              sϕ          cϕ*cθ;
    ]
end

(*)(e::RPY, r::SVector{3}) = rotmat(e)*r
(\)(e::RPY, r::SVector{3}) = rotmat(e)'r

function rotmat_to_rpy(R::SMatrix{3,3,T}) where T
    # ψ = atan(-R[1,2], R[1,1])
    # ϕ = atan(-R[2,3], R[3,3])
    # θ = asin(R[1,3])
    θ = atan(-R[3,1], R[3,3])
    ψ = atan(-R[1,2], R[2,2])
    ϕ = asin(clamp(R[3,2],-1,1))
    return SVector{3,T}(ϕ, θ, ψ)
end

function from_rotmat(R::SMatrix{3,3,T}) where T
    ϕ,θ,ψ = rotmat_to_rpy(R)
    return RPY(ϕ, θ, ψ)
end

function (*)(e2::RPY, e1::RPY)
    from_rotmat(rotmat(e2)*rotmat(e1))
end

function (\)(e1::RPY, e2::RPY)
    from_rotmat(rotmat(e1)'rotmat(e2))
end

function (/)(e1::RPY, e2::RPY)
    from_rotmat(rotmat(e1)*rotmat(e2)')
end

function (⊖)(e1::RPY, e2::RPY)
    SVector(e1\e1)
end

function kinematics(e::RPY, ω::SVector{3})
    sθ,cθ = sincos(e.θ)
    sϕ,cϕ = sincos(e.ϕ)
    A = @SMatrix [
        cθ 0 -cϕ*sθ;
        0  1  sϕ;
        sθ 0  cϕ*cθ
    ]
    A\ω
end

function ∇rotate(e::RPY, r::SVector{3})
    rotate(e) = RPY(e)*r
    ForwardDiff.jacobian(rotate, SVector(e))
end

function ∇composition1(e2::RPY, e1::RPY)
    R2 = rotmat(e2)
    rotate(e) = rotmat_to_rpy(R2*rotmat(e[1],e[2],e[3]))
    ForwardDiff.jacobian(rotate,SVector(e1))
end

function ∇composition2(e2::RPY, e1::RPY)
    R1 = rotmat(e1)
    rotate(e) = rotmat_to_rpy(rotmat(e[1],e[2],e[3])*R1)
    ForwardDiff.jacobian(rotate,SVector(e2))
end

############################################################################################
#                                 CONVERSIONS
############################################################################################
""" Convert from a rotation matrix to a unit quaternion
Uses formula from Markely and Crassidis's book
    "Fundamentals of Spacecraft Attitude Determination and Control" (2014), section 2.9.3
"""
function rotmat_to_quat(A::SMatrix{3,3,T}) where T
    trA = tr(A)
    v,i = findmax(diag(A))
    if trA > v
        i = 1
    else
        i += 1
    end
    if i == 1
        q = UnitQuaternion(1+trA, A[2,3]-A[3,2], A[3,1]-A[1,3], A[1,2]-A[2,1])
    elseif i == 2
        q = UnitQuaternion(A[2,3]-A[3,2], 1 + 2A[1,1] - trA, A[1,2]+A[2,1], A[1,3]+A[3,1])
    elseif i == 3
        q = UnitQuaternion(A[3,1]-A[1,3], A[2,1]+A[1,2], 1+2A[2,2]-trA, A[2,3]+A[3,2])
    elseif i == 4
        q = UnitQuaternion(A[1,2]-A[2,1], A[3,1]+A[1,3], A[3,2]+A[2,3], 1 + 2A[3,3] - trA)
    end
    return normalize(inv(q))
end

UnitQuaternion(e::RPY) = rotmat_to_quat(rotmat(e))
(::Type{<:RPY})(q::UnitQuaternion) = from_rotmat(rotmat(q))

function (::Type{<:UnitQuaternion})(p::MRP)
    p = SVector(p)
    n2 = p'p
    M = 2/(1+n2)
    UnitQuaternion{MRPMap}((1-n2)/(1+n2), M*p[1], M*p[2], M*p[3])
end

(::Type{<:RodriguesParam})(p::MRP) = RodriguesParam(UnitQuaternion(p))

(::Type{<:RPY})(p::MRP) = RPY(UnitQuaternion(p))

############################################################################################
#                             INVERSE RETRACTION MAPS
############################################################################################
(::Type{ExponentialMap})(q::UnitQuaternion) = scaling(ExponentialMap)*logm(q)

(::Type{VectorPart})(q::UnitQuaternion) = scaling(VectorPart)*vector(q)

(::Type{CayleyMap})(q::UnitQuaternion) = scaling(CayleyMap) * vector(q)/q.s

(::Type{MRPMap})(q::UnitQuaternion) = scaling(MRPMap)*vector(q)/(1+q.s)

(::Type{IdentityMap})(q::UnitQuaternion) = SVector(q)

function jacobian(::Type{ExponentialMap}, q::UnitQuaternion, eps=1e-5)
    μ = scaling(ExponentialMap)
    s = scalar(q)
    v = vector(q)
    θ2 = v'v
    θ = sqrt(θ2)
    datan = 1/(θ2 + s^2)
    ds = -datan*v

    if θ < eps
        return 2*μ*[ds (v*v' + I)/s]
    else
        atanθ = atan(θ,s)
        dv = ((s*datan - atanθ/θ)v*v'/θ + atanθ*I )/θ
        d0 = ((s*datan - atanθ/θ)v*v'/θ^2 + atanθ/θ*I )
        d1 = (s*datan - atanθ/θ)
        d2 = v*v'/θ2
        d3 = atanθ/θ * I
        return 2*μ*[ds dv]
    end
end

function ∇jacobian(::Type{ExponentialMap}, q::UnitQuaternion, b::SVector{3}, eps=1e-5)
    μ = scaling(ExponentialMap)
    s = scalar(q)
    v = vector(q)
    θ2 = v'v
    θ = sqrt(θ2)
    datan = 1/(θ2 + s^2)
    ds = -datan*v

    if θ < eps
        # return 2*μ*[b'ds; (v*v'b + b)/s]
        return 2*μ*[b'*(datan^2*2s*v) -b'datan*I;
                    -(v*v'b +b)/s^2 (I*(v'b) + v*b')/s]
    else
        dsds = 2b's*datan^2*v
        dsdv = b'*(-datan*I + 2datan^2*v*v')

        atanθ = atan(θ,s)
        d1 = (s*datan - atanθ/θ)
        d2 = v*v'b/θ2
        d3 = atanθ/θ*b
        d1ds = (datan - 2s^2*datan^2 + datan)
        dvds = d1ds*d2 - datan*b

        d1dv =  (-2s*datan^2*v' - s*datan*v'/θ^2 + atanθ/θ^3*v')
        d2dv = (I*(v'b) + v*b')/θ2 - 2(v*v'b)/θ^4 * v'
        d3dv = b*(s*datan*v'/θ^2 - atanθ/θ^3*v')
        dvdv = d2*d1dv + d1*d2dv + d3dv

        # return 2*μ*[ds'b; dv'b]
        return 2*μ*@SMatrix [
            dsds    dsdv[1] dsdv[2] dsdv[3];
            dvds[1] dvdv[1] dvdv[4] dvdv[7];
            dvds[2] dvdv[2] dvdv[5] dvdv[8];
            dvds[3] dvdv[3] dvdv[6] dvdv[9];
        ]
    end
end

function jacobian(::Type{VectorPart}, q::UnitQuaternion)
    μ = scaling(VectorPart)
    return @SMatrix [0. μ 0 0;
                     0. 0 μ 0;
                     0. 0 0 μ]
end

function ∇jacobian(::Type{VectorPart}, q::UnitQuaternion, b::SVector{3})
    μ = scaling(VectorPart)
    @SMatrix zeros(4,4)
end

function jacobian(::Type{CayleyMap}, q::UnitQuaternion)
    μ = scaling(CayleyMap)
    si = 1/q.s
    return μ*@SMatrix [-si^2*q.x si 0 0;
                       -si^2*q.y 0 si 0;
                       -si^2*q.z 0 0 si]
end

function ∇jacobian(::Type{CayleyMap}, q::UnitQuaternion, b::SVector{3})
    μ = scaling(CayleyMap)
    si = 1/q.s
    v = vector(q)
    μ*@SMatrix [
        2*si^3*(v'b) -si^2*b[1] -si^2*b[2] -si^2*b[3];
       -si^2*b[1] 0 0 0;
       -si^2*b[2] 0 0 0;
       -si^2*b[3] 0 0 0;
    ]
end

function jacobian(::Type{MRPMap}, q::UnitQuaternion)
    μ = scaling(MRPMap)
    si = 1/(1+q.s)
    return μ*@SMatrix [-si^2*q.x si 0 0;
                       -si^2*q.y 0 si 0;
                       -si^2*q.z 0 0 si]
end

"""
Jacobian of G(q)'b, where G(q) = jacobian(MRPMap,q)
"""
function ∇jacobian(::Type{MRPMap}, q::UnitQuaternion, b::SVector{3})
    μ = scaling(MRPMap)
    si = 1/(1+q.s)
    v = vector(q)
    μ * @SMatrix [
        2*si^3*(v'b) -si^2*b[1] -si^2*b[2] -si^2*b[3];
       -si^2*b[1] 0 0 0;
       -si^2*b[2] 0 0 0;
       -si^2*b[3] 0 0 0;
    ]
end

jacobian(::Type{IdentityMap}, q::UnitQuaternion) = I

inverse_map_jacobian(q::R) where R<:Rotation = I
inverse_map_jacobian(q::UnitQuaternion{T,D}) where {T,D} = jacobian(D,q)

inverse_map_∇jacobian(q::R, b::SVector{3}) where R<:Rotation = I*0
inverse_map_∇jacobian(q::UnitQuaternion{T,D}, b::SVector{3}) where {T,D} = ∇jacobian(D, q, b)


using ConvexBodyProximityQueries
using CurveProximityQueries
using Plots
import Random: seed!

pgfplots()

include("EllipseBounds.jl")
using .EllipseBounds

function drawellipses!(a, b::Curve, maxnum; atol=1e-8)
    leaves, glb, gub, soln = CurveProximityQueries._init(a, b)
    i = 0
    while (gub - glb) > atol && i < maxnum
        glb, gub, soln = CurveProximityQueries._loop(a, b, leaves, glb, gub)
        i += 1
    end
    for interval in leaves
        β, lb = interval
        if diam(β) > 1e-3
            bell = Ellipse(b, β)
            plot!(bell,
                 linecolor=RGBA(102/255,130/225,223/255,0.5),
                 fillcolor=RGBA(102/255,130/225,223/255,0.5),
                 )
        end
    end
end

seed!(22)
poly = randpoly([-0.5, 0.4]; n=20, scale=0.5)

curve = Bernstein([[0.944223, 0.177264], [0.46193, 0.105887], [0.791169, 0.172386], [0.256546, 0.463735], [0.70966, 0.3173], [0.664713, 0.830956], [0.952688, 0.736242], [0.00763912, 0.97189], [0.270595, 0.827442], [0.16636, 0.996615], [0.381073, 0.543077], [0.225381, 0.754049], [0.570043, 0.638543], [0.254696, 0.988563]])

pts = closest_points(poly, curve)

cases = [nothing, 0, 1, 3, 7, 100]
alph = 'a':'z'
for (i, c) in enumerate(cases)
    plot(poly,
         fillalpha=1.0,
         fillcolor=Gray(0.7),
        )
    isnothing(c) || drawellipses!(poly, curve, c)
    plot!(curve,
          linewidth=2.0,
          linecolor=:black,
         )
    savefig("out/fig6$(alph[i]).pdf")
end
plot!(pts,
      linestyle=:dash,
      markersize=8.0,
      linewidth=2.0,
     )
savefig("out/fig6$(alph[length(cases)]).pdf")


using Test
using VariantsNMF

X1 = [0.21 0.40 0.4; 0.01 0.4 0.45]
Y1 = [0.12 0.48 0.4; 0.1 0.55 0.45]

α_values = [-0.5, 0, 0.33, 1, 1.5, 2, 2.8]
α_divg_values = [0.283580, 0.195769, 0.162286, 0.124183, 0.109648, 0.101371, 0.095786]
β_values = [-0.5, 0, 0.33, 1, 1.5, 2, 2.8]
β_divg_values = [7.81200, 1.65435, 0.64633, 0.12418, 0.04732, 0.02255, 0.00952]

# test α-divergence function
@testset "α_divergence" begin
    for (α, α_divg) in zip(α_values, α_divg_values)
        @test α_divergence(X1, X1, α) ≈ 0  atol = 1e-9
        @test α_divergence(Y1, Y1, α) ≈ 0  atol = 1e-9
        @test α_divergence(X1, Y1, α) ≈ α_divg atol = 1e-5
    end
end

# test β-divegence function
@testset "β_divergence" begin
    for (β, β_divg) in zip(β_values, β_divg_values)
        @test β_divergence(X1, X1, β) ≈ 0  atol = 1e-9
        @test β_divergence(Y1, Y1, β) ≈ 0  atol = 1e-9
        @test β_divergence(X1, Y1, β) ≈ β_divg atol = 1e-5
    end
end


#-----------------------------------------------------------------------
#   MULTIPLE EMISSION SET VECTOR GENERATOR
#-----------------------------------------------------------------------
"""
    multivec( X::RealMtx , k::Int )

This function takes a ``m×n`` matrix `X`, and for `k```<n``,
constructs a vector of ``m×```k` matrices constructed from all choices
of `k` columns from the ``m×n`` matrix `X`.

"""
function multivec( X::RealMtx , k::Int )
    tpfl = typeof(X[1,1])
    d    = size(X)[1]
    np   = size(X)[2]
    Z    = [zeros(tpfl,d) for _ in 1:np]

    for i=1:np
        Z[i] = X[:,i]
    end

    w   = collect(combinations(Z,k))

    l   = length(w)
    z   = [zeros(tpfl,d,k) for _ in 1:l]
    for i=1:l, j=1:k
        z[i][:,j] = w[i][j]
    end

    return z
end     #---------------------------------------------------------------

#-----------------------------------------------------------------------
#   MULTIPLE EMISSION SET LOCATOR
#-----------------------------------------------------------------------
"""
    mlocator( X::RealMtx )

The function `mlocator` computes a single intersection point ``X_c``
from an ``m×n`` matrix `X` formed from `n>4` emission points, which form
the column vectors of `X`.

"""
function mlocator( X::RealMtx )
    #   Calculates location for more than five emission points
    tpfl = typeof(X[1,1])

    l = size(X)

    if l[2] == 5
        return locator5RTC21(X)
    elseif l[2] > 5
        W   = multivec(X,5)
        k   = length(W)

        w   = [zeros(tpfl,4) for _ =1:2*k ]

        for a=1:k
            w[a] = locator5RTC21(W[a])
        end

        #   Re-sort location points according to smallest Minkowski norm
        δW = zeros(tpfl,k)
        for a=1:k
            for i=1:4
                δW[a] += abs(ηdot(X[:,i]-w[a],X[:,i]-w[a]))
            end
        end
        iW = sortperm(δW)

        return w[iW][1]
    else
        return zeros(tpfl,4)
    end
end     #---------------------------------------------------------------


"""
    CPInterval <: ConfidenceInterval

Naive Clopper-Pearson confidence interval using default ordering.
"""
struct CPInterval{TD<:Design,TR<:Real} <: ConfidenceInterval

  design::TD
  confidence::TR

  function CPInterval{TD,TR}(
    design::TD, confidence::TR
  ) where {TD<:Design,TR<:Real}

    @checkprob confidence
    new(design, confidence)

  end # inner constructor

end # CPInterval

"""
    CPInterval{T<:Real}(
      design::Design;
      confidence::T = .9
    )

Naive Clopper-Pearson confidence interval using default ordering.

# Parameters

| Parameter    | Description |
| -----------: | :---------- |
| design       | binary two-stage design |
| confidence   | confidence level of the interval |
"""
function CPInterval(
  design::TD; confidence::Real = .9
) where {TD<:Design}

  return CPInterval{TD,typeof(confidence)}(design, confidence)

end # CPInterval

Base.show(io::IO, ci::CPInterval) = print("CPInterval")

function limits(ci::CPInterval, x1::T, x2::T) where {T<:Integer}

    d              = design(ci)
    ispossible(d, x1, x2) ? nothing : error("(x1, x2) not compatible with underlying design")
    alpha::Float64 = 1 - confidence(ci)
    n1             = interimsamplesize(d)
    nmax::Int64    = maximum(samplesize(d))
    x              = x1 + x2
    n              = samplesize(d, x1)
    if x == 0
        limits = [0.0; (1.0 - (alpha/2)^(1/n))]
    elseif x == n
        limits = [(alpha/2)^(1/n); 1.0]
    else
        limits = [quantile(Distributions.Beta(x, n - x + 1), alpha/2); quantile(Distributions.Beta(x + 1, n - x), 1 - alpha/2)]
    end
    return limits

end


import GreyDecision.GreyNumbers: GreyNumber
import GreyDecision.Utility: makeminmax
import GreyDecision.Electre: electre

@testset "Electre with white numbers" begin
    tol = 0.00001
        w = [0.110, 0.035, 0.379, 0.384, 0.002, 0.002, 0.010, 0.077]
        Amat = [
            100 92 10 2 80 70 95 80
            80 70 8 4 100 80 80 90
            90 85 5 0 75 95 70 70
            70 88 20 18 60 90 95 85
        ]
        greyamat = Array{GreyNumber{Float64}, 2}(undef, 4, 8)
        for i in 1:4
            for j in 1:8
                greyamat[i,j] = GreyNumber{Float64}(Float64(Amat[i,j]), Float64(Amat[i, j]))
            end
        end
        dmat = greyamat
        fns = makeminmax([maximum for i = 1:8])
        result = electre(dmat, w, fns)

        @test isa(result, Dict)
        @test isa(result[:bestindex], Tuple)
        @test result[:bestindex][1] == 4

        @test isapprox(result[:C][1], GreyNumber(0.36936937,0.36936937), atol = tol)
        @test isapprox(result[:C][2], GreyNumber(0.01501502, 0.01501502),  atol = tol)
        @test isapprox(result[:C][3], GreyNumber(-2.47347347, -2.47347347),  atol = tol)
        @test isapprox(result[:C][4], GreyNumber(2.08908909, 2.08908909), atol = tol)

        @test isapprox(result[:D][1], GreyNumber(0.1914244, 0.1914244), atol = tol)
        @test isapprox(result[:D][2], GreyNumber(-0.1903929,-0.1903929),  atol = tol)
        @test isapprox(result[:D][3], GreyNumber(2.8843076, 2.8843076),  atol = tol)
        @test isapprox(result[:D][4], GreyNumber(-2.8853391, -2.8853391), atol = tol)
end