# Randomization option slows down processing
# but improves quality of solution considerably
# Would be better to do randomization in place
function cddual(X::SparseOrFullMat,
	            Y::Vector;
	            C::Real = 1.0,
	            norm::Integer = 2,
	            randomized::Bool = true,
	            maxpasses::Integer = 1000,
	            eps::Real = 0.1)
	const PG_EPS = 1.0e-12

	# l: # of samples
	# n: # of features
	n, l = size(X)
	alpha = zeros(l)
	w = zeros(n)

	# Set U and D
	#  * L1-SVM: U = C, D[i] = 0
	#  * L2-SVM: U = Inf, D[i] = 1 / (2C)
	U = 0.0
	D = Array(Float64, l)
	if norm == 1
		U = C
		fill!(D, 0.0)
	elseif norm	== 2
		U = Inf
		fill!(D, 1.0 / (2.0 * C))
	else
		DomainError("Only L1-SVM and L2-SVM are supported")
	end

	# Set Qbar
	Qbar = Array(Float64, l)
	for i in 1:l
		Qbar[i] = D[i] + vecnorm(X[:, i], 2)^2
	end

	# Loop over examples
	converged = false
	pass = 0

	indices = randomized ? randperm(l) : [1:l]

	while !converged
		pass += 1
		if pass > maxpasses
			break
		end
		inv_ins = 0
		pg_max = -Inf
		pg_min = Inf

		if randomized
			shuffle!(indices)
		end

		# Process all observations
		for i in indices
			g = Y[i] * inner_prod(w, X[:, i]) - 1.0 + D[i] * alpha[i]

			if alpha[i] == 0.0
				pg = min(g, 0.0)
				if g <= 0.0
					inv_ins += 1
				end
			elseif alpha[i] == U
				pg = max(g, 0.0)
				if g >= 0.0
					inv_ins += 1
				end
			else
				pg = g
			end
			pg_max = max(pg_max, pg)
			pg_min = min(pg_min, pg)

			if abs(pg) > PG_EPS
				alphabar = alpha[i]
				alpha[i] = min(max(alpha[i] - g / Qbar[i], 0.0), U)
				add_vec!(w, X[:, i], e -> (alpha[i] - alphabar) * Y[i] * e)
			end
		end

		if (pg_max - pg_min <= eps) && (inv_ins == 0)
			converged = true
			break
		end
	end

	return SVMFit(w, pass, converged)
end


using AbstractGPs

mutable struct AbstractGPSurrogate{X, Y, GP, GP_P, S} <: AbstractSurrogate
    x::X
    y::Y
    gp::GP
    gp_posterior::GP_P
    Σy::S
 end

# constructor
function AbstractGPSurrogate(x, y; gp = GP(Matern52Kernel()), Σy = 0.1)
    AbstractGPSurrogate(x, y, gp, posterior(gp(x, Σy),y), Σy)
 end

# predictor
function (g::AbstractGPSurrogate)(val)
    return only(mean(g.gp_posterior([val])))
end

# for add point
# copies of x and y need to be made because we get
#"Error: cannot resize array with shared data " if we push! directly to x and y
function add_point!(g::AbstractGPSurrogate, new_x, new_y)
    if new_x in g.x
        println("Adding a sample that already exists, cannot build AbstracgGPSurrogate.")
        return
    end
    x_copy = copy(g.x)
    push!(x_copy, new_x)
    y_copy = copy(g.y)
    push!(y_copy, new_y)
    updated_posterior = posterior(g.gp(x_copy, g.Σy), y_copy)
    g.x, g.y, g.gp_posterior = x_copy, y_copy, updated_posterior
    nothing
end

function std_error_at_point(g::AbstractGPSurrogate, val)
    return sqrt(only(var(g.gp_posterior([val]))))
end

# Log marginal posterior predictive probability.
function logpdf_surrogate(g::AbstractGPSurrogate)
    return logpdf(g.gp_posterior(g.x), g.y)
end


# Surrogate Model with Gaussian Processes
module GPs

using ..DE

using LinearAlgebra

export model_training, predict

function kernel_trick(x1::Float64, x2::Float64,
                      σ_f::Float64, l::Float64)
    re = σ_f^2 * exp((x1 - x2)^2 / -2)
end

function K(x::Array{Float64}, σ_f::Float64, l::Float64)
    le = length(x)
    re = Array{Float64}(undef, le, le)
    for i=1:1:le
        for j=1:1:le
            re[i, j] = kernel_trick(x[i], x[j], σ_f, l)
        end
    end
    re
end

function K(x1::Array{Float64}, x2::Float64, σ_f::Float64, l::Float64)
    le = length(x1)
    re = Array{Float64}(undef, le, 1)
    for i=1:1:le
        re[i, 1] = kernel_trick(x1[i], x2, σ_f, l)
    end
    re
end

function K(x::Float64, σ_f::Float64, l::Float64)
    kernel_trick(x, x, σ_f, l)
end

function marginal_likelihood(tr_x::Array{Float64}, tr_y::Array{Float64},
                             σ_f::Float64, l::Float64, σ_n::Float64)
    A = K(tr_x, σ_f, l) + σ_n^2 * UniformScaling(length(tr_x))
    n = length(tr_x)
    -(-1/2 * tr_y' * inv(A) * tr_y - 1/2 * log(det(A)) - n/2 * log(2 * pi))
end

function model_training(tr_x::Array{Float64}, tr_y::Array{Float64})
    f(pop::DE.Population) = begin
        for i in 1:size(pop.x, 1)
            pop.fit[i] = marginal_likelihood(tr_x, tr_y,
                            pop.x[i, 1], pop.x[i, 2], pop.x[i, 3])
        end
    end

    σ_f, l, σ_n= DE_evolution(f, 10, 3, 50)[1]
    A = K(tr_x, σ_f, l) + σ_n * UniformScaling(length(tr_x))
    while !isposdef(A)
        σ_f, l, σ_n= DE_evolution(f, 10, 3, 50)[1]
        A = K(tr_x, σ_f, l) + σ_n * UniformScaling(length(tr_x))
    end

    L = cholesky(A)
    α = L.U\(L.L\tr_y)
    return σ_f, l, L, α
end

function predict(σ_f::Float64, l::Float64, L::Cholesky, α::Array{Float64},
                 tr_x::Array{Float64}, pr_x::Float64)
    k_s = K(tr_x, pr_x, σ_f, l)
    k_ss = K(pr_x, σ_f, l)
    mean = k_s' ⋅ α
    v = L\k_s
    vari = k_ss - v' ⋅ v
    return mean, vari
end

end  # modue SuM


function ekepler(m::T,ecc::T) where {T <: Real}
KEPLER_TOL = sqrt(eps(m))
if m != zero(T)
#real*8 e,e0,eps,m,ms,pi2,f0,f1,f2,f3,d1,d2,d3
# This routine solves Kepler's equation for E as a function of (e,M)
# using the procedure outlined in Murray & Dermott:
  pi2=2pi
  ms=mod(m,pi2)
  d3 =one(T)
  de0=ecc*0.85*sign(ms)
  de1=2*de0
  de2=3*de0
  iter = 0
  ITMAX = 20
#  while abs(d3) > KEPLER_TOL
  while true
    de2 = de1
    de1 = de0
    f3=ecc*cos(de0+ms)
    f2=ecc*sin(de0+ms)
#    f1=1.0-f3
#    f0=de0-f2
#    d1=-f0/f1
#    d2=-f0/(f1+0.5*d1*f2)
#    d3=-f0/(f1+d2*0.5*(f2+d2*f3/3.))
#    de0 += d3
    de0 = (f2-de1*f3)/(1-f3)
    iter += 1
    if iter >= ITMAX || de0 == de1 || de0 == de2
      break
    end
  end
  if iter >= ITMAX && !(T == BigFloat && minimum([abs(de0-de1),abs(de0-de2)]) < eps(one(T)))
    println("iterations in ekepler: ",iter," de0: ",de0," de1-de0: ",de1-de0," de2-de0: ",de2-de0)
  end
  ekep=de0+m
else
  ekep = zero(T)
end
return ekep::typeof(m)
end

function ekepler2(m::T,ecc::T) where {T <: Real}
KEPLER_TOL = sqrt(eps(m))
if m != 0.0
#real*8 e,e0,eps,m,ms,pi2,f0,f1,f2,f3,d1,d2,d3
# This routine solves Kepler's equation for E as a function of (e,M)
# using the procedure outlined in Murray & Dermott:
  pi2=2.0*pi
  ms=mod(m,pi2)
  d3 =one(T)
  e0=ms+ecc*0.85*sin(ms)/abs(sin(ms))
  e1=2*e0
  e2=3*e0
#  while abs(d3) > KEPLER_TOL
  ITMAX = 50
  while true
    e2 = e1
    e1 = e0
    f3=ecc*cos(e0)
    f2=ecc*sin(e0)
    f1=1.0-f3
    f0=e0-ms-f2
    d1=-f0/f1
    d2=-f0/(f1+0.5*d1*f2)
    d3=-f0/(f1+d2*0.5*(f2+d2*f3/3.))
    e0=e0+d3
    if iter >= ITMAX || e0 == e1 || e0 == e2
      break
    end
  end
  if iter >= ITMAX
    println("iterations in ekepler2: ",iter," de0: ",de0," de1-de0: ",de1-de0," de2-de0: ",de2-de0)
  end
  ekep=e0+m-ms
else
  ekep = 0.0
end
return ekep::typeof(m)
end

function kepler(m,ecc)
@assert(ecc >= 0.0)
@assert(ecc <= 1.0)
f=m
if ecc > 0
  ekep=ekepler(m,ecc)
#  println(m-ekep+ecc*sin(ekep))
#  f=2.0*atan(sqrt((1.0+ecc)/(1.0-ecc))*tan(0.5*ekep))
#  f=2.0*atan2(sqrt(1.0+ecc)*sin(0.5*ekep),sqrt(1.0-ecc)*cos(0.5*ekep))
  f=2.0*atan(sqrt(1.0+ecc)*sin(0.5*ekep),sqrt(1.0-ecc)*cos(0.5*ekep))
end
return f
end


using Random, Statistics
Random.seed!(0)

trueVar, trueStd = 1/12, sqrt(1/12)

function estVar(n)
    sample = rand(n)
    sum((sample .- 0.5).^2)/n
end

N = 10^7
for n in 5:5:30
    biasVar = mean([estVar(n) for _ in 1:N]) - trueVar
    biasStd = mean([sqrt(estVar(n)) for _ in 1:N]) - trueStd
    println("n = ",n, " Var bias: ", round(biasVar, digits=5),
           	    "\t Std bias: ", round(biasStd, digits=5))
end


using JuMP, Cbc

"""
    kantorovich_distance(ν, μ)

Calculate the kantorovich distance between two probability distributions.

Thanks to this [hero] (https://stla.github.io/stlapblog/posts/KantorovichWithJulia.html)

```jldoctest
julia> mu = [1/7, 2/7, 4/7];
julia> nu = [1/4, 1/4, 1/2];
julia> kantorovich_distance(mu, nu)
0.10714285714285714
```
"""
function kantorovich_distance(ν, μ)
    n = length(ν)
    m = Model(solver=CbcSolver())
    @variable(m, p[1:n, 1:n] >= 0)
    @objective(m, Min, sum(p[i, j] for i in 1:n for j in 1:n if i != j))
    for k in 1:n
        @constraint(m, sum(p[k, :]) == μ[k])
        @constraint(m, sum(p[:, k]) == ν[k])
    end
    solve(m)
    return getobjectivevalue(m)
end


using Plots, LaTeXStrings, Measures; pyplot()

delta = 0.01
grid = 0:delta:1
f(x,y) = 9/8*(4x+y)*sqrt((1-x)*(1-y))
z = [f(x,y) for y in grid, x in grid]

densityIntegral = sum(z)*delta^2
println("2-dimensional Riemann sum over density: ", densityIntegral)

probB = sum([sum([f(x,y)*delta for y in x:delta:1])*delta for x in grid])
println("2-dimensional Riemann sum to evaluate probability: ", probB)

p1 = surface(grid, grid, z,
	c=cgrad([:blue, :red]), la=1, camera=(60,50),
	ylabel="y", zlabel=L"f(x,y)", legend=:none)
p2 = contourf(grid, grid, z,
	c=cgrad([:blue, :red]))
p2 = contour!(grid, grid, z,
	c=:black, xlims=(0,1), ylims=(0,1), ylabel="y", ratio=:equal)

plot(p1, p2, size=(800, 400), xlabel="x", margin=5mm)


module SineFit

import LsqFit
import FFTW
import Statistics

export estimate_wave_parameters, calculate_wave_shape, sin

struct WaveFitParams
    amplitude::Float64
    frequency::Float64
    phaseshft::Float64
    vertoffst::Float64
end

"""
    estimate_wave_parameters(xdata::Vector{T}, ydate::Vector{T};
        sample_rate=nothing)::WaveFitParams where {T<:Number}

Use Fourier analysis and statistics to find the best monochromatic sine wave parameters for data.
`xsym` and `ysym` are the column symbols for the x and y columns within DataFrame `df`, respectively.
Returns a `WaveFitParams` struct with frequency and phase shift as circular frequencies resp. radians.

If `sample_rate` is not specified, the rate is determined automatically from the `xdata` argument.
"""
function estimate_wave_parameters(xdata::Vector{T}, ydata::Vector{T};
        sample_rate=nothing)::WaveFitParams where {T<:Number}
    if sample_rate === nothing
        sample_rate = 1 / Statistics.mean(xdata[2:end] - xdata[1:end-1])
    end

    offset = (maximum(ydata)+minimum(ydata))/2
    ampl = Statistics.mean(Statistics.quantile(abs.(ydata .- offset), 99/100))
    fft = FFTW.rfft(ydata .- offset)
    fftfreq = FFTW.rfftfreq(length(ydata), sample_rate)

    # Determine dominant frequency and calculate phase shift.
    maxfreqloc = argmax(abs.(fft))
    maxfreq = fftfreq[maxfreqloc]
    shift = mod(atan(imag(fft[maxfreqloc]), real(fft[maxfreqloc])) + pi/2, 2pi)
    freq = maxfreq * 2pi

    WaveFitParams(ampl, freq, shift, offset)
end

function sin_model(xs, params)
    params[1] .* Base.sin.(params[2] .* xs .+ params[3]) .+ params[4]
end

"""
    calculate_wave_shape(xdata::Vector{T}, ydate::Vector{T};
        sample_rate=nothing)::WaveFitParams where {T<:Number}

Fit a sine function (`sin_model`, i.e. `a sin(b x + c) + d`) to the given data `xdata`/`ydata`.

This function first estimates the parameters using normal statistics and fourier analysis using `estimate_wave_parameters()`.
Then, a least-squares fit (`LsqFit`) is used to refine the parameters -- for example, to adjust the frequency, as a FFT has
a very limited frequency resolution. This helps a lot with accuracy.

If `sample_rate` is not specified, the rate is determined automatically from the `xdata` argument.
"""
function calculate_wave_shape(xdata::Vector{T}, ydata::Vector{T};
        sample_rate=nothing)::WaveFitParams where {T<:Number}

    estimate = estimate_wave_parameters(xdata, ydata, sample_rate=sample_rate)
    initial = [estimate.amplitude, estimate.frequency, estimate.phaseshft, estimate.vertoffst]

    fit = LsqFit.curve_fit(sin_model, xdata, ydata, initial)
    params = fit.param

    if params[1] < 0
        params[1] *= -1
        params[3] -= 180
    end
    params[3] = mod(params[3], 360)
    if params[3] > 180
        params[3] = params[3] - 360
    elseif params[3] < -180
        params[3] = params[3] + 360
    end

    WaveFitParams(params...)
end

"""
    sin(x::Vector{T}, wp::WaveFitParams)::Vector{T} where {T<:Number}

Return sin for `x` with the parameters given in `wp`. Useful in order to plot the fit function, for example.

```
wp = calculate_wave_shape(xs, ys)
Plots.plot(xs, ys)
Plots.plot!(xs, sin.(xs, wp))
```

"""
function sin(xs::Vector{T}, wp::WaveFitParams)::Vector{T} where {T<:Number}
    wp.amplitude .* Base.sin.(wp.frequency .* xs .+ wp.phaseshft) .+ wp.vertoffst
end

end # module


export  w_legendre,
        build_w_jacobi,
        build_w_jacobi01,
        w_jacobi,
        w_jacobi01,
        w_laguerre,
        w_hermite,
        build_w_genhermite,
        build_w_genlaguerre,
        w_meixner_pollaczek,
        build_w_meixner_pollaczek,

        w_gaussian,
        w_uniform01,
        w_logistic,
        w_genhermite,
        build_w_beta,
        build_w_gamma

function w_legendre(t)
    -1. <= t <= 1. ? 1. : throw(error("$t not in support"))
end

function build_w_jacobi(a,b)
    @assert a > -1. && b > -1. "Invalid shape parameters"
    # return w(t) = (1-Float64(t))^a*(1+Float64(t))
    return t->w_jacobi(t,a,b)
end
function w_jacobi(t,a,b)
    -1. <=t<= 1. ? ((1-t)^a*(1+t)^b) : throw(error("$t not in support"))
end

function build_w_jacobi01(a,b)
    @assert a > -1. && b > -1. "Invalid shape parameters"
    # return w(t) = (1-Float64(t))^a*(1+Float64(t))
    return t->w_jacobi01(t,a,b)
end
function w_jacobi01(t,a,b)
    0. <=t<= 1. ? ((1-t)^a*t^b) : throw(error("$t not in support"))
end

function w_hermite(t)
    exp(-t^2)
end

function build_w_genhermite(mu)
    @assert mu>-0.5 "invalid parameter ($mu !> -0.5)"
    return t->w_genhermite(t,mu)
end
function w_genhermite(t,μ::Float64)
    abs(t)^(2*μ)*exp(-t^2)
end
function build_w_genlaguerre(a)
    @assert a>-1 "Invalid shape parameter"
    # return w(t) = Float64(t)^a*exp(-Float64(t))
    return t->w_genlaguerre(t,a)
end
function w_genlaguerre(t,a)
    t>=0. ? Float64(t)^a*exp(-Float64(t)) : throw(error("$t not in support"))
end

function w_laguerre(t)
    t>=0. ? exp(-t) : throw(error("$t not in support"))
end

function w_meixner_pollaczek(t,lambda,phi)
    1/(2*pi)*exp((2*phi-pi)*t)*abs(gamma(lambda+im*t))^2
end

function build_w_meixner_pollaczek(lambda,phi)
    @assert lambda>0 "lambda has to be positive"
    @assert 0<phi<pi "phi has to be between 0 and pi"
    t->w_meixner_pollaczek(t,lambda,phi)
end

##################################################
# probability density functions
function w_gaussian(t)
    1/(sqrt(2*pi))*exp(-0.5*t^2)
end

function build_w_beta(α,β)
    @assert α>-0. && β>-0. "Invalid shape parameters"
    return t->w_beta(t,α,β)
end
function w_beta(t,α,β)
    # 0. <=t<= 1. ? (t^(α-1)*(1-t)^(β-1)/beta(α,β)) : (error("$t not in support"))
    t^(α-1)*(1-t)^(β-1)/beta(α,β)
end

function build_w_gamma(α)
    @assert α>0 "Invalid shape parameter"
    return t->w_gamma(t,α)
end
function w_gamma(t,α)
    t>=0. ? (1/gamma(α)*Float64(t)^(α-1)*exp(-t)) : (error("$t not in support"))
end

function w_uniform01(t)
    0. <=t<= 1. ? 1. : (error("$t not in support"))
end

function w_logistic(t::Float64)
    0.25*sech(0.5t)^2
end


module Laplacian

# Create a Kronecker Delta matrix with the given relationship and dimensions
function δ(fi::Function, fj::Function, n::Integer)
    r = zeros(n,n)
    for j in [1:n]
        for i in [1:n]
            fi(i) == fj(j) && (r[i,j] = 1.0)
        end
    end
    return r
end

kronecker = δ

# Create a laplacian matrix with specified grid size and grid divisions
function lap(N::Integer, L::Real)

    Np1sq = (N+1)*(N+1)                 # have (N+1)^2 grid points
    a = -4.0δ(i->i, j->j, Np1sq)
    a += δ(i->mod1(i+1, Np1sq), j->j, Np1sq)
    a += δ(i->mod1(i-1, Np1sq), j->j, Np1sq)
    a += δ(i->mod1(i+N+1, Np1sq), j->j, Np1sq)
    a += δ(i->mod1(i-N-1, Np1sq), j->j, Np1sq)
    return a * (N*N / (L*L))

end

lap(N::Integer) = lap(N,1)

# Split a laplacian matrix into free and boundary parts
function opsplit(A::Array, β::Array{Int,1})

    ndims(A) == 2 || error("operator matrix must be 2-d")
    length(β) == size(A)[1] || error("specify which indices are boundary conditions")
    size(A)[1] == size(A)[2] || error("operator matrix must be square")

    B = zeros(size(A))
    F = zeros(size(A))
    # Set the boundary condition columns equal to the corresponding columns of A
    for i in 1:length(β)
        β[i] == 1 ? B[:,i] = A[:,i] : F[:,i] = A[:,i]
    end
    return F, B

end

# β[i] = 1 iff ϕ[i] is a boundary condition
function rhoeff(ρ::Array, Δb::Array, ϕb::Array, β::Array{Int,1})
    return (ρ + Δb*ϕb) .* (1 .- β)
end

# Cancel out bound rows to get Δeffective
function deltaeff(Δf, β::Array{Int,1})
    return Δf .* (1 .- β)
end

end


# Ported to Julia by Andy Ferris, 2016, and re-released under an MIT license.
#/**
# * Copyright (c) Charles Karney (2008-2015) <charles@karney.com> and licensed
# * under the MIT/X11 License.  For more information, see
# * http://geographiclib.sourceforge.net/
# **********************************************************************/

"""
eatanhe(x, e2) returns e*atanh(e*x) where e = sqrt(e2)
e2 is a scalar; x can be any shape.
"""
function eatanhe(x, e2)
    e = sqrt(abs(e2))
    if (e2 >= 0)
        y = e * atanh(e * x)
    else
        y = -e * atan(e * x)
    end
end

"""
%TAUF   tan(phi)
%
%   TAUF(taup, e2) returns tangent of phi in terms of taup the tangent of
%   chi.  e2, the square of the eccentricity, is a scalar; taup can be any
%   shape.
"""
function tauf(taup, e2)
    numit = 5
    e2m = 1 - e2     # This possibly disagrees with the C++ code in the prolate case
    tau = taup / e2m
    stol = 0.1 * sqrt(eps(Float64)) * max(1.0, abs(taup))
    g = isfinite(tau)
    for i = 1 : numit
        if !g
            break
        end
        tau1 = hypot(1, tau)
        sig = sinh( eatanhe( tau / tau1, e2 ) )
        taupa = hypot(1, sig) * tau - sig * tau1
        dtau = (taup - taupa) * (1 + e2m * tau*tau) / (e2m * tau1 * hypot(1, taupa))
        tau = tau + dtau
        g = g & (abs(dtau) >= stol)
    end
    return tau
end

"""
taupf(tau, e2) returns tangent of chi in terms of tau the tangent of
phi.  e2, the square of the eccentricity, is a scalar; tau can be any
shape.
"""
function taupf(tau, e2)
  tau1 = hypot(1, tau)
  sig = sinh( eatanhe( tau / tau1, e2 ) )
  return hypot(1, sig) * tau - sig * tau1
end

# Code adapted from MATLAB port of geograhiclib, which was licensed under
# a permissive MIT-like license and the following copyright notice
#     Copyright (c) 2016, Charles Karney
#     All rights reserved.

"""
Evaluate polynomial with coefficients p[i] at x
"""
function polyval(p, x)
    y = 1
    out = p[end]
    for i = length(p)-1:-1:1
        y *= x
        out += p[i] * y
    end
    return out
end

"""
LatFix(x) returns x is it is in the range [-90, 90]; otherwise it
returns NaN.
"""
function LatFix(x::Number)
    y = x
    if abs(x) > 90
        y = NaN
    end
    return y
end

function AngDiff(x,y)
    AngNormalize(AngNormalize(y) - AngNormalize(x))
    #(d, t) = sumx(AngNormalize(x), AngNormalize(-y))
    #d = - AngNormalize(d)
    #if d == 180 && t < 0
    #    d = -180
    #end
    #return sumx(d, -t)[1]
end

"""
%AngNormalize:  Reduce angle to range [-180, 180)
%
%   x = AngNormalize(x) reduces angles to the range [-180, 180).
"""
function AngNormalize(x::Number)
    x = rem(x, 360)
    if x >= 180
        x = x - 360
    elseif x < -180
        x = x + 360
    end

    return x
end

function atan2d(y, x)
    # In order to minimize round-off errors, this function rearranges the
    # arguments so that result of atan is in the range [-pi/4, pi/4] before
    # converting it to degrees and mapping the result to the correct
    # quadrant.
    q = 0
    if (abs(y) > abs(x))
        tmp = x
        x = y
        y = tmp
        q = 2
    end
    if (x < 0)
        x = -x
        q = q + 1
    end
    # here x >= 0 and x >= abs(y), so angle is in [-pi/4, pi/4]
    ang = atan(y, x) * 180 / pi
    # Note that atan2d(-0.0, 1.0) will return -0.  However, we expect that
    # atan2d will not be called with y = -0.  If need be, include
    #
    #   case 0: ang = 0 + ang; break;
    #
    # and handle mpfr as in AngRound.
    if q == 1
        ang = (y > 0 ? 180 : -180) - ang
    elseif q == 2
        ang =  90 - ang
    elseif q == 3
        ang = -90 + ang
    end
    return ang
end

"""
    immutable TransverseMercator{MaxPow}
    TransverseMercator(datum)
    TransverseMercator(ellipsoid)
    TransverseMercator(a, f, [::Type{Val{MaxPow}} = Val{6}])

Cache of ellipsoidal calculations necessary for transverse-Mercator and
polar-stereographic transformations. Series expansion coefficients up to order
`MaxPow` (between 4 and 8, default 6) are calculated and stored for fast
transverse-Mercator and UTM calculations.
"""
struct TransverseMercator{MaxPow}
    a::Float64
    f::Float64
    e2::Float64
    es::Float64
    e2m::Float64
    c::Float64
    n::Float64
    a1::Float64
    b1::Float64
    alp::NTuple{MaxPow, Float64}
    bet::NTuple{MaxPow, Float64}
end

TransverseMercator(datum) = TransverseMercator(ellipsoid(datum))
TransverseMercator(el::Ellipsoid) = TransverseMercator(el.a, 1-el.b/el.a)
TransverseMercator(a,f) = TransverseMercator(a,f,Val{6}) # Default to sixth-order expansion
function TransverseMercator(a::Float64, f::Float64, ::Type{Val{MaxPow}}) where MaxPow
    e2 = f * (2 - f)
    es = (f < 0 ? -1 : 1) * sqrt(abs(e2))
    e2m = (1 - e2)
    # c = sqrt( pow(1 + e, 1 + e) * pow(1 - e, 1 - e) ) )
    # See, for example, Lee (1976), p 100.
    c = sqrt(e2m) * exp(eatanhe(1.0, e2)) # Maybe revert to C++ version with es
    n = f / (2 - f)

    (1 - f) * exp(eatanhe(1.0, es))

    if !(isa(MaxPow,Int)) || MaxPow < 4 || MaxPow > 8
        error("MaxPow must be 4, 5, 6, 7 or 8")
    end

    # Stack-allocated constants
    if MaxPow == 4
        b1coeff = (1, 16, 64, 64)

        alpcoeff = (# alp[1]/n^1, polynomial in n of order 3
                    164, 225, -480, 360, 720,
                    # alp[2]/n^2, polynomial in n of order 2
                    557, -864, 390, 1440,
                    # alp[3]/n^3, polynomial in n of order 1
                    -1236, 427, 1680,
                    # alp[4]/n^4, polynomial in n of order 0
                    49561, 161280)

        betcoeff = (# bet[1]/n^1, polynomial in n of order 3
                    -4, 555, -960, 720, 1440,
                    # bet[2]/n^2, polynomial in n of order 2
                    -437, 96, 30, 1440,
                    # bet[3]/n^3, polynomial in n of order 1
                    -148, 119, 3360,
                    # bet[4]/n^4, polynomial in n of order 0
                    4397, 161280)
    elseif MaxPow == 5
        b1coeff = (1, 16, 64, 64)

        alpcoeff = (# alp[1]/n^1, polynomial in n of order 4
                    -635, 328, 450, -960, 720, 1440,
                    # alp[2]/n^2, polynomial in n of order 3
                    4496, 3899, -6048, 2730, 10080,
                    # alp[3]/n^3, polynomial in n of order 2
                    15061, -19776, 6832, 26880,
                    # alp[4]/n^4, polynomial in n of order 1
                    -171840, 49561, 161280,
                    # alp[5]/n^5, polynomial in n of order 0
                    34729, 80640)

        betcoeff = (# bet[1]/n^1, polynomial in n of order 4
                    -3645, -64, 8880, -15360, 11520, 23040,
                    # bet[2]/n^2, polynomial in n of order 3
                    4416, -3059, 672, 210, 10080,
                    # bet[3]/n^3, polynomial in n of order 2
                    -627, -592, 476, 13440,
                    # bet[4]/n^4, polynomial in n of order 1
                    -3520, 4397, 161280,
                    # bet[5]/n^5, polynomial in n of order 0
                    4583, 161280)
    elseif MaxPow == 6
        b1coeff = (1, 4, 64, 256, 256)

        alpcoeff = (# alp[1]/n^1, polynomial in n of order 5
                    31564, -66675, 34440, 47250, -100800, 75600, 151200,
                    # alp[2]/n^2, polynomial in n of order 4
                    -1983433, 863232, 748608, -1161216, 524160, 1935360,
                    # alp[3]/n^3, polynomial in n of order 3
                    670412, 406647, -533952, 184464, 725760,
                    # alp[4]/n^4, polynomial in n of order 2
                    6601661, -7732800, 2230245, 7257600,
                    # alp[5]/n^5, polynomial in n of order 1
                    -13675556, 3438171, 7983360,
                    # alp[6]/n^6, polynomial in n of order 0
                    212378941, 319334400)

        betcoeff = (# bet[1]/n^1, polynomial in n of order 5
                    384796, -382725, -6720, 932400, -1612800, 1209600, 2419200,
                    # bet[2]/n^2, polynomial in n of order 4
                    -1118711, 1695744, -1174656, 258048, 80640, 3870720,
                    # bet[3]/n^3, polynomial in n of order 3
                    22276, -16929, -15984, 12852, 362880,
                    # bet[4]/n^4, polynomial in n of order 2
                    -830251, -158400, 197865, 7257600,
                    # bet[5]/n^5, polynomial in n of order 1
                    -435388, 453717, 15966720,
                    # bet[6]/n^6, polynomial in n of order 0
                    20648693, 638668800)
    elseif MaxPow == 7
        b1coeff = (1, 4, 64, 256, 256)

        alpcoeff = (# alp[1]/n^1, polynomial in n of order 6
                    1804025, 2020096, -4267200, 2204160, 3024000, -6451200, 4838400, 9676800,
                    # alp[2]/n^2, polynomial in n of order 5
                    4626384, -9917165, 4316160, 3743040, -5806080, 2620800, 9676800,
                    # alp[3]/n^3, polynomial in n of order 4
                    -67102379, 26816480, 16265880, -21358080, 7378560, 29030400,
                    # alp[4]/n^4, polynomial in n of order 3
                    155912000, 72618271, -85060800, 24532695, 79833600,
                    # alp[5]/n^5, polynomial in n of order 2
                    102508609, -109404448, 27505368, 63866880,
                    # alp[6]/n^6, polynomial in n of order 1
                    -12282192400, 2760926233, 4151347200,
                    # alp[7]/n^7, polynomial in n of order 0
                    1522256789, 1383782400)

        betcoeff = (# bet[1]/n^1, polynomial in n of order 6
                    -5406467, 6156736, -6123600, -107520, 14918400, -25804800, 19353600,
                    38707200,
                    # bet[2]/n^2, polynomial in n of order 5
                    829456, -5593555, 8478720, -5873280, 1290240, 403200, 19353600,
                    # bet[3]/n^3, polynomial in n of order 4
                    9261899, 3564160, -2708640, -2557440, 2056320, 58060800,
                    # bet[4]/n^4, polynomial in n of order 3
                    14928352, -9132761, -1742400, 2176515, 79833600,
                    # bet[5]/n^5, polynomial in n of order 2
                    -8005831, -1741552, 1814868, 63866880,
                    # bet[6]/n^6, polynomial in n of order 1
                    -261810608, 268433009, 8302694400,
                    # bet[7]/n^7, polynomial in n of order 0
                    219941297, 5535129600)
    elseif MaxPow == 8
        b1coeff = (25, 64, 256, 4096, 16384, 16384)

        alpcoeff = (# alp[1]/n^1, polynomial in n of order 7
                    -75900428, 37884525, 42422016, -89611200, 46287360, 63504000, -135475200,
                    101606400, 203212800,
                    # alp[2]/n^2, polynomial in n of order 6
                    148003883, 83274912, -178508970, 77690880, 67374720, -104509440,
                    47174400, 174182400,
                    # alp[3]/n^3, polynomial in n of order 5
                    318729724, -738126169, 294981280, 178924680, -234938880, 81164160,
                    319334400,
                    # alp[4]/n^4, polynomial in n of order 4
                    -40176129013, 14967552000, 6971354016, -8165836800, 2355138720,
                    7664025600,
                    # alp[5]/n^5, polynomial in n of order 3
                    10421654396, 3997835751, -4266773472, 1072709352, 2490808320,
                    # alp[6]/n^6, polynomial in n of order 2
                    175214326799, -171950693600, 38652967262, 58118860800,
                    # alp[7]/n^7, polynomial in n of order 1
                    -67039739596, 13700311101, 12454041600,
                    # alp[8]/n^8, polynomial in n of order 0
                    1424729850961, 743921418240)

        betcoeff = (# bet[1]/n^1, polynomial in n of order 7
                    31777436, -37845269, 43097152, -42865200, -752640, 104428800, -180633600,
                    135475200, 270950400,
                    # bet[2]/n^2, polynomial in n of order 6
                    24749483, 14930208, -100683990, 152616960, -105719040, 23224320, 7257600,
                    348364800,
                    # bet[3]/n^3, polynomial in n of order 5
                    -232468668, 101880889, 39205760, -29795040, -28131840, 22619520,
                    638668800,
                    # bet[4]/n^4, polynomial in n of order 4
                    324154477, 1433121792, -876745056, -167270400, 208945440, 7664025600,
                    # bet[5]/n^5, polynomial in n of order 3
                    457888660, -312227409, -67920528, 70779852, 2490808320,
                    # bet[6]/n^6, polynomial in n of order 2
                    -19841813847, -3665348512, 3758062126, 116237721600,
                    # bet[7]/n^7, polynomial in n of order 1
                    -1989295244, 1979471673, 49816166400,
                    # bet[8]/n^8, polynomial in n of order 0
                    191773887257, 3719607091200)
    end

    m = div(MaxPow, 2)
    b1 = polyval(b1coeff[1:m+1], n*n) / (b1coeff[m+2] * (1 + n))

    # a1 is the equivalent radius for computing the circumference of ellipse.
    a1 = b1 * a
    o = 1
    d = n
    _alp = Vector{Float64}(undef, MaxPow)
    _bet = Vector{Float64}(undef, MaxPow)
    for l = 1:MaxPow
        m = MaxPow - l
        _alp[l] = d * polyval(alpcoeff[o:o+m], n) / alpcoeff[o + m + 1]
        _bet[l] = d * polyval(betcoeff[o:o+m], n) / betcoeff[o + m + 1]
        o += m + 2
        d *= n
    end
    # Post condition: o == sizeof(alpcoeff) / sizeof(real) &&
    # o == sizeof(betcoeff) / sizeof(real)
    alp = (_alp...,)
    bet = (_bet...,)

    return TransverseMercator{MaxPow}(a,f,e2,es,e2m,c,n,a1,b1,alp,bet)
end

const wgs84_tm = TransverseMercator(wgs84_ellipsoid)
const airy1830_tm = TransverseMercator(airy1830)
const clarke1866_tm = TransverseMercator(clarke1866)
const grs80_tm = TransverseMercator(grs80)

TransverseMercator(::WGS84) = wgs84_tm
TransverseMercator(::OSGB36) = airy1830_tm
TransverseMercator(::NAD27) = clarke1866_tm
TransverseMercator(::GDA94) = grs80_tm

# // Engsager and Poder (2007) use trigonometric series to convert between phi
# // and phip.  Here are the series...
# //
# // Conversion from phi to phip:
# //
# //     phip = phi + sum(c[j] * sin(2*j*phi), j, 1, 6)
# //
# //       c[1] = - 2 * n
# //              + 2/3 * n^2
# //              + 4/3 * n^3
# //              - 82/45 * n^4
# //              + 32/45 * n^5
# //              + 4642/4725 * n^6;
# //       c[2] =   5/3 * n^2
# //              - 16/15 * n^3
# //              - 13/9 * n^4
# //              + 904/315 * n^5
# //              - 1522/945 * n^6;
# //       c[3] = - 26/15 * n^3
# //              + 34/21 * n^4
# //              + 8/5 * n^5
# //              - 12686/2835 * n^6;
# //       c[4] =   1237/630 * n^4
# //              - 12/5 * n^5
# //              - 24832/14175 * n^6;
# //       c[5] = - 734/315 * n^5
# //              + 109598/31185 * n^6;
# //       c[6] =   444337/155925 * n^6;
# //
# // Conversion from phip to phi:
# //
# //     phi = phip + sum(d[j] * sin(2*j*phip), j, 1, 6)
# //
# //       d[1] =   2 * n
# //              - 2/3 * n^2
# //              - 2 * n^3
# //              + 116/45 * n^4
# //              + 26/45 * n^5
# //              - 2854/675 * n^6;
# //       d[2] =   7/3 * n^2
# //              - 8/5 * n^3
# //              - 227/45 * n^4
# //              + 2704/315 * n^5
# //              + 2323/945 * n^6;
# //       d[3] =   56/15 * n^3
# //              - 136/35 * n^4
# //              - 1262/105 * n^5
# //              + 73814/2835 * n^6;
# //       d[4] =   4279/630 * n^4
# //              - 332/35 * n^5
# //              - 399572/14175 * n^6;
# //       d[5] =   4174/315 * n^5
# //              - 144838/6237 * n^6;
# //       d[6] =   601676/22275 * n^6;
# //
# // In order to maintain sufficient relative accuracy close to the pole use
# //
# //     S = sum(c[i]*sin(2*i*phi),i,1,6)
# //     taup = (tau + tan(S)) / (1 - tau * tan(S))
#
# // In Math::taupf and Math::tauf we evaluate the forward transform explicitly
# // and solve the reverse one by Newton's method.
# //
# // There are adapted from TransverseMercatorExact (taup and taupinv).  tau =
# // tan(phi), taup = sinh(psi)

"""
    (x, y, γ, k) = transverse_mercator_forward(lon0, lat, lon, k0, tm::TransverseMercator)

Perform transverse-Mercator projection of `lat` and `lon` with respect to reference
meridian `lat0` and horizontal scaling `k0` (`= 0.9996` for UTM) using a
series expansion approach (see `TransverseMercator`). `γ` and `k` are the local
convergence and scaling factors, respectively.
"""
function transverse_mercator_forward(lon0, lat, lon, k0, tm::TransverseMercator{MaxPow}) where MaxPow
    lat = LatFix(lat)
    lon = AngDiff(lon0, lon)
    if abs(lon) > 50
        error("Relative longitude is too large for transverse-Mercator expansion, and must be less than 50° (received $(lon)°). Possibly used wrong UTM zone.")
    end
    # Explicitly enforce the parity
    latsign = lat < 0 ? -1 : 1
    lonsign = lon < 0 ? -1 : 1
    lon *= lonsign
    lat *= latsign
    backside = lon > 90
    if backside
        if (lat == 0)
            latsign = -1
        end
        lon = 180 - lon
    end

    sphi = sind(lat)
    cphi = cosd(lat)
    slam = sind(lon)
    clam = cosd(lon)

    # phi = latitude
    # phi' = conformal latitude
    # psi = isometric latitude
    # tau = tan(phi)
    # tau' = tan(phi')
    # [xi', eta'] = Gauss-Schreiber TM coordinates
    # [xi, eta] = Gauss-Krueger TM coordinates
    #
    # We use
    #   tan(phi') = sinh(psi)
    #   sin(phi') = tanh(psi)
    #   cos(phi') = sech(psi)
    #   denom^2    = 1-cos(phi')^2*sin(lam)^2 = 1-sech(psi)^2*sin(lam)^2
    #   sin(xip)   = sin(phi')/denom          = tanh(psi)/denom
    #   cos(xip)   = cos(phi')*cos(lam)/denom = sech(psi)*cos(lam)/denom
    #   cosh(etap) = 1/denom                  = 1/denom
    #   sinh(etap) = cos(phi')*sin(lam)/denom = sech(psi)*sin(lam)/denom
    if (lat != 90)
        tau = sphi / cphi
        taup = taupf(tau, tm.e2) # TODO maybe switch to the C++ version of taupf using tm.es?
        xip = atan(taup, clam)
        # Used to be
        #   etap = Math::atanh(sin(lam) / cosh(psi));
        etap = asinh(slam / hypot(taup, clam))
        # convergence and scale for Gauss-Schreiber TM (xip, etap) -- gamma0 =
        # atan(tan(xip) * tanh(etap)) = atan(tan(lam) * sin(phi'));
        # sin(phi') = tau'/sqrt(1 + tau'^2)
        # Krueger p 22 (44)
        gamma = atan2d(slam * taup, clam * hypot(1.0, taup))
        # k0 = sqrt(1 - _e2 * sin(phi)^2) * (cos(phi') / cos(phi)) * cosh(etap)
        # Note 1/cos(phi) = cosh(psip);
        # and cos(phi') * cosh(etap) = 1/hypot(sinh(psi), cos(lam))
        #
        # This form has cancelling errors.  This property is lost if cosh(psip)
        # is replaced by 1/cos(phi), even though it's using "primary" data (phi
        # instead of psip).
        k = sqrt(tm.e2m + tm.e2 * cphi*cphi) * hypot(1.0, tau) / hypot(taup, clam)
    else
        xip = pi/2
        etap = 0.0
        gamma = lon
        k = tm.c
    end

    # {xi',eta'} is {northing,easting} for Gauss-Schreiber transverse Mercator
    # (for eta' = 0, xi' = bet). {xi,eta} is {northing,easting} for transverse
    # Mercator with constant scale on the central meridian (for eta = 0, xip =
    # rectifying latitude).  Define
    #
    #   zeta = xi + i*eta
    #   zeta' = xi' + i*eta'
    #
    # The conversion from conformal to rectifying latitude can be expressed as
    # a series in _n:
    #
    #   zeta = zeta' + sum(h[j-1]' * sin(2 * j * zeta'), j = 1..maxpow_)
    #
    # where h[j]' = O(_n^j).  The reversion of this series gives
    #
    #   zeta' = zeta - sum(h[j-1] * sin(2 * j * zeta), j = 1..maxpow_)
    #
    # which is used in Reverse.
    #
    # Evaluate sums via Clenshaw method.  See
    #    http:#mathworld.wolfram.com/ClenshawRecurrenceFormula.html
    #
    # Let
    #
    #    S = sum(c[k] * F[k](x), k = 0..N)
    #    F[n+1](x) = alpha(n,x) * F[n](x) + beta(n,x) * F[n-1](x)
    #
    # Evaluate S with
    #
    #    y[N+2] = y[N+1] = 0
    #    y[k] = alpha(k,x) * y[k+1] + beta(k+1,x) * y[k+2] + c[k]
    #    S = c[0] * F[0](x) + y[1] * F[1](x) + beta(1,x) * F[0](x) * y[2]
    #
    # Here we have
    #
    #    x = 2 * zeta'
    #    F[n](x) = sin(n * x)
    #    a(n, x) = 2 * cos(x)
    #    b(n, x) = -1
    #    [ sin(A+B) - 2*cos(B)*sin(A) + sin(A-B) = 0, A = n*x, B = x ]
    #    N = maxpow_
    #    c[k] = _alp[k]
    #    S = y[1] * sin(x)
    #
    # For the derivative we have
    #
    #    x = 2 * zeta'
    #    F[n](x) = cos(n * x)
    #    a(n, x) = 2 * cos(x)
    #    b(n, x) = -1
    #    [ cos(A+B) - 2*cos(B)*cos(A) + cos(A-B) = 0, A = n*x, B = x ]
    #    c[0] = 1; c[k] = 2*k*_alp[k]
    #    S = (c[0] - y[2]) + y[1] * cos(x)
    c0 = cos(2 * xip)
    ch0 = cosh(2 * etap)
    s0 = sin(2 * xip)
    sh0 = sinh(2 * etap)
    ar = 2 * c0 * ch0
    ai = -2 * s0 * sh0 # 2 * cos(2*zeta')

    n = MaxPow

    xi0 = (isodd(n) ? tm.alp[n] : 0.0)
    eta0 = 0.0
    xi1 = 0.0
    eta1 = 0.0

    # Accumulators for dzeta/dzeta'
    if isodd(n)
        yr0 = 2 * MaxPow * tm.alp[n]
        n = n - 1
    else
        yr0 = 0.0
    end
    yi0 = 0.0
    yr1 = 0.0
    yi1 = 0.0
    while n > 0
        xi1  = ar * xi0 - ai * eta0 - xi1 + tm.alp[n]
        eta1 = ai * xi0 + ar * eta0 - eta1
        yr1 = ar * yr0 - ai * yi0 - yr1 + 2 * n * tm.alp[n]
        yi1 = ai * yr0 + ar * yi0 - yi1

        n = n - 1

        xi0  = ar * xi1 - ai * eta1 - xi0 + tm.alp[n]
        eta0 = ai * xi1 + ar * eta1 - eta0
        yr0 = ar * yr1 - ai * yi1 - yr0 + 2 * n * tm.alp[n]
        yi0 = ai * yr1 + ar * yi1 - yi0

        n = n - 1
    end

    ar *= 0.5
    ai *= 0.5      # cos(2*zeta')
    yr1 = 1 - yr1 + ar * yr0 - ai * yi0
    yi1 =   - yi1 + ai * yr0 + ar * yi0
    ar = s0 * ch0
    ai = c0 * sh0  # sin(2*zeta')

    xi  = xip  + ar * xi0 - ai * eta0
    eta = etap + ai * xi0 + ar * eta0

    # Fold in change in convergence and scale for Gauss-Schreiber TM to
    # Gauss-Krueger TM.

    gamma -= atan2d(yi1, yr1)
    k *= tm.b1 * hypot(yr1, yi1)
    y = tm.a1 * k0 * (backside ? pi - xi : xi) * latsign
    x = tm.a1 * k0 * eta * lonsign
    if backside
        gamma = 180 - gamma
    end
    gamma *= latsign * lonsign
    gamma = AngNormalize(gamma)
    k *= k0

    return (x, y, gamma, k)
end

"""
    (lat, lon, γ, k) = transverse_mercator_reverse(lon0, x, y, k0, tm::TransverseMercator)

Invert transverse-Mercator projection of `x` and `y` with respect to reference
meridian `lat0` and horizontal scaling `k0` (`= 0.9996` for UTM) using a
series expansion approach (see `TransverseMercator`). `γ` and `k` are the local
convergence and scaling factors, respectively.
"""
function transverse_mercator_reverse(lon0, x, y, k0, tm::TransverseMercator{MaxPow}) where MaxPow
    # This undoes the steps in transverse_mercator_forward.  The wrinkles are: (1) Use of the
    # reverted series to express zeta' in terms of zeta. (2) Newton's method
    # to solve for phi in terms of tan(phi).
    xi = y / (tm.a1 * k0)
    eta = x / (tm.a1 * k0)

    # Explicitly enforce the parity
    xisign = xi < 0 ? -1 : 1
    etasign = eta < 0 ? -1 : 1
    xi *= xisign
    eta *= etasign
    backside = xi > pi/2
    if backside
        xi = pi - xi
    end

    c0 = cos(2 * xi)
    ch0 = cosh(2 * eta)
    s0 = sin(2 * xi)
    sh0 = sinh(2 * eta)

    ar = 2 * c0 * ch0
    ai = -2 * s0 * sh0 # 2 * cos(2*zeta)
    n = MaxPow

    # Accumulators for zeta'
    xip0 = (isodd(n) ? -tm.bet[n] : 0.0)
    etap0 = 0.0
    xip1 = 0.0
    etap1 = 0.0

    # Accumulators for dzeta'/dzeta
    if isodd(n)
        yr0 =  2 * MaxPow * tm.bet[n]
        n -= 1
    else
        yr0 = 0.0
    end
    yi0 = 0.0
    yr1 = 0.0
    yi1 = 0.0

    while (n > 0)
        xip1  = ar * xip0 - ai * etap0 - xip1 - tm.bet[n]
        etap1 = ai * xip0 + ar * etap0 - etap1
        yr1 = ar * yr0 - ai * yi0 - yr1 - 2 * n * tm.bet[n]
        yi1 = ai * yr0 + ar * yi0 - yi1

        n = n - 1

        xip0  = ar * xip1 - ai * etap1 - xip0 - tm.bet[n]
        etap0 = ai * xip1 + ar * etap1 - etap0
        yr0 = ar * yr1 - ai * yi1 - yr0 - 2 * n * tm.bet[n]
        yi0 = ai * yr1 + ar * yi1 - yi0

        n = n - 1
    end

    ar *= 0.5
    ai *= 0.5  # cos(2*zeta')
    yr1 = 1 - yr1 + ar * yr0 - ai * yi0
    yi1 =   - yi1 + ai * yr0 + ar * yi0
    ar = s0 * ch0
    ai = c0 * sh0 # sin(2*zeta)

    xip  = xi  + ar * xip0 - ai * etap0
    etap = eta + ai * xip0 + ar * etap0

    # Convergence and scale for Gauss-Schreiber TM to Gauss-Krueger TM.
    gamma = atand(yi1, yr1)
    k = tm.b1 / hypot(yr1, yi1)
    # JHS 154 has
    #
    #   phi' = asin(sin(xi') / cosh(eta')) (Krueger p 17 (25))
    #   lam = asin(tanh(eta') / cos(phi')
    #   psi = asinh(tan(phi'))

    s = sinh(etap)
    c = max(0.0, cos(xip))  # cos(pi/2) might be negative
    r = hypot(s, c)

    if (r != 0)
        lon = atand(s, c)# Krueger p 17 (25)

        # Use Newton's method to solve for tau
        sxip = sin(xip)
        tau = tauf(sxip/r, tm.e2) # TODO maybe change to C++ es version
        gamma += atand(sxip * tanh(etap), c) # Krueger p 19 (31)
        lat = atand(tau)
        # Note cos(phi') * cosh(eta') = r
        k *= sqrt(tm.e2m + tm.e2 / (1 + tau*tau)) * hypot(1.0, tau) * r
    else
        lat = 90.0
        lon = 0.0
        k *= tm.c
    end

    lat *= xisign
    if backside
        lon = 180 - lon
    end
    lon *= etasign
    if abs(lon) > 50
        error("Relative longitude is too large for transverse-Mercator expansion, and must be less than 50° (calculated $(lon)°). Possibly used wrong UTM zone.")
    end
    lon = AngNormalize(lon + lon0)
    if backside
        gamma = 180 - gamma
    end
    gamma *= xisign * etasign
    gamma = AngNormalize(gamma)
    k *= k0

    return (lat, lon, gamma, k)
end


"""
x = nnls(A, b; ...)

Solves non-negative least-squares problem by the active set method
of Lawson & Hanson (1974).

Optional arguments:
    tol: tolerance for nonnegativity constraints
    max_iter: maximum number of iterations (counts inner loop iterations)

References:
    Lawson, C.L. and R.J. Hanson, Solving Least-Squares Problems,
    Prentice-Hall, Chapter 23, p. 161, 1974.
"""
function nnls(A::Matrix{Float64},
              b::Vector{Float64};
              tol::Float64=1e-8,
              max_iter=30*size(A,2),
              kwargs...)

    # dimensions, initialize solution
    m,n = size(A)
    x = zeros(n)

    # P is a bool array storing positive elements of x
    # i.e., x[P] > 0 and x[~P] == 0
    P = zeros(Bool,n)

    # We have reached an optimum when either:
    #   (a) all elements of x are positive (no nonneg constraints activated)
    #   (b) ∂f/∂x = A' * (b - A*x) > 0 for all nonpositive elements of x
    w = A' * (b - A*x)
    iter = 0
    while sum(P)<n && any(w[~P].>tol) && iter < max_iter

        # find i that maximizes w, restricting i to indices not in P
        # Note: the while loop condition guarantees at least one w[~P]>0
        i = indmax(w .* ~P)

        # Move i to P
        P[i] = true

        # Solve least-squares problem, with zeros for columns/elements not in P
        Ap = zeros(m,n)
        Ap[:,P] = A[:,P]
        s = pinv(Ap)*b
        s[~P] = 0.0 # zero out elements not in P

        # Inner loop: deal with negative elements of s
        while any(s[P].<=tol)
            iter += 1

            # find indices in P where s is negative
            ind = (s.<=tol) & P

            # calculate step size, α, to prevent any xᵢ from going negative
            α = minimum(x[ind] ./ (x[ind] - s[ind]))

            # update solution (pushes some xᵢ to zero)
            x += α*(s-x)

            # Remove all i in P where x[i] == 0
            for i = 1:n
                if P[i] && abs(x[i]) < tol
                    # remove i from P
                    P[i] = false
                    # zero out column i of Ap
                    Ap[:,i] *= 0.0
                end
            end

            # Solve least-squares problem again, zeroing nonpositive columns
            s = pinv(Ap)*b
            s[~P] = 0.0 # zero out elements not in P
        end

        # update solution
        x = deepcopy(s)
        w = A' * (b - A*x)
    end
    return x
end

function nnls(A::Matrix{Float64},
              B::Matrix{Float64};
              use_parallel = true,
              kwargs...)

    m,n = size(A)
    k = size(B,2)

    if use_parallel && nprocs()>1
        X = SharedArray(Float64,n,k)
        @sync @parallel for i = 1:k
            X[:,i] = nnls(A, B[:,i]; kwargs...)
        end
        X = convert(Array,X)
    else
        X = Array(Float64,n,k)
        for i = 1:k
            X[:,i] = nnls(A, B[:,i]; kwargs...)
        end
    end

    return X
end


# Algebra generator for CliffordAlgebras.jl

import Base.show
import Combinatorics.levicivita
import PrettyTables.pretty_table

"""
    basevectorproduct(Npos::Integer, Nneg::Integer, Nzero::Integer, base::Tuple, kl::Integer, kr::Integer)

Finds the product of the kl-th base vector with the kr-th base vector. Returns a base vector index and a scale factor.
"""
function basevectorproduct(Npos::Integer, Nneg::Integer, base::Tuple, kl::Integer, kr::Integer)
    K = length(base)
    @assert kr <= K && kl <= K
    selector_left = base[kl]
    selector_right = base[kr]
    selector_prod = collect((selector_left..., selector_right...))
    permutation = sortperm(selector_prod)
    coeff = levicivita(permutation)
    v = []
    for b in unique(selector_prod[permutation])
        c = count(isequal(b), selector_prod)
        @assert c in (1, 2)
        if c == 1
            push!(v, b)
        else
            coeff *= (b <= Npos) ? 1 : (b <= Npos + Nneg) ? -1 : 0
        end
    end
    v = Tuple(v)
    vl = length(v)
    bi = findfirst(b -> length(b) == vl && length(intersect(b, v)) == vl, base)
    coeff *= levicivita(findpermutation(v, base[bi]))
    (bi, coeff)
end

"""
    multiplicationstable(Npos::Integer, Nneg::Integer, Nzero::Integer, base::Tuple)

Generates a multiplication table for the Clifford algebra with the specified signature and base vectors.
"""
function multiplicationtable(Npos::Integer, Nneg::Integer, Nzero::Integer, base::Tuple)
    N = Npos + Nneg + Nzero
    K = 2^N
    @assert length(base) == K
    M = Matrix{Tuple}(undef, K, K)
    for kl = 1:K, kr = 1:K
        M[kl, kr] = basevectorproduct(Npos, Nneg, base, kl, kr)
    end
    ntuple(row -> ntuple(col -> M[row, col], K), K)
end

"""
    CliffordAlgebra(Npos::Integer, Nneg::Integer, Nzero::Integer, S::NTuple(N,Symbol))

Singleton instance of the type CliffordAlgebra that describes a geometric algebra with the signature (Npos,Nneg,Nzero), base symbols S.
The base symbols are in order of the signature.
"""
struct CliffordAlgebra{Np,Nn,Nz,S,BT}
    function CliffordAlgebra(
        Npos::Integer,
        Nneg::Integer,
        Nzero::Integer,
        BaseSymbols::NTuple{N,Symbol},
    ) where {N}
        Npos = Int(Npos)
        Nneg = Int(Nneg)
        Nzero = Int(Nzero)
        @assert Npos >= 0 && Nneg >= 0 && Nzero >=0 "Algebra signature must be non-negative."
        @assert Npos + Nneg + Nzero == N "Base symbol count must match signature."
        BT = adaptbasefordual(enumeratebase(Int(N)))
        new{Npos,Nneg,Nzero,BaseSymbols,BT}()
    end
end

"""
    CliffordAlgebra(N::Integer)

Generates a geometric algebra with signature (N,0,0).
"""
function CliffordAlgebra(N::Integer)
    @assert N>=0 "Algbra signature must be non-negative."
    CliffordAlgebra(N, 0, 0, ntuple(i -> Symbol(:e, i), N))
end

"""
    CliffordAlgebra(Npos::Integer, Nneg::Integer)

Generates a geometric algebra with signature (Npos,Nneg,0).
"""
function CliffordAlgebra(Npos::Integer, Nneg::Integer)
    @assert Npos >= 0 && Nneg >= 0 "Algbra signature must be non-negative."
    CliffordAlgebra(Npos, Nneg, 0, ntuple(i -> Symbol(:e, i), Npos + Nneg))
end

"""
    CliffordAlgebra(Npos::Integer, Nneg::Integer, Nzero::Integer)

Generates a geometric algebra with signature (Npos,Nneg,Nzero).
"""
function CliffordAlgebra(Npos::Integer, Nneg::Integer, Nzero::Integer)
    @assert Npos >= 0 && Nneg >= 0 && Nzero >= 0 "Algbra signature must be non-negative."
    CliffordAlgebra(Npos, Nneg, Nzero, ntuple(i -> Symbol(:e, i), Npos + Nneg + Nzero))
end

"""
    CliffordAlgebra(a::Symbol)

Generates a predefined algebra from a identifier. Known algebras are
    - :Hyperbolic or :Hyper
    - :Complex or :ℂ
    - :Dual or :Grassmann
    - :Grassmann2D or :G2
    - :Grassmann3D or :G3
    - :Quaternions or :ℍ
    - :Cl2 and :Cl3
    - :Spacetime
    - :PGA2D or :Projective2D or :Plane2D
    - :PGA3D or :Projective3D or :Plane3D
    - :CGA2D or :Conformal2D
    - :CGA3D or :Conformal3D
    - :DCGA3D or :DoubleConformal3D
    - :TCGA3D or :TripleConformal3D
    - :DCGSTA or :DoubleConformalSpacetime
    - :QCGA or :QuadricConformal
"""
function CliffordAlgebra(a::Symbol)
    if a in (:Hyperbolic, :Hyper)
        return CliffordAlgebra(1, 0, 0, (:j,))
    elseif a in (:Complex, :ℂ)
        return CliffordAlgebra(0, 1, 0, (:i,))
    elseif a in (:Dual, :Grassmann)
        return CliffordAlgebra(0, 0, 1, (:ε,))
    elseif a in (:Grassmann2D, :G2)
        return CliffordAlgebra(0, 0, 2, (:ε₁, :ε₂))
    elseif a in (:Grassmann3D, :G3)
        return CliffordAlgebra(0, 0, 3, (:ε₁, :ε₂, :ε₃))
    elseif a in (:Quaternions, :ℍ)
        return CliffordAlgebra(0, 2, 0, (:i, :j))
    elseif a in (:Cl2,)
        return CliffordAlgebra(2)
    elseif a in (:Cl3,)
        return CliffordAlgebra(3)
    elseif a in (:Spacetime, :STA)
        return CliffordAlgebra(1, 3, 0, (:t, :x, :y, :z))
    elseif a in (:PGA2D, :Projective2D, :Plane2D)
        return CliffordAlgebra(2, 0, 1, (:e1, :e2, :e0))
    elseif a in (:PGA3D, :Projective3D, :Plane3D)
        return CliffordAlgebra(3, 0, 1, (:e1, :e2, :e3, :e0))
    elseif a in (:CGA2D, :Conformal2D)
        return CliffordAlgebra(3, 1, 0, (:e1, :e2, :e₊, :e₋))
    elseif a in (:CGA3D, :Conformal3D)
        return CliffordAlgebra(4, 1, 0, (:e1, :e2, :e3, :e₊, :e₋))
    elseif a in (:DCGA3D, :DoubleConformal3D)
        return CliffordAlgebra(6, 2)
    elseif a in (:TCGA3D, :TripleConformal3D)
        return CliffordAlgebra(9, 3)
    elseif a in (:DCGSTA, :DoubleConformalSpacetime)
        return CliffordAlgebra(4, 8, 0, (:t₁, :t₂, :e₊₁, :e₊₂, :x₁, :x₂, :y₁, :y₂, :z₁, :z₂, :e₋₁, :e₋₂))
    elseif a in (:QCGA, :QuadricConformal)
        return CliffordAlgebra(9, 6)
    else
        throw(ArgumentError("Unknown algebra."))
    end
end

"""
    basesymbols(::CliffordAlgebra)
    basesymbols(::Type{<:CliffordAlgebra})

Returns the 1-vector space basis symbols of the algebra.
"""
basesymbols(::Type{<:CliffordAlgebra{Np,Nn,Nz,S}}) where {Np,Nn,Nz,S} = S
basesymbols(ca::CliffordAlgebra) = basesymbols(typeof(ca))

"""
    basetable(::CliffordAlgebra)
    basetable(::Type{<:CliffordAlgebra})

Returns the internal basis table of the algebra.
"""
basetable(::Type{<:CliffordAlgebra{Np,Nn,Nz,S,BT}}) where {Np,Nn,Nz,S,BT} = BT
basetable(ca::CliffordAlgebra) = basetable(typeof(ca))

"""
    multtable(::CliffordAlgebra)
    multtable(::Type{<:CliffordAlgebra})

Returns the internal multuplication table of the geometric product of the algebra.
"""
multtable(::Type{<:CliffordAlgebra{Np,Nn,Nz,S,BT}}) where {Np,Nn,Nz,S,BT} = multiplicationtable(Np, Nn, Nz, BT)
multtable(ca::CliffordAlgebra) = multtable(typeof(ca))

"""
    baseproduct(::CliffordAlgebra, nleft::Integer, nright::Integer)
    baseproduct(::Type{<:CliffordAlgebra}, nleft::Integer, nright::Integer)

Returns a tuple holding the basis index and the scalar coefficient of the geometric product of the two basis vectors with indices nleft and nright.
"""
baseproduct(::Type{CliffordAlgebra{Np,Nn,Nz,S,BT}}, nleft::Integer, nright::Integer) where {Np,Nn,Nz,S,BT} = basevectorproduct(Np, Nn, BT, nleft, nright)
baseproduct(ca::CliffordAlgebra, nleft::Integer, nright::Integer) = baseproduct(typeof(ca), nleft, nright)

"""
    order(::CliffordAlgebra)
    order(::Type{<:CliffordAlgebra})

Returns the order of the algebra. The order is the sum of the signature.
"""
order(::Type{<:CliffordAlgebra{Np,Nn,Nz}}) where {Np,Nn,Nz} = Np + Nn + Nz
order(ca::CliffordAlgebra) = order(typeof(ca))

"""
    signature(::CliffordAlgebra)
    signature(::Type{<:CliffordAlgebra})

Returns the signature of the algebra.
"""
signature(::Type{<:CliffordAlgebra{Np,Nn,Nz}}) where {Np,Nn,Nz} = (Np, Nn, Nz)
signature(ca::CliffordAlgebra) = signature(typeof(ca))

"""
    basesignature(::CliffordAlgebra, n::Integer)
    basesignature(::Type{<:CliffordAlgebra}, n::Integer)

Returns the signature value of the n-th basis 1-vector. The return value is +1, -1 or 0.
"""
function basesignature(CA::Type{<:CliffordAlgebra{Np,Nn,Nz}}, n::Integer) where {Np,Nn,Nz}
    @assert 1 <= n <= order(CA)
    if n <= Np
        return +1
    elseif n <= Np + Nn
        return -1
    else
        return 0
    end
end

basesignature(ca::CliffordAlgebra, n::Integer) = basesignature(typeof(ca), n)

"""
    dimension(::CliffordAlgebra)
    dimension(::Type{<:CliffordAlgebra})

Returns the dimension of the algebra, i.e. the number of coefficients in a general multivector.
"""
dimension(CA::Type{<:CliffordAlgebra}) = 2^order(CA)
dimension(ca::CliffordAlgebra) = dimension(typeof(ca))

"""
    character(::CliffordAlgebra)
    character(::Type{<:CliffordAlgebra})

Returns the square of the pseudoscalar of the algebra.
"""
character(CA::Type{<:CliffordAlgebra}) = baseproduct(CA, dimension(CA), dimension(CA))[2]
character(ca::CliffordAlgebra) = character(typeof(ca))

"""
    basegrade(::CliffordAlgebra, n::Integer)
    basegrade(::Type{<:CliffordAlgebra}, n::Integer)

Returns the grade of the n-th basis multivector of the algebra.
"""
basegrade(CA::Type{<:CliffordAlgebra}, n::Integer) = length(basetable(CA)[n])
basegrade(ca::CliffordAlgebra, n::Integer) = basegrade(typeof(ca), n)

"""
    basereverse(::CliffordAlgebra, n::Integer)
    basereverse(::Type{<:CliffordAlgebra}, n::Integer)

Returns the sign change of the n-th basis multivector under reversal.
"""
basereverse(CA::Type{<:CliffordAlgebra}, n::Integer) = (-1)^(basegrade(CA, n) ÷ 2)
basereverse(ca::CliffordAlgebra, n::Integer) = basereverse(typeof(ca), n)

"""
    basesymbol(::CliffordAlgebra, n::Integer)
    basesymbol(::Type{<:CliffordAlgebra}, n::Integer)

Returns the symbol used for the n-th basis multivector of the algebra.
"""
function basesymbol(CA::Type{<:CliffordAlgebra}, n::Integer)
    @assert 1 <= n <= dimension(CA)
    s = basetable(CA)[n]
    if isempty(s)
        Symbol(:𝟏)
    else
        Symbol(map(i -> basesymbols(CA)[i], s)...)
    end
end

basesymbol(ca::CliffordAlgebra, n::Integer) = basesymbol(typeof(ca), n)

"""
    caleytable(io::IO, ca::CliffordAlgebra)
    caleytable(io::IO, CA::Type{<:CliffordAlgebra})

Generates a Cayley table view of the algebra.
"""
cayleytable(io::IO, ca::CliffordAlgebra) = cayleytable(io, typeof(ca))

function cayleytable(io::IO, CA::Type{<:CliffordAlgebra})
    dim = dimension(CA)
    bs = map(s -> s == Symbol() ? Symbol("1") : s, ntuple(k -> basesymbol(CA, k), dim))
    mt = multtable(CA)
    table = Matrix{String}(undef, dim, dim)
    for col = 1:dim
        for row = 1:dim
            baseidx, coeff = mt[row][col]
            @assert coeff in (-1, 0, 1)
            if baseidx == 1
                table[row, col] = coeff == -1 ? "-1" : coeff == 1 ? "+1" : "0"
            else
                if iszero(coeff)
                    table[row, col] = "0"
                else
                    table[row, col] = string(coeff > 0 ? :+ : :-, bs[baseidx])
                end
            end
        end
    end
    lines = vcat(0, cumsum([binomial(order(CA), i) for i = 0:order(CA)]))
    pretty_table(
        io,
        table;
        show_row_number = false,
        crop = :none,
        noheader = true,
        alignment = :c,
        vlines = lines,
        hlines = lines,
    )
end

"""
    signaturetable(io::IO, ca::CliffordAlgebra)
    signaturetable(io::IO, CA::Type{<:CliffordAlgebra})

Prints the 1-vector basis symbols and their squares.
"""
signaturetable(io::IO, ca::CliffordAlgebra) = signaturetable(io, typeof(ca))

function signaturetable(io::IO, CA::Type{<:CliffordAlgebra})
    bs = ntuple( k -> basesymbol(CA, k+1), order(CA))
    table = Matrix{String}(undef, order(CA), 2)
    for (n,s) = enumerate(bs)
        sig = basesignature(CA,n)
        table[n,1] = string(s)
        table[n,2] = sig == +1 ? "+1" : sig == -1 ? "-1" : "0"
    end
    pretty_table(
        io,
        table;
        show_row_number = false,
        crop = :none,
        noheader = true,
        alignment = :c
    )
end

function show(io::IO, ca::CliffordAlgebra)
    (Np, Nn, Nz) = signature(ca)
    print(io, "Cl(", Np, ",", Nn, ",", Nz, ")")
end


function derivative(A::LinearInterpolation{<:AbstractVector}, t::Number)
  idx = searchsortedfirst(A.t, t)
  if A.t[idx] >= t
    idx -= 1
  end
  idx == 0 ? idx += 1 : nothing
  θ = 1 / (A.t[idx+1] - A.t[idx])
  (A.u[idx+1] - A.u[idx]) / (A.t[idx+1] - A.t[idx])
end

function derivative(A::LinearInterpolation{<:AbstractMatrix}, t::Number)
  idx = searchsortedfirst(A.t, t)
  if A.t[idx] >= t
    idx -= 1
  end
  idx == 0 ? idx += 1 : nothing
  θ = 1 / (A.t[idx+1] - A.t[idx])
  @views @. (A.u[:, idx+1] - A.u[:, idx]) / (A.t[idx+1] - A.t[idx])
end

function derivative(A::QuadraticInterpolation{<:AbstractVector}, t::Number)
  idx = searchsortedfirst(A.t, t)
  if A.t[idx] >= t
    idx -= 1
  end
  idx == 0 ? idx += 1 : nothing
  if idx == length(A.t) - 1
    i₀ = idx - 1; i₁ = idx; i₂ = i₁ + 1;
  else
    i₀ = idx; i₁ = i₀ + 1; i₂ = i₁ + 1;
  end
  dl₀ = (2t - A.t[i₁] - A.t[i₂]) / ((A.t[i₀] - A.t[i₁]) * (A.t[i₀] - A.t[i₂]))
  dl₁ = (2t - A.t[i₀] - A.t[i₂]) / ((A.t[i₁] - A.t[i₀]) * (A.t[i₁] - A.t[i₂]))
  dl₂ = (2t - A.t[i₀] - A.t[i₁]) / ((A.t[i₂] - A.t[i₀]) * (A.t[i₂] - A.t[i₁]))
  A.u[i₀] * dl₀ + A.u[i₁] * dl₁ + A.u[i₂] * dl₂
end

function derivative(A::QuadraticInterpolation{<:AbstractMatrix}, t::Number)
  idx = searchsortedfirst(A.t, t)
  if A.t[idx] >= t
      idx -= 1
  end
  idx == 0 ? idx += 1 : nothing
  if idx == length(A.t) - 1
    i₀ = idx - 1; i₁ = idx; i₂ = i₁ + 1;
  else
    i₀ = idx; i₁ = i₀ + 1; i₂ = i₁ + 1;
  end
  dl₀ = (2t - A.t[i₁] - A.t[i₂]) / ((A.t[i₀] - A.t[i₁]) * (A.t[i₀] - A.t[i₂]))
  dl₁ = (2t - A.t[i₀] - A.t[i₂]) / ((A.t[i₁] - A.t[i₀]) * (A.t[i₁] - A.t[i₂]))
  dl₂ = (2t - A.t[i₀] - A.t[i₁]) / ((A.t[i₂] - A.t[i₀]) * (A.t[i₂] - A.t[i₁]))
  @views @. A.u[:, i₀] * dl₀ + A.u[:, i₁] * dl₁ + A.u[:, i₂] * dl₂
end

function derivative(A::LagrangeInterpolation{<:AbstractVector}, t::Number)
  idxs = findRequiredIdxs(A, t)
  if A.t[idxs[1]] == t
    return zero(A.u[idxs[1]])
  end
  G = zero(A.u[1]); F = zero(A.t[1])
  DG = zero(A.u[1]); DF = zero(A.t[1])
  tmp = G
  for i = 1:length(idxs)
    if isnan(A.bcache[idxs[i]])
      mult = one(A.t[1])
      for j = 1:(i - 1)
        mult *= (A.t[idxs[i]] - A.t[idxs[j]])
      end
      for j = (i+1):length(idxs)
        mult *= (A.t[idxs[i]] - A.t[idxs[j]])
      end
      A.bcache[idxs[i]] = mult
    else
      mult = A.bcache[idxs[i]]
    end
    wi = inv(mult)
    tti = t - A.t[idxs[i]]
    tmp = wi / (t - A.t[idxs[i]])
    g = tmp * A.u[idxs[i]]
    G += g
    DG -= g / (t - A.t[idxs[i]])
    F += tmp
    DF -= tmp / (t - A.t[idxs[i]])
  end
  (DG * F - G * DF) / (F ^ 2)
end

function derivative(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number)
  idxs = findRequiredIdxs(A, t)
  if A.t[idxs[1]] == t
    return zero(A.u[:, idxs[1]])
  end
  G = zero(A.u[:, 1]); F = zero(A.t[1])
  DG = zero(A.u[:, 1]); DF = zero(A.t[1])
  tmp = G
  for i = 1:length(idxs)
    if isnan(A.bcache[idxs[i]])
      mult = one(A.t[1])
      for j = 1:(i - 1)
        mult *= (A.t[idxs[i]] - A.t[idxs[j]])
      end
      for j = (i+1):length(idxs)
        mult *= (A.t[idxs[i]] - A.t[idxs[j]])
      end
      A.bcache[idxs[i]] = mult
    else
      mult = A.bcache[idxs[i]]
    end
    wi = inv(mult)
    tti = t - A.t[idxs[i]]
    tmp = wi / (t - A.t[idxs[i]])
    g = tmp * A.u[:, idxs[i]]
    @. G += g
    @. DG -= g / (t - A.t[idxs[i]])
    F += tmp
    DF -= tmp / (t - A.t[idxs[i]])
  end
  @. (DG * F - G * DF) / (F ^ 2)
end

function derivative(A::AkimaInterpolation{<:AbstractVector}, t::Number)
  i = searchsortedlast(A.t, t)
  i == 0 && return zero(A.u[1])
  i == length(A.t) && return zero(A.u[end])
  wj = t - A.t[i]
  @evalpoly wj A.b[i] 2A.c[i] 3A.d[i]
end

function derivative(A::ConstantInterpolation{<:AbstractVector}, t::Number)
  return isempty(searchsorted(A.t, t)) ? zero(A.u[1]) : eltype(A.u)(NaN)
end

function derivative(A::ConstantInterpolation{<:AbstractMatrix}, t::Number)
  return isempty(searchsorted(A.t, t)) ? zero(A.u[:, 1]) : eltype(A.u)(NaN) .* A.u[:, 1]
end

# QuadraticSpline Interpolation
function derivative(A::QuadraticSpline{<:AbstractVector{<:Number}}, t::Number)
  i = searchsortedfirst(A.t, t)
  i == 1 ? i += 1 : nothing
  σ = 1//2 * (A.z[i] - A.z[i - 1]) / (A.t[i] - A.t[i - 1])
  A.z[i-1] + 2σ * (t - A.t[i-1])
end

# CubicSpline Interpolation
function derivative(A::CubicSpline{<:AbstractVector{<:Number}}, t::Number)
  i = searchsortedfirst(A.t, t)
  isnothing(i) ? i = length(A.t) - 1 : i -= 1
  i == 0 ? i += 1 : nothing
  dI = -3A.z[i] * (A.t[i + 1] - t)^2 / (6A.h[i + 1]) + 3A.z[i + 1] * (t - A.t[i])^2 / (6A.h[i + 1])
  dC = A.u[i + 1] / A.h[i + 1] - A.z[i + 1] * A.h[i + 1] / 6
  dD = -(A.u[i] / A.h[i + 1] - A.z[i] * A.h[i + 1] / 6)
  dI + dC + dD
end

function derivative(A::BSplineInterpolation{<:AbstractVector{<:Number}}, t::Number)
  # change t into param [0 1]
  idx = searchsortedlast(A.t,t)
  idx == length(A.t) ? idx -= 1 : nothing
  n = length(A.t)
  scale = (A.p[idx+1] - A.p[idx]) / (A.t[idx+1] - A.t[idx])
  t_ = A.p[idx] + (t - A.t[idx]) * scale
  N = DataInterpolations.spline_coefficients(n, A.d-1, A.k, t_)
  ducum = zero(eltype(A.u))
  for i = 1:(n - 1)
    ducum += N[i + 1] * (A.c[i + 1] - A.c[i]) / (A.k[i + A.d + 1] - A.k[i + 1])
  end
  ducum * A.d * scale
end

# BSpline Curve Approx
function derivative(A::BSplineApprox{<:AbstractVector{<:Number}}, t::Number)
  # change t into param [0 1]
  idx = searchsortedlast(A.t,t)
  idx == 0 ? idx += 1 : nothing
  scale = (A.p[idx+1] - A.p[idx]) / (A.t[idx+1] - A.t[idx])
  t_ = A.p[idx] + (t - A.t[idx]) * scale
  N = spline_coefficients(A.h, A.d-1, A.k, t_)
  ducum = zero(eltype(A.u))
  for i = 1:(A.h - 1)
    ducum += N[i + 1] * (A.c[i + 1] - A.c[i]) / (A.k[i + A.d + 1] - A.k[i + 1])
  end
  ducum * A.d * scale
end


using CSV
using LinearAlgebra: dot, I
using CairoMakie

const Σ = [1.0  0.1  0.1;
           0.1  1.0  0.1;
           0.1  0.1  1.0]

const v = [1.0/sqrt(3), 1.0/sqrt(3), 1.0/sqrt(3)]
const σ² = 1

const Σ2 = -0.95 * v * v' + I(3)

f(x) = abs2(dot(v, Σ * x)) / (dot(x, Σ * x) + σ² * dot(x, x))

g(x) = abs2(dot(v, Σ2 * x)) / (dot(x, Σ2 * x) + σ² * dot(x, x))

# Helper function to compute extra coordinate
function getZ(x,y)
    if abs2(x) + abs2(y) > 1 - 10*eps()
        return NaN
    end
    return sqrt(1 - abs2(x) - abs2(y))
end

function rewardPositive(x,y)
    return f([x,y,getZ(x,y)])
end

function rewardNegative(x,y)
    return f([x,y,-getZ(x,y)])
end

function rewardPositive2(x,y)
    return g([x,y,getZ(x,y)])
end

function rewardNegative2(x,y)
    return g([x,y,-getZ(x,y)])
end

# Sample Circle
x = -1.1:0.01:1.1
y = -1.1:0.01:1.1

noto_sans = "./resources/NotoSans-Regular.ttf"
noto_sans_bold = "./resources/NotoSans-Bold.ttf"

tickfontsize    = 26
labelfontsize   = 28
basewidth = 2

fig = Figure(font=noto_sans, resolution=(1500,525), figure_padding=40)

ax1 = Axis( fig[1,1],
            xticklabelsize=tickfontsize,
            yticklabelsize=tickfontsize,
            yticklabelpad=2,
            xlabel="x", ylabel = "y",
            xlabelsize=labelfontsize,
            ylabelsize=labelfontsize,
            titlesize=labelfontsize,
            ratio=AxisAspect(1))

hm = heatmap!(ax1,x,y,rewardPositive)

ax2 = Axis( fig[1,2],
            xticklabelsize=tickfontsize,
            yticklabelsize=tickfontsize,
            yticklabelpad=2,
            xlabel="x", ylabel = "y",
            xlabelsize=labelfontsize,
            ylabelsize=labelfontsize,
            titlesize=labelfontsize,
            ratio=AxisAspect(1))

heatmap!(ax2,x,y,rewardNegative)

ax3 = Axis( fig[1,3],
            xticklabelsize=tickfontsize,
            yticklabelsize=tickfontsize,
            yticklabelpad=2,
            xlabel="x", ylabel = "y",
            xlabelsize=labelfontsize,
            ylabelsize=labelfontsize,
            titlesize=labelfontsize,
            ratio=AxisAspect(1))

hm2 = heatmap!(ax3,x,y,rewardPositive2)

ax4 = Axis( fig[1,4],
            xticklabelsize=tickfontsize,
            yticklabelsize=tickfontsize,
            yticklabelpad=2,
            xlabel="x", ylabel = "y",
            xlabelsize=labelfontsize,
            ylabelsize=labelfontsize,
            titlesize=labelfontsize,
            ratio=AxisAspect(1))

heatmap!(ax4,x,y,rewardNegative2)

Colorbar(fig[2, 1:2], hm, vertical = false, flipaxis = false, ticklabelsize=tickfontsize)
Colorbar(fig[2, 3:4], hm2, vertical = false, flipaxis = false, ticklabelsize=tickfontsize)

colgap!(fig.layout,15)
colgap!(fig.layout,2,100)

Label(fig[1,1,TopLeft()], "A", font=noto_sans_bold, textsize = 36, halign=:left, valign=:bottom, padding=(25,0,10,0))
Label(fig[1,3,TopLeft()], "B", font=noto_sans_bold, textsize = 36, halign=:left, valign=:bottom, padding=(25,0,10,0))

save("out/OptSurface.pdf",fig)


using Gridap

# Geometry
domain = (0,1,0,1); cells = (10,10)
model = CartesianDiscreteModel(domain,cells)

# Integration
degree = 2
Ω = Triangulation(model)
Γ = BoundaryTriangulation(model)
dΩ = LebesgueMeasure(Ω,degree)
dΓ = LebesgueMeasure(Γ,degree)

# Manufactured solution
u(x) = x[1]+x[2]

# Parameter-dependent weak form
a(p,u,v) = ∫( (p*p)*∇(v)⋅∇(u) )*dΩ
l(v) = ∫( -v*Δ(u) )*dΩ

# Objective function in terms of primal solution
# (squared distance wrt exact solution u on Ω and Γ
# as an example involving different domains)
# Note: the following function returns cell contributions.
# The final objective is computed as sum(j(v))
j(v) = ∫( abs2(u-v) )*dΩ + ∫( abs2(u-v) )*dΓ

# FE spaces: P for params, V, U for primal solution
P = FESpace(model,ReferenceFE(:Lagrangian,Float64,0),conformity=:L2)
V = TestFESpace(model,ReferenceFE(:Lagrangian,Float64,1),dirichlet_tags="boundary")
U = TrialFESpace(V,u)

using Gridap.FESpaces

# Objective function (scalar), plus its gradient (vector) in terms of p
# fp, ∇fp = f_∇f(p)
function f_∇f(p)
  # Direct solve
  a_at_p(u,v) = a(p,u,v)
  op = AffineFEOperator(a_at_p,l,U,V)
  u = solve(op)
  # Adjoint solve
  A = get_matrix(op)
  dj_du = ∇(j)(u) # autodiff
  b = assemble_vector(dj_du,V)
  λ = FEFunction(V,A\b) # assuming A self-adjoint
  # we want to have something like this
  # op_t = AdjointFEOperator(op, ∇(j), selfadjoint=true )
  # u, lambda = solve(op,op_t)
  # for the moment, we can do more low level things at the driver level, e.g., extracting matrices, etc for the adjoint
  # in any case, it is not needed for self-adjoint problems
  # Objective
  fp = sum(j(u))
  # Its gradient
  ∂a_∂p = ∇(x -> a(x,u,λ))(p) # Autodiff
  ∇fp = -1*assemble_vector(∂a_∂p,P)
  fp, ∇fp
end

# run for a given realization of p
p = FEFunction(P,rand(num_free_dofs(P)).+1)
fp, ∇fp = f_∇f(p)
# Print results
@show fp
writevtk(Ω,"trian",cellfields=["∇fp"=>FEFunction(P,∇fp)])

function df_dp(uvec,pvec,λvec)
  da(p,u,v,dp) = (2*p*dp)*∇(v)⋅∇(u)
  uh = FEFunction(U,uvec)
  ph = FEFunction(P,pvec)
  λh = FEFunction(V,λvec)
  t = FESource((dp)->da(ph,uh,λh,dp),trian,quad)
  op = AffineFEOperator(P,Q,t)
  get_vector(op)
end


module LCA
export lca

#Make all columns of the input matrix have a norm of 1
# D - input matrix (modified in place)
function unitary_columns!(D::Matrix{Float64})
    for i = 1:size(D,2)
        D[:,i] = D[:,i]/norm(D[:,i]);
    end
    nothing
end

#Perform the positive/negative soft threshold function on each element
# a - result vector (modified in place)
# u - input vector
# T - threshold
function soft_threshold!(a::Vector{Float64}, u::Vector{Float64}, T::Float64)
    for i = 1:length(u)
        if abs(u[i]) > T
            a[i,1] = u[i]-sign(u[i])*T;
        else
            a[i,1] = 0;
        end
    end
    nothing
end

#Perform Locally Competitive Algorithm for sparse recovery
# s      - Input to be approximated
# D      - Dictionary
# T_soft - Soft threshold value (higher promotes more sparseness)
# iter   - maximum number of iterations
# τ      - 1/learning rate
function lca(s::Vector{Float64}, D::Matrix{Float64}; T_soft::Float64=0.1,
             iter::Int=10000, τ::Float64=100.0)
    #Impose Unitary norm
    unitary_columns!(D)

    # Inhibition Matrix
    G = D'*D;
    for i=1:size(G,1)
        G[i,i] = 0;
    end

    # Initialize LCA Parameters
    b = D'*s; # initial projection / excitatory input

    u = zeros(size(b)); # initial state of nodes = 0
    a = zeros(size(b)); # initial sparse rep = 0

    for i=1:iter
        # soft thresholding function
        soft_threshold!(a, u, T_soft)

        # node dynamics
        Δu = (b  - u - G*a)/τ;
        u += Δu;

        if(norm(Δu) < 1e-5)
            break
        end
    end

    return a
end

end # module


#=
P35 (**) Determine the prime factors of a given positive integer.
Construct a flat list containing the prime factors in ascending order.
Example:
* (prime-factors 315)
(3 3 5 7)
=#

SMALLEST_PRIME = 2

function next_prime(n)
    # Brute-force incremental check
    isprime(n) ? n : next_prime(n + 1)
end

function _prime_factors(n, x, pf)
    if isprime(n)
        vcat(pf, n)
    else
        if mod(n, x) == 0
            _prime_factors(div(n, x), SMALLEST_PRIME, vcat(pf, x))
        else
            _prime_factors(n, next_prime(x + 1), pf)
        end
    end
end

function prime_factors(n)
    # Starting with the smallest prime, 2
    isprime(n) ? n : _prime_factors(n, SMALLEST_PRIME, [])
end

@assert prime_factors(315) == [3, 3, 5, 7]
@assert prime_factors(360) == [2, 2, 2, 3, 3, 5]
@assert prime_factors(144) == [2, 2, 2, 2, 3, 3]

println("Tests passed: JL-35.jl")


# Simple example of a Neumann problem:
#
#           y
#             |
#             |     u_y = cos(omega x)
#          Ly ----------------------------------
#             |                                |
#             |                                |
#             |                                |
#     u_x = 0 |    -u_xx - u_yy + u = 0        | u_x = 0
#             |                                |
#             |                                |
#             |                                |
#           -----------------------------------|-----  x
#                         u_y = 0              Lx
#
# Here, omega = 2 n pi / Lx, and the exact solution is
#
#      u = cos(omega x) * cosh(alpha y) / (alpha sinh(alpha Ly)
#
# where alpha^2 = 1 + omega^2.

using FinElt
using FinElt.PlanarPoisson
using Printf

include("params.jl")

const omega = 2 * pi / Lx
const alpha = sqrt(1.0 + omega^2)

function exact_u(x)
    return cos(omega*x[1]) * cosh(alpha*x[2]) / (alpha*sinh(alpha*Ly))
end

function gN(x)
    return cos(omega*x[1])
end

maxerr = zeros(refinements+1)
@printf("%10s  %12s  %8s  %8s\n\n",
        "N", "max error", "rate", "seconds")
for k = 0:refinements
    start = time()
    mesh = read_msh_file("rect$k.msh")
    vp = VariationalProblem(mesh)
    add_bilin_form!(vp, "Omega", grad_dot_grad!)
    add_bilin_form!(vp, "Omega", func_times_func!)
    add_lin_functnl!(vp, "Top", bdry_source_times_func!, gN)
    A, b = assembled_linear_system(vp)
    ufree = A \ b
    uh = complete_soln(ufree, vp)
    u = get_nodal_vals(exact_u, mesh)
    finish = time()
    maxerr[k+1] = maximum(abs.(uh-u))
    N = length(ufree)
    if k == 0
        @printf("%10d  %12.4e\n", N, maxerr[k+1])
    else
        rate = log2(maxerr[k]/maxerr[k+1])
        elapsed = finish - start
        @printf("%10d  %12.4e  %8.4f  %8.4f\n",
                N, maxerr[k+1], rate, elapsed)
    end
end


module SfGravity

using PhysicalConstants.CODATA2018: c_0, g_n, G, StefanBoltzmannConstant, ħ, k_B
using Unitful, UnitfulAstro, Documenter

export vis_viva, gravity, planetary_mass, planetary_radius, escape_velocity, hill_sphere,
	orbital_velocity, orbital_period, gravitational_binding_energy, roche_limit, gravity_tug_mass,
	barycentric_distance, tidal_acceleration,
	radial_orbit_time, radial_orbit_displacement

DocMeta.setdocmeta!(SfGravity, :DocTestSetup, :(using Unitful, UnitfulAstro, ..SfGravity); recursive=true)

"""
    vis_viva(parent_mass::Unitful.Mass, semimajor_axis::Unitful.Length, current_radius::Unitful.Length)

Return the square of the relative velocities of a body with negligible mass and orbit with `semimajor_axis` and `current_radius` from a central body of `parent_mass`.

``v^2 = GM\\left( \\frac{2}{r} - \\frac{1}{a} \\right)``

where ``G`` is the gravitational constant, ``M`` is the mass of the central body, ``r`` is the current separation of the bodies and ``a`` is the semi-major axis of the orbit.
"""
function vis_viva(parent_mass::Unitful.Mass, semimajor_axis::Unitful.Length, current_radius::Unitful.Length)
	G*parent_mass*(2/current_radius - 1/semimajor_axis) |> u"m^2/s^2"
end

"""
	gravity(m::Unitful.Mass, r::Unitful.Length)

Calculate acceleration due to gravity at distance `r` from point mass `m`

# Example
```jldoctest
julia> gravity(1u"Mearth", 1u"Rearth")
9.798398133669465 m s^-2
```"""
gravity(m::Unitful.Mass, r::Unitful.Length) = G * m / r^2 |> u"m/s/s"

"""
	gravity(m1::Unitful.Mass, m2::Unitful.Mass, r::Unitful.Length)

Calculate the strength of gravity at a distance `r` between two masses, `m1` and `m2`.

# Example
```jldoctest
julia> gravity(1u"Mearth", 100u"kg", 1u"Rearth")
979.8398133669466 N
```"""
gravity(m1::Unitful.Mass, m2::Unitful.Mass, r::Unitful.Length) = G * m1 * m2 / r^2 |> u"N"

"""
	planetary_mass(g::Unitful.Acceleration, r::Unitful.Length)

Calculate the mass of a body required to produce gravitational acceleration `g` at a distance of `r`.
"""
planetary_mass(g::Unitful.Acceleration, r::Unitful.Length) = (g * r^2) / G |> u"kg"

"""
	planetary_mass(ve::Unitful.Velocity, r::Unitful.Length)

Calculate the mass of a body required to produce escape velocity `ve` at a distance of `r`.
"""
planetary_mass(ve::Unitful.Velocity, r::Unitful.Length) = r * ve^2 / 2G |> u"kg"

"""
	planetary_radius(m::Unitful.Mass, g::Unitful.Acceleration)

Radius of a spherical planet of mass `m` with surface gravitational acceleration of `g`.
"""
planetary_radius(m::Unitful.Mass, g::Unitful.Acceleration) = sqrt((G * m) / g) |> u"m"

"""
	planetary_radius(m::Unitful.Mass, g::Unitful.Acceleration)

Radius of a spherical planet of mass `m` with surface escape velocity `ve`.
"""
planetary_radius(m::Unitful.Mass, ve::Unitful.Velocity) = 2G * m / ve^2 |> u"m"

"""
	planetary_radius(ρ::Unitful.Density, g::Unitful.Acceleration)

Radius of a spherical planet of density `ρ` with surface gravitational acceleration of `g`.
"""
planetary_radius(ρ::Unitful.Density, g::Unitful.Acceleration) = 3g / (4G * π * ρ) |> u"m"

"""
	planetary_radius(ρ::Unitful.Density, ve::Unitful.Velocity)

Radius of a spherical planet of density `ρ` with surface escape velocity `ve`.
"""
planetary_radius(ρ::Unitful.Density, ve::Unitful.Velocity) = sqrt(3ve^2 / (8G * π * ρ)) |> u"m"

# kepler's third

"""
	orbital_period(m::Unitful.Mass, r::Unitful.Length)

Calculate the period of an orbit with semimajor axis `r` about a body with mass `m`.

``P = \\sqrt{\\frac{4π^2r^3}{Gm}}``
"""
orbital_period(m::Unitful.Mass, r::Unitful.Length) = sqrt((4π^2 * r^3) / (G * m)) |> u"s"

"""
	orbital_radius(m::Unitful.Mass, t::Unitful.Time)

Calculate the semimajor axis of an orbit with period `t` about a body with mass `m`.

``a = \\sqrt[3]{\\frac{t^2Gm}{4π^2}}``
"""
orbital_radius(m::Unitful.Mass, t::Unitful.Time) = cbrt((t^2 * G * m)/(4π^2)) |> u"m"

"""
    orbital_radius(m::Unitful.Mass, v::Unitful.Velocity)

Approximate the radius of a circular orbit with velocity `v` about a body with mass `m`.
"""
orbital_radius(m::Unitful.Mass, v::Unitful.Velocity) = (G * m) / v^2 |> u"m"

"""
	orbital_velocity(sma::Unitful.Length, t::Unitful.Time)

Calculate the average orbital velocity of a circular orbit of period `t`.

The orbit is assumed to be circular.
"""
orbital_velocity(sma::Unitful.Length, t::Unitful.Time) = 2π * sma / t |> u"km/s"

"""
    orbital_velocity(sma::Unitful.Length, t::Unitful.Time, e)

Approximate the average orbital velocity of a orbit with eccentricity `e` and period `t`.

``s_o = \\frac{2πa}{t} \\left( 1 - \\frac{e^2}{4} - \\frac{3e^4}{64} - \\frac{5e^6}{256} - \\frac{175e^8}{16384} \\right)``
"""
function orbital_velocity(sma::Unitful.Length, t::Unitful.Time, e)
	(2π * sma / t) * (1 - e^2 / 4 - 3e^4 / 64 - 5e^6 / 256 - 175e^8 / 16384) |> u"km/s"
end

"""
    orbital_velocity(parent_mass::Unitful.Mass, semimajor_axis::Unitful.Length, current_radius::Unitful.Length)

Approximate the orbital velocity of a body with an eccentric orbit with `semimajor_axis` and current orbital radius `current_radius` about a body of mass `parent_mass`
"""
orbital_velocity(parent_mass::Unitful.Mass, semimajor_axis::Unitful.Length, current_radius::Unitful.Length) = sqrt(vis_viva(parent_mass, sem, current_radius)) |> u"km/s"

"""
    orbital_velocity(parent_mass::Unitful.Mass, radius::Unitful.Length)

Approximate the orbital velocity of a body with a circular orbit of radius `radius` about a body with mass `parent_mass`.
"""
orbital_velocity(parent_mass::Unitful.Mass, radius::Unitful.Length) = sqrt(G * parent_mass / radius) |> u"km/s"

"""
	planetary_mass(orbital_radius::Unitful.Length, orbital_period::Unitful.Time)

Calculate the mass of a body orbited by a satellite with semimajor axis `sma` and period `orbital_period`.

``M_p = \\frac{4π^2a^3}{GT^2}``

where ``a`` is the semi-major axis and ``T`` is the orbital period.
"""
planetary_mass(sma::Unitful.Length, orbital_period::Unitful.Time) = (4π^2 * sma^3) / (G * orbital_period^2) |> u"kg"

"""
	escape_velocity(m::Unitful.Mass, r::Unitful.Length)

Calculate the velocity required to escape a body with mass `m` from distance `r`.
"""
escape_velocity(m::Unitful.Mass, r::Unitful.Length) = sqrt(2G * m / r) |> u"km/s"

"""
	hill_sphere(m_parent::Unitful.Mass, m::Unitful.Mass, sma::Unitful.Length, e = 0)

Calculate the approximate Hill sphere radius at periapse of a body of mass `m` that orbits a body of mass `m_parent` at a distance of `sma` with orbital eccentricity `e`.

``h_r = a(1-e)\\sqrt[3]{\\frac{m}{3M}}``

where ``a`` is the semi-major axis, `m` is the mass of the smaller body and ``M`` is the mass of the central body.
"""
function hill_sphere(m_parent::Unitful.Mass, m::Unitful.Mass, sma::Unitful.Length, e = 0)
	sma * (1-e) * cbrt(m / 3m_parent) |> Unitful.unit(sma)
end

"""
	gravitational_binding_energy(m::Unitful.Mass, r::Unitful.Length)

Approximate the gravitational binding energy of a body with mass `m` and radius `r`.

``E_g = \\frac{3Gm^2}{5r}``
"""
gravitational_binding_energy(m::Unitful.Mass, r::Unitful.Length) = (3G*m^2) / 5r |>u"J"

"""
    roche_limit(r_primary::Unitful.Length, ρ_primary::Unitful.Density, ρ_satellite::Unitful.Density)

Rigid body approximation of the Roche limit for a body with radius `r_primary` and density `ρ_primary`, approached by a body with density `ρ_satellite`.

``r_r = r\\sqrt[2]{\\frac{2ρ_p}{ρ_s}}``

where ``r`` and ``ρ_p`` are the radius and density of the central body, and ρ_s is the density of the orbiting body.
"""
roche_limit(r_primary::Unitful.Length, ρ_primary::Unitful.Density, ρ_satellite::Unitful.Density) = r_primary * cbrt(2ρ_primary / ρ_satellite) |> u"km"

"""
    gravity_tug_mass(force::Unitful.Force, separation::Unitful.Length, body_mass::Unitful.Mass)

Compute the mass required to apply `force` to a body of mass `body_mass` given their barycenters are at a distance of `separated`.
"""
gravity_tug_mass(force::Unitful.Force, separation::Unitful.Length, body_mass::Unitful.Mass) = (force * separation^2)/(G * body_mass) |> u"kg"

"""
    barycentric_distance(m_1::Unitful.Mass, m_2::Unitful.Mass, a::Unitful.Length)

Compute the distance from the barycenter of `m_1` to the barycenter of the `m_1`-`m_2` system, where the masses have an average separation of `a`.
"""
barycentric_distance(m1::Unitful.Mass, m2::Unitful.Mass, a::Unitful.Length) = (a * m2) / (m1 + m2) |> u"km"

"""
    tidal_acceleration(body_radius::Unitful.Length, parent_mass::Unitful.Mass, separation::Unitful.Length)

Approximate tidal acceleration felt by a body of radius `body_radius` at a distance of `separation` from a body of mass `parent_mass`.

Changes in tidal acceleration are associated with effects like heating, etc.

``a_t = \frac{2GMr}{s^3}``

where ``M`` is the mass of the central body, ``r`` is the radius of the body being affected and ``s`` is the distance between the two bodies.
"""
tidal_acceleration(body_radius::Unitful.Length, parent_mass::Unitful.Mass, separation::Unitful.Length) = 2body_radius * G * parent_mass / separation^3

function radial_orbit_time(m1::Unitful.Mass, m2::Unitful.Mass, x0::Unitful.Length, v0::Unitful.Velocity, x::Unitful.Length)
	μ = G * (m1 + m2)
	w = 1/x0 - v0^2/2μ

	return (asin(sqrt(w * x)) - sqrt(w * x * (1 - w * x))) / sqrt(2μ * w^3)
end

function radial_orbit_displacement(m1::Unitful.Mass, m2::Unitful.Mass, x0::Unitful.Length, v0::Unitful.Velocity, t::Unitful.Time)
	μ = G * (m1 + m2)
	w = 1/x0 - v0^2/2μ
	p = cbrt(9μ * t^2 / 2)

	return p - w*p^2 / 5 - 3w^2 * p^3 / 175 - 23w^3 * p^4/7875 - 1894w^4 * p^5 / 3931875 - 3293w^5 * p^6 / 21896875 - 2418092w^6 * p^7/62077640625
end

function radial_orbit_displacement2(m1::Unitful.Mass, m2::Unitful.Mass, x0::Unitful.Length, v0::Unitful.Velocity, t::Unitful.Time)
	μ = G * (m1 + m2)
	w = 1/x0 - v0^2/2μ
	p = cbrt(9μ * t^2 / 2)

	return p - w*p^2 / 5 - 3w^2 * p^3 / 175 - 23w^3 * p^4/7875 - 1894w^4 * p^5 / 3931875 - 3293w^5 * p^6 / 21896875 - 2418092w^6 * p^7/62077640625
end

function radial_orbit_displacement(m1::Real, m2::Real, x0::Real, v0::Real, t::Real)
	μ = ustrip(G) * (m1 + m2)
	w = 1/x0 - v0^2/2μ
	p = cbrt(9μ * t^2 / 2)

	return p - w*p^2 / 5 - 3w^2 * p^3 / 175 - 23w^3 * p^4/7875 - 1894w^4 * p^5 / 3931875 - 3293w^5 * p^6 / 21896875 - 2418092w^6 * p^7/62077640625
end

end


# ## Data

n = 9
k = 6;

# ## Model

using Turing

@model function globe_toss(n, k)
    θ ~ Beta(1, 1)
    k ~ Binomial(n, θ)
    return k, θ
end;

# ## Output

using Random

Random.seed!(1)
chains = sample(globe_toss(n, k), NUTS(0.65), 1000)

# \defaultoutput{}


using LinearAlgebra
include("findlocalmaxima.jl")

function MUSIC(Y,K,m::Int=0,f_steps=1000)
    # Estimation of signal parameters via rotational invariance techniques
    # (ESPRIT) algorithm for line spectral estimation from measurements.
    # Can handle multiple snapshots by constructing a low rank Hankel matrix
    # from the data.
    #
    # Inputs:
    # Y: Data matrix
    # K: number of sinusoids
    # m: Hankel parameter of the Hankel matrix of the data
    # f_steps: number of data points of the MUSIC pseudo spectrum
    #
    # Parameters:
    # N = size(Y,1): number of measurements per snapshot
    # L = size(Y,2): number of snapshots
    #
    # Output:
    # f_hat: estimate of the frequencies of the K sinusoids in [rad]

(N,L) = size(Y)
f_range = range(0, 1, length=f_steps)

if m == 0
m = ceil(Int,N/2)
end

C = zeros(m,m)
for j = 1:L, i = m:N
    C = C + Y[i-m+1:i,j]*Y[i-m+1:i,j]'/(L*N)
end

    evdC = eigen(C)
    p = sortperm(real(evdC.values))[1:m-K]
    Un = evdC.vectors[:,p]

    a(f) = exp.(2*pi*1im*f*(0:m-1))
    spec(f) = 1/norm(Un'*a(f))^2
    Mspec = spec.(f_range)/maximum(spec.(f_range))
    localmaxima = findlocalmaxima(Mspec)
    q = localmaxima[sortperm(Mspec[localmaxima],rev=true)[1:min(K,length(localmaxima))]]
    f_hat = f_range[q]
    return f_hat #, Mspec

end


using DiffEqFlux, OrdinaryDiffEq, Flux, Optim, Plots

# Condiciones iniciales del problema
u0 = Float32[2.0; 0.0]
datasize = 30
tspan = (0.0f0, 1.5f0)
tsteps = range(tspan[1], tspan[2], length = datasize)

# Se define y resuelve el problema con parámetros reales
function trueODEfunc(du, u, p, t)
    true_A = [-0.1 2.0; -2.0 -0.1]
    du .= ((u.^3)'true_A)'
end

prob_trueode = ODEProblem(trueODEfunc, u0, tspan)
ode_data = Array(solve(prob_trueode, Tsit5(), saveat = tsteps))

# Se define una red neuronal de con dos capas densas/lineales.
# a ésta se le agrega la asunción de que el sistema transforma
# x hacia x^3. Esto como conocimiento previo que mejore el tiempo.
dudt2 = FastChain((x, p) -> x.^3,
                  FastDense(2, 50, tanh),
                  FastDense(50, 2))

# Se define el problema junto con su método de resolución y tiempo al cual parar.
prob_neuralode = NeuralODE(dudt2, tspan, Tsit5(), saveat = tsteps)

# Funciones que guardarán la predicción de la red neuronal y su valor de error.
function predict_neuralode(p)
  Array(prob_neuralode(u0, p))
end

function loss_neuralode(p)
    pred = predict_neuralode(p)
    loss = sum(abs2, ode_data .- pred)
    return loss, pred
end

# Utilizado para graficar a tiempo real las predicciones realizadas.
callback = function (p, l, pred; doplot = true)
  display(l)
  # plot current prediction against data
  plt = scatter(tsteps, ode_data[1,:], label = "data")
  scatter!(plt, tsteps, pred[1,:], label = "prediction")
  if doplot
    display(plot(plt))
  end
  return false
end

# Se entrena la red neuronal utilizando la data de la resolución con parámetros reales.
# Ésto se hace en dos pasos, primero con el optimizador ADAM para acercarnos rápidamente
# al mínimo, y una vez cerca utilizamos Limited-memory BFGS para un ajuste más eficiente.
result_neuralode = DiffEqFlux.sciml_train(loss_neuralode, prob_neuralode.p,
                                          ADAM(0.05), cb = callback,
                                          maxiters = 300)

result_neuralode2 = DiffEqFlux.sciml_train(loss_neuralode,
                                           result_neuralode.minimizer,
                                           LBFGS(),
                                           cb = callback,
                                           allow_f_increases = false)


#=
# The BUGS Book
# Example 2.1.2, page 17:
# For
#   Y ~ Binomial(0.5, 8)
# we want to know Pr(Y <= 2)
#

  See ~/jags/bugs_book_2_1_2.jags
       Mean     SD Naive SE Time-series SE
  P2 0.1442 0.3513 0.001434       0.001434
  Y  4.0025 1.4177 0.005788       0.005839

  ~/webppl/bugs_book_2_1_2.wppl

=#

using Turing, StatsPlots, DataFrames
include("jl_utils.jl")

@model function bugs_book_2_1_2()

    y ~ Binomial(8,0.5)
    p2 ~ y <= 2 ? flip(1.0) : flip(0.0);

end

model = bugs_book_2_1_2()

num_chns = 4
# chns = sample(model, Prior(), MCMCThreads(), 10_000, num_chns)

# chns = sample(model, MH(), MCMCThreads(), 40_000, num_chns)

# chns = sample(model, PG(15), MCMCThreads(), 10_000, num_chns)

# chns = sample(model, SMC(1000), MCMCThreads(), 10_000, num_chns)

chns = sample(model, IS(), MCMCThreads(), 10_000, num_chns)

display(chns)
# display(plot(chns))

show_var_dist_pct(chns,:y)
show_var_dist_pct(chns,:p2)


#    Problem : GROUP A
#    *********
#    A simple quartic function.
#
#    Origonal SIF Source: problem 157 (p. 87) in
#    A.R. Buckley,
#   "Test functions for unconstrained minimization",
#    TR 1989CS-3, Mathematics, statistics and computing centre,
#    Dalhousie University, Halifax (CDN), 1989.
#
#    QUARTC.SIF classification OUR2-AN-V-0
#
#    Number of variables is variable
#
# Daniel Henderson, 08/2021

f = x -> begin
    return sum((x[i] - i)^4 for i in 1:lastindex(x))
end

g! = (g, x) -> begin
	for i in 1:lastindex(x)
		g[i] = 4(x[i] - i)^3
	end
    return g
end

fg! = (g, x) -> begin
	fx = 0.0
	for i in 1:lastindex(x)
		fx  += (x[i] - i)^4
		g[i] = 4(x[i] - i)^3
	end
    return fx, g
end

init = (n::Int=5000) -> begin
    return n, 2.0ones(n)
end

TestSet["QUARTC"] = UncProgram("QUARTC",  f, g!, fg!, init)


using JuMP
using GLPKMathProgInterface

model = Model(solver=GLPKSolverMIP())

@variable(model, vials_of_panacea >= 0, Int)
@variable(model, ampules_of_ichor >= 0, Int)
@variable(model, bars_of_gold >= 0, Int)

@objective(model, Max, 3000*vials_of_panacea + 1800*ampules_of_ichor + 2500*bars_of_gold)

@constraint(model, 0.3*vials_of_panacea + 0.2*ampules_of_ichor + 2.0*bars_of_gold <= 25.0)
@constraint(model, 0.025*vials_of_panacea + 0.015*ampules_of_ichor + 0.002*bars_of_gold <= 0.25)

println("The optimization problem to be solved is:")
println(model)

status = solve(model)

println("Objective value: ", getobjectivevalue(model))
println("vials of panacea = ", getvalue(vials_of_panacea))
println("ampules of ichor = ", getvalue(ampules_of_ichor))
println("bars of gold = ", getvalue(bars_of_gold))


function dlyap(A,Q)

	# Solving Discrete LYAPunov equation using Silvester equation
	#
	# Sylvester equation A*X + X*B + C = 0
	# if B = -(A')^(-1), C = Q*(A')^(-1)
	# => A*X - X*(A')^(-1) + Q*(A')^(-1) = 0
	# => A*X*A' - X + Q = 0  -> Discrete Lyapunov equation

	Atinv = Array{Float64,2}(inv(A'))
	B = -Atinv
	C = Q*Atinv
	X = sylvester(A, B, C)

	return X

end


using DiscriminantAnalysis, Gadfly, Colors

### Helper functions ###

function rotationmatrix2D{T<:AbstractFloat}(θ::T)
    T[cos(θ) -sin(θ);
      sin(θ)  cos(θ)]
end

# Generates two normally distributed variables
function boxmuller(n::Integer)
    u1 = rand(n)
    u2 = rand(n)
    Z = Float64[(√(-2log(u1)) .* cos(2π*u2)) (√(-2log(u1)) .* sin(2π*u2))]
end

# Create the decision boundary using Contour.jl
function boundary(model, xrange, yrange, is_quad::Bool = false)
    Z = hcat(vec(Float64[x for x in xrange, y in yrange]),
             vec(Float64[y for x in xrange, y in yrange]))
    δ = DiscriminantAnalysis.discriminants(model, is_quad ? hcat(Z, Z.^2, Z[:,1] .* Z[:,2]) : Z)
    Z = reshape(δ[:,1] - δ[:,2], length(xrange), length(yrange))
    Contour.coordinates(Contour.lines(Contour.contour(xrange,yrange,Z,0.0))[1])
end

limits(X::Matrix) = (minimum(X[:,1]), maximum(X[:,1]), minimum(X[:,2]), maximum(X[:,2]))

### Sample Data ###

n = 250

Z1 = boxmuller(n)
σ1 = [0.5 2.0]
X1 = ((Z1 .* σ1) .- [0.0 4.25]) * rotationmatrix2D(π/4)

Z2 = boxmuller(n)
σ2 = [3.0 1.5]
X2 = ((Z2 .* σ2) .+ [0.0 2.25]) * rotationmatrix2D(π/4)

X = vcat(X1,X2)
y = repeat([1,2], inner=[n])

xmin, xmax, ymin, ymax = limits(X)
aspect = (ymax-ymin)/(xmax-xmin)

m = 250  # Used for interpolating the decision boundary
xrange = linspace(xmin,xmax,m)
yrange = linspace(ymin,ymax,m)

### LDA & QDA Plots ###
for (obj, desc, is_quad) in ((:lda, "Linear Discriminant Analysis", false),
                             (:qda, "Quadratic Discriminant Analyisis", false),
                             (:lda, "Quadratic Linear Discriminant Analysis", true))
    @eval begin
        model = ($obj)($is_quad ? hcat(X, X.^2, X[:,1] .* X[:,2]) : X, y)
        cx, cy = boundary(model, xrange, yrange, $is_quad)

        P = plot(
                x = vec(X[:,1]),
                y = vec(X[:,2]),
                color = map(class -> "Class $class", y),
                Geom.point,
                Scale.color_discrete_manual(colorant"red",colorant"blue"),
                Guide.XLabel("X Variable"),
                Guide.YLabel("Y Variable"),
                Guide.title($desc),
                Guide.colorkey(""),
                Coord.Cartesian(xmin=xmin, ymin=ymin, xmax=xmax, ymax=ymax)
            )
        L = layer(x=cx, y=cy, Geom.line(preserve_order=true),
                  Theme(default_color=colorant"black", line_width=.4mm))
        unshift!(P.layers,L[1])

        draw(PNG(($is_quad ? "q" : "") * $(string(obj)) * ".png", 6inch, (6*aspect)inch), P)
    end
end

### CDA Plot ###

model = cda(X, y)
C = vec(X * model.W)

P = plot(
    x = C,
    color = map(class -> "Class $class", y),
    Geom.histogram(bincount=100),
    Scale.color_discrete_manual(colorant"red",colorant"blue"),
    Guide.XLabel("Canonical Coordinate"),
    Guide.YLabel("Count"),
    Guide.title("Canonical Discriminant Analysis"),
    Guide.colorkey(""),
    Coord.Cartesian(xmin=minimum(C), xmax=maximum(C))
)

draw(PNG("cda.png", 6inch, 4inch), P)


using SpecialFunctions, Nemo, StatsFuns, Memoization
import Nemo.binom, Nemo.gamma

"Session-wide precision"
const prec = 5000 ## Increase for better precision

"Converter from real to arbitrary precision real (Arb)"
const RR = RealField(prec)

"Converter from real to arbitrary precision complex (Acb)"
const CC = ComplexField(prec)

binom(n::Int64, k::Int64, r::Nemo.ArbField) = binom(convert(UInt64, n),convert(UInt64, k), r)

# Making sure the factorial function uses the Nemo version of the gamma and returns an Arb
gamma(x::Int64) = Nemo.gamma(RR(x))

# Useful typed constants
"1 with required arbitrary precision"
const arb_1 = RR(1)
"0 with required arbitrary precision"
const arb_0 = RR(0)

"""
  unsigned_Stirling1(n::Integer, k::Integer)

Computation of unsigned Stirling numbers of the first kind, using Memoization and a recursive formula.

# Examples
```julia-repl
julia> GibbsTypePriors.unsigned_Stirling1(10, 5)
269325
```
"""
@memoize function unsigned_Stirling1(n::Integer, k::Integer)
  # special cases
  if k<0 || n<0
    throw(DomainError())
  end
  if k>n
    return big(0)
  end
  if n==0  # and, by logic, k==0
    return big(1)
  end
  if k==0  # and, by logic, n>0
    return big(0)
  end
  # end of special cases, invoke recursion
  return unsigned_Stirling1(n-1,k-1) + (n-1)*unsigned_Stirling1(n-1,k)
end

@memoize function unsigned_Stirling2(n::Integer,k::Integer)
  # special cases
  if k<0 || n<0
    throw(DomainError())
  end
  if k>n
    return big(0)
  end
  if n==0  # and, by logic, k==0
    return big(1)
  end
  if k==0  # and, by logic, n>0
    return big(0)
  end
  # end of special cases, invoke recursion
  return  (k)*unsigned_Stirling2(n-1,k) + unsigned_Stirling2(n-1,k-1)
end

"""
  has_reasonable_precision(arb_num)

Check whether arb_num has at least roughly the double precision (maybe replace by 64 by 53).

# Examples
```julia-repl
julia> GibbsTypePriors.Cnk_rec(6, 5, 0.5)
0.234375
```
"""
function has_reasonable_precision(arb_num)
    return accuracy_bits(arb_num) ≥ 53
end

import Nemo.risingfac
"""
  risingfac(r, n)

Computes the rising factorial.

# Examples
```julia-repl
julia> GibbsTypePriors.Cnk_rec(6, 5, 0.5)
0.234375
```
"""
function risingfac(r, n)
  if r == 0
    return 0
  else
    return prod(r + i for i in 0:(n-1))
  end
end
function risingfac(r::arb, n)
  if r == 0
    return arb_0
  else
    return prod(r + i for i in 0:(n-1))
  end
end