# Newton-like methods for NEPs

using LinearAlgebra
using Printf
using Random

export newton
export resinv
export augnewton
export quasinewton
export newtonqr
export implicitdet

#############################################################################
"""
    λ,v = newton([eltype],nep::NEP;[errmeasure,][tol,][maxit,][λ,][v,][c,][logger,][armijo_factor=1,][armijo_max])

Applies Newton-Raphsons method on the system of
nonlinear equations with `n+1` unknowns:
```math
M(λ)v=0
```
```math
c^Hv-1=0
```
The vector `c` is the
orthogonalization vector.  If `c=0` the current approximation will be used for the orthogonalization. See [`augnewton`](@ref) for other parameters.

# Example
```julia-repl
julia> using LinearAlgebra
julia> nep=nep_gallery("dep0");
julia> λ,v=newton(nep);
julia> minimum(svdvals(compute_Mder(nep,λ)))
1.9997125567227177e-16
```

# References
* Nichtlineare Behandlung von Eigenwertaufgaben, Z. Angew. Math. Mech. 30 (1950) 281-282.
* A. Ruhe, Algorithms for the nonlinear eigenvalue problem, SIAM J. Numer. Anal. 10 (1973) 674-689

"""
    newton(nep::NEP;params...)=newton(ComplexF64,nep;params...)
    function newton(::Type{T},
                    nep::NEP;
                    errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                    tol::Real=eps(real(T))*100,
                    maxit::Int=10,
                    λ::Number=zero(T),
                    v::Vector=randn(size(nep,1)),
                    c::Vector=v,
                    logger=0,
                    armijo_factor::Real=1,
                    armijo_max::Int=5) where {T<:Number}

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ=T(λ)
        v=Vector{T}(v)
        c=Vector{T}(c)

        err=Inf;
        v[:] = v/dot(c,v);

        for k=1:maxit
            err=estimate_error(errmeasure,λ,v)

            push_iteration_info!(logger,k,err=err,λ=λ,v=v,continues=true);
            if (err< tol)
                push_info!(logger,"")
                return (λ,v)
            end

            # Compute NEP matrix and derivative
            M = compute_Mder(nep,λ)
            Md = compute_Mder(nep,λ,1)

            # Create jacobian
            J = [M Md*v; c' 0];
            F = [M*v; c'*v-1];

            # Compute update
            delta=-J\F;  # Hardcoded backslash

            Δv=Vector{T}(delta[1:size(nep,1)]);
            Δλ=T(delta[size(nep,1)+1]);

            (Δλ,Δv,j,scaling)=armijo_rule(nep,errmeasure,err,
                                          λ,v,Δλ,Δv,real(T(armijo_factor)),armijo_max)
            if (j>0)
                push_info!(logger," Armijo scaling=$scaling")
            else
                push_info!(logger,"")
            end

            # Update eigenvalue and eigvec
            v[:] += Δv
            λ = λ+Δλ
        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

    ############################################################################
"""
    λ,v = resinv([eltype],nep::NEP;[errmeasure,][tol,][maxit,][λ,][v,][c,][logger,][armijo_factor=1,][armijo_max,][linsolvecreator])

Applies residual inverse iteration method for nonlinear eigenvalue problems.
The kwarg `linsolvecreator`
is a function which specifies how the linear system is created.
The function calls `compute_rf` for the computation
of the Rayleigh functional.
See [`augnewton`](@ref) for other parameters.

# Example
The example shows how to specify if the method should run in real
or complex mode (or any other `Number` type).
```julia-repl
julia> nep=nep_gallery("qdep0");
julia> λ,v=resinv(nep,λ=-2,v=ones(size(nep,1)))
julia> typeof(λ)
Complex{Float64}
julia> norm(compute_Mlincomb(nep,λ,v))
6.688224435370382e-12
julia> λ,v=resinv(Float64,nep,λ=-2,v=ones(size(nep,1)))
julia> typeof(λ)
Float64
julia> norm(compute_Mlincomb(nep,λ,v))
5.939894690000396e-12
```

# References
*  A. Neumaier, Residual inverse iteration for the nonlinear eigenvalue problem, SIAM J. Numer. Anal. 22 (1985) 914-923

"""
    resinv(nep::NEP;params...)=resinv(ComplexF64,nep;params...)
    function resinv(::Type{T},
                    nep::NEP;
                    errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                    tol::Real=eps(real(T))*100,
                    maxit::Int=100,
                    λ::Number=zero(T),
                    v::Vector=randn(real(T),size(nep,1)),
                    c::Vector=v,
                    logger=0,
                    inner_solver= @default_compute_rf_inner_solver(nep),
                    linsolvercreator=DefaultLinSolverCreator(),
                    armijo_factor::Real=1,
                    armijo_max::Int=5) where T

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ::T=T(λ)
        v=Vector{T}(v)
        c=Vector{T}(c)
        n=size(v,1);

        push_info!(logger,"Precomputing linsolver")
        local linsolver::LinSolver=create_linsolver(linsolvercreator,nep,λ)

        # If c is zero vector we take eigvec approx as left vector in
        # generalized Rayleigh functional
        use_v_as_rf_vector=false;
        if (norm(c)==0)
            use_v_as_rf_vector=true;
        end

        push_info!(logger,2,
                   "use_v_as_rf_vector=$use_v_as_rf_vector");

        σ::T=λ;
        err=Inf;

        for k=1:maxit
            # Normalize
            v[:] = v/norm(v);

            err=estimate_error(errmeasure,λ,v)

            if (use_v_as_rf_vector)
                c[:]=v;
            end

            push_iteration_info!(logger,k,err=err,λ=λ,v=v,continues=true);

            if (err< tol)
                push_info!(logger,"")
                return (λ,v)
            end

            # Compute eigenvalue update
            λ_vec = compute_rf(T, nep, v, inner_solver, y=c, λ=λ, target=σ)
            local λ1::T = closest_to(λ_vec,  λ)
            Δλ=λ1-λ

            # Compute eigenvector update
            Δv = -lin_solve(linsolver,compute_Mlincomb(nep,λ1,reshape(v,n,1))) #M*v);

            (Δλ,Δv,j,scaling)=armijo_rule(nep,errmeasure,err,
                                          λ,v,Δλ,Δv,real(T(armijo_factor)),armijo_max)
            if (j>0)
                push_info!(logger," Armijo scaling=$scaling")
            else
                push_info!(logger,"")
            end

            # Update the eigenpair
            λ+=Δλ
            v[:] += Δv;

        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

    # New augnewton
"""
    augnewton([eltype], nep::NEP; [errmeasure,][tol,][maxit,][λ,][v,][c,][logger,][linsolvercreator,][armijo_factor,][armijo_max])

Run the augmented Newton method. The method is equivalent to `newton()`
in exact arithmetic,  but works only with operations on vectors of
length `n`.

The following keyword arguments are in common for many NEP-solvers:

* `logger` is either a [`Logger`](@ref) object or an `Int`. If it is an `Int`, a `PrintLogger(logger)` will be instantiated. `logger=0` prints nothing, `logger=1` prints more, etc.

* `errmeasure` determines how error is measured. It is either a function handle or an object of the type `Errmeasure`.  If it is a function handle, it should take `(λ,v)` as input and return a real scalar (the error). See [`Errmeasure`](@ref) and [`ErrmeasureType`](@ref) for further description.

* `tol` is a scalar which determines termination. If `errmeasure` is less than `tol` the eigenpair is marked as converged.

* The scalar `λ` and the vector `v` are starting approximations.

* `maxit` determines the maximum number of iterations. The error `NoConvergenceException` is thrown if this is exceeded.

*  The `linsolvecreator` specifies how the linear system should be solved. See [`LinSolver`](@ref) for further information.

* `armijo_factor` specifies if an Armijo rule should be applied, and its value specifies the scaling factor of the step length (per reduction step). The variable `armijo_max` specifies the maximum number of step length reductions.

# Example
This illustrates the equivalence between `newton` and `augnewton`.
```julia-repl
julia> nep=nep_gallery("dep1")
julia> λ1,v1=newton(nep,maxit=20,v=ones(size(nep,1)),λ=0)
julia> λ2,v2=augnewton(nep,maxit=20,v=ones(size(nep,1)),λ=0)
julia> λ1-λ2
0.0 + 0.0im
```
# References
* Nichtlineare Behandlung von Eigenwertaufgaben, Z. Angew. Math. Mech. 30 (1950) 281-282.
* A. Ruhe, Algorithms for the nonlinear eigenvalue problem, SIAM J. Numer. Anal. 10 (1973) 674-689
"""
    augnewton(nep::NEP;kwargs...)=augnewton(ComplexF64,nep::NEP;kwargs...)
    function augnewton(::Type{T},
                       nep::NEP;
                       errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                       tol::Real=eps(real(T))*100,
                       maxit::Int=30,
                       λ::Number=zero(T),
                       v::Vector=randn(real(T),size(nep,1)),
                       c::Vector=v,
                       logger=0,
                       linsolvercreator=DefaultLinSolverCreator(),
                       armijo_factor::Real=one(real(T)),
                       armijo_max::Int=5) where {T<:Number}

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ=T(λ)
        v=Vector{T}(v)
        c=Vector{T}(c)

        err=Inf;
        # If c is zero vector we take eigvec approx as normalization vector
        use_v_as_normalization_vector=false;
        if norm(c) == 0
            use_v_as_normalization_vector=true;
            c[:] = v / norm(v)^2
        end
        v[:] = v/dot(c,v);
        local linsolver::LinSolver
        local tempvec = Vector{T}(undef, size(nep,1))

        push_info!(logger,2,
                   "use_v_as_normalization_vector=$use_v_as_normalization_vector");

        for k=1:maxit
            err=estimate_error(errmeasure,λ,v)
            push_iteration_info!(logger,k,err=err,λ=λ,v=v,continues=true);
            if (err< tol)
                push_info!(logger,"")
                return (λ,v)
            end
            # tempvec =  (M(λ_k)^{-1})*M'(λ_k)*v_k
            # α = 1/(c'*(M(λ_k)^{-1})*M'(λ_k)*v_k);

            z::AbstractVector=compute_Mlincomb(nep,λ,v,[T(1.0)],1)

            linsolver = create_linsolver(linsolvercreator,nep,λ)
            tempvec[:] = Vector{T}(lin_solve(linsolver, z, tol=tol));

            if (use_v_as_normalization_vector)
                c[:] = v /norm(v)^2
            end
            α = T(1)/ dot(c,tempvec);

            Δλ=-α
            Δv=α*tempvec-v;

            (Δλ,Δv,j,scaling)=armijo_rule(nep,errmeasure,err,
                                          λ,v,Δλ,Δv,real(T(armijo_factor)),armijo_max)

            if (j>0)
                push_info!(logger," Armijo scaling=$scaling")
            else
                push_info!(logger,"")
            end

            λ+=Δλ
            v[:]+=Δv

        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

"""
    quasinewton([T=ComplexF64],nep,[errmeasure,][tol,][maxit,][λ,][v][ws][logger][linsolvercreator,][armijo_factor,][armijo_max])

An implementation of the quasi-Newton approach referred to as quasi-Newton 2 in the reference.
The method involves one linear system solve per iteration corresponding with the
matrix ``M(λ)``, where ``λ`` is constant.
The vector `ws` is a representation of the normalization, in the sense that ``c^T=w_s^TM(λ)``,
where all iterates satisfy ``c^Tx_i=1``.
See [`augnewton`](@ref) for other parameters.

# Example
```julia-repl
julia> nep=nep_gallery("pep0")
julia> λ,v=quasinewton(nep,λ=1.0,v=ones(size(nep,1)));
julia> norm(compute_Mlincomb(nep,λ,v))/norm(v)
5.448264607410413e-12
```

# References
* Jarlebring, Koskela, Mele, Disguised and new Quasi-Newton methods for nonlinear eigenvalue problems, Numer. Algorithms, 79:311-335, 2018. [preprint](https://arxiv.org/abs/1702.08492)
"""
    quasinewton(nep::NEP;params...)=quasinewton(ComplexF64,nep;params...)
    function quasinewton(::Type{T},
                         nep::NEP;
                         errmeasure = DefaultErrmeasure(nep),
                         tol::Real=eps(real(T))*100,
                         maxit::Int=100,
                         λ::Number=zero(T),
                         v::Vector=randn(real(T),size(nep,1)),
                         ws::Vector=v,
                         logger=0,
                         linsolvercreator=DefaultLinSolverCreator(),
                         armijo_factor::Real=1,
                         armijo_max::Int=5) where T

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ=T(λ)
        v=Vector{T}(v)
        ws=Vector{T}(ws) # Left vector such that c'=w'M(λ) where c normalization

        n = size(nep,1)
        u = zeros(T,n)
        w = zeros(T,n)

        err=Inf;

        local linsolver::LinSolver;
        push_info!(logger,"Precomputing linsolver")
        linsolver = create_linsolver(linsolvercreator,nep,λ)

        for k=1:maxit
            err=estimate_error(errmeasure,λ,v)
            push_iteration_info!(logger,k,err=err,λ=λ,v=v,continues=true);
            if (err< tol)
                push_info!(logger,"")
                return (λ,v)
            end

            # Compute u=M(λ)v and w=M'(λ)v
            u[:] = compute_Mlincomb(nep,λ,v,[T(1)],0);
            w[:] = compute_Mlincomb(nep,λ,v,[T(1)],1);

            # Intermediate quantities
            Δλ=-dot(ws,u)/dot(ws,w);
            z=Δλ*w+u;
            # Throws an error if lin_solve returns incorrect type.
            local Δv::Vector{T}= -lin_solve(linsolver, z, tol=tol)

            normΔv=norm(Δv);
            push_info!(logger,2," norm(Δv)=$normΔv",continues=true)

            (Δλ,Δv,j,scaling)=armijo_rule(nep,errmeasure,err,
                                          λ,v,Δλ,Δv,real(T(armijo_factor)),armijo_max)

            if (j>0)
                push_info!(logger," Armijo scaling=$scaling")
            else
                push_info!(logger,"");
            end

            # Update eigenpair
            λ += Δλ
            v[:] += Δv; # eigvec update

        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

"""
    λ,v = newtonqr([eltype],nep::NEP;[errmeasure,][tol,][maxit,][λ,][v,][c,][logger])

This function implements the Newton-QR method as formulated in the reference. The method ivolves the computation of a rank-revealing QR factorization
of ``M(λ)``, with the idea that on convergence the the last diagonal element ``R[n,n]`` of the upper-triangular matrix ``R`` becomes zero as a result of ``M(λ)``
becoming singular. Since the computation of a QR factorization is expensive, it is advisable to use this method for problems of small size or problems with
a certain structure that makes the QR computation less expensive.
See [`augnewton`](@ref) for other parameters.

# Example
```julia-repl
julia> nep=nep_gallery("pep0")
julia> λ,v=newtonqr(nep,v=ones(size(nep,1)));
julia> norm(compute_Mlincomb(nep,λ,v))/norm(v)
8.440206093655014e-15
```

# References
* Kublanovskaya, V. N., (1970).  On an approach to the solution of the generalized latent value problem for λ-matrices, SIAM J. Numer. Anal. 7, 532–537
* Güttel, S., & Tisseur, F. (2017). The nonlinear eigenvalue problem. Acta Numerica, 26, 1-94. doi:10.1017/S0962492917000034
"""
    newtonqr(nep::NEP;params...)=newtonqr(ComplexF64,nep;params...)
    function newtonqr(::Type{T},
                      nep::NEP;
                      errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                      tol::Real=eps(real(T))*100,
                      maxit::Int=100,
                      λ::Number=zero(T),
                      v::Vector=randn(real(T),size(nep,1)),
                      c::Vector=v,
                      logger=0) where T

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ=T(λ)
        v=Vector{T}(v)
        c=Vector{T}(c)

        n = size(nep,1);
        local err
        local w

        en = zeros(n);
        en[n] = 1;

        for k=1:maxit
            A = compute_Mder(nep,λ);
            Q,R,PI = qr(A, Val(true)) #QR factorization with pivoting.
            Q = Matrix(Q)

            P = Matrix{T}(I, n, n)[:,PI] #The permutation matrix corresponding to the pivoted QR.

            p = R[1:n-1,1:n-1]\R[1:n-1,n];
            v = P*[-p;T(1)];#Right eigenvector
            w = Q*en;#Left eigenvector

            #err = abs(R[n,n])/norm(compute_Mder(nep,λ),2); # Frobenius norm
            err=estimate_error(errmeasure,λ,v);

            push_iteration_info!(logger,k,err=err,λ=λ,v=v);
            if(err < tol)
                return λ,v,w;
            end

            d = dot(Q[:,n],compute_Mlincomb(nep,λ,reshape(v,n,1),[T(1)],1));
            #d = dot(Q[:,n],compute_Mder(nep,λ,1)*P*[-p;T(1.0)]);
            λ = λ - R[n,n]/d;
        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

"""
    λ,v = implicitdet([eltype],nep::NEP;[errmeasure,][tol,][maxit,][λ,][v,][c,][logger])

This function implements the Implicit determinant method as formulated Algorithm 4.3 in the reference. The method applies Newton-Raphson to the equation
``det(M(λ))/det(G(λ)) = 0``, where ``G(λ)`` is a saddle point matrix with ``M(λ)``
in the (1,1) block. The (2,1) and (1,2) blocks of ``G(λ)`` are set to
``c^H`` and ``c`` respectively. Note that ``G(λ) `` can be non-singular even when ``M(λ) ``
is singular. See reference for more information.
See [`augnewton`](@ref) for other parameters.

# Example
```julia-repl
julia> nep=nep_gallery("pep0")
julia> λ,v=implicitdet(nep,v=ones(size(nep,1)));
julia> norm(compute_Mlincomb(nep,λ,v))/norm(v)
2.566371972986362e-14
```

# References
* Spence, A., & Poulton, C. (2005). Photonic band structure calculations using nonlinear eigenvalue techniques, J. Comput. Phys., 204 (2005), pp. 65–8
* Güttel, S., & Tisseur, F. (2017). The nonlinear eigenvalue problem. Acta Numerica, 26, 1-94. doi:10.1017/S0962492917000034
"""
    implicitdet(nep::NEP;params...)=implicitdet(ComplexF64,nep;params...)
    function implicitdet(::Type{T},
                         nep::NEP;
                         errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                         tol=eps(real(T))*100,
                         maxit=100,
                         λ=zero(T),
                         v=randn(real(T),size(nep,1)),
                         c=v,
                         logger=0) where T

        @parse_logger_param!(logger)

        n = size(nep,1);
        v = Vector{T}(vcat(v,one(T)))
        vp = zeros(T,n+1)
        c = Vector{T}(c);
        b = c;
        P = Matrix{T}(I, n+1, n+1)

        local err

        for k=1:maxit

            A = compute_Mder(nep,λ);
            AA = [A b;c' 0];#The matrix G(λ)

            F = lu(AA);

            v[:] = F\([zeros(T,n);T(1)]);
            vp[:] = F\([-1*compute_Mder(nep,λ,1)*v[1:n];0]);

            #err = estimate_error(errmeasure,λ,v[1:n]);
            err = abs(v[n+1])/norm(compute_Mder(nep,λ),2); # Frobenius norm based error
            push_iteration_info!(logger,k,err=err,λ=λ,v=v);
            if(err < tol)
                return λ,v[1:n];
            end

            λ = λ - v[n+1]/vp[n+1];#Newton update for the equation det(M(λ))/det(G(λ)) = 0
        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,NaN,msg))
    end

    # Armijo rule implementation
    function armijo_rule(nep,errmeasure,err0,λ,v,Δλ,Δv,armijo_factor,armijo_max)
        j=0
        if (armijo_factor<1)
            # take smaller and smaller steps until errmeasure is decreasing
            while (estimate_error(errmeasure,λ+Δλ,v+Δv)>err0 && j<armijo_max)
                j=j+1;
                Δv=Δv*armijo_factor;
                Δλ=Δλ*armijo_factor;
            end
        end
        return  (Δλ,Δv,j,armijo_factor^j)
    end


#An Extended Kalman Filter (EKF) example taken from Markus Gesmann's
#Mages' blog titled "Extended Kalman filter example in R" [1] which in turn
#was taken from a blog post titled "Fun with (Extended Kalman) Filters"
#by Dominic Steinitz [2].
#The web addresses are:
#[1] http://www.magesblog.com/2015/01/extended-kalman-filter-example-in-r.html
#[2] https://idontgetoutmuch.wordpress.com/2014/09/09/fun-with-extended-kalman-filters-4/

#Here we import the required modules
using StateSpace
using Distributions
using Gadfly
using DataFrames
using Colors

################################################################################
#Section: Generate noisy Observations
#-------------------------------------------------------------------------------

#Here we set the parameters with their true values
r = 0.2 #r is the growth rate
k = 100.0 #k is the carrying capacity
p0 = 0.1 * k # p0 is the initial population
Δt = 0.1 # Δt is the change in time.

#Define the logistic growth function to set the observations
function logisticGrowth(r, p, k, t)
    k * p * exp(r*t) / (k + p * (exp(r*t) - 1))
end
logisticGrowth(state) = logisticGrowth(state[1], state[2], k, Δt)

#Set the measurement noise variance
measurement_noise_variance = 25.0

#create the noisy observations (zero mean Gaussian noise)
numObs = 100
true_values = Vector{Float64}(numObs)
population_measurements = Vector{Float64}(numObs)
for i in 1:numObs
    true_values[i] = logisticGrowth(r, p0, k, i*Δt)
    population_measurements[i] = true_values[i] + randn() * sqrt(measurement_noise_variance)
end

#Since we're going to assume that our state consists of a growth rate, r,
#as well as a population rate, p, we need measurements of the growth rate.
#The problem is that we don't actually observe the growth rate directly.
#So we will set these measurements to zero.
growth_rate_measurements = zeros(numObs)

#Then we put these measurements together
measurements = [growth_rate_measurements population_measurements]'
#End Section: Generate noisy Observations
################################################################################

################################################################################
#Section: Describe Extended Kalman Filter parameters
#-------------------------------------------------------------------------------
#The state consists of the growth rate, r, and the population number, p. We'll
#assume that the growth rate is constant throughout time. And we'll assume that
#the population update follows the logistic growth pattern. So let's create that
#process function
function process_fcn(state)
    predict_growth_rate = state[1]
    predict_population = logisticGrowth(state)
    new_state = [predict_growth_rate, predict_population]
    return new_state
end
#Here we assume that there is no evolution noise so well create a zero matrix
process_noise_mat = diagm([0.001, 0.001])

#Now we need to describe the observation model. We'll assume that we don't
#observe the growth rate but we do observe the population. So the observation
#function is:
function observation_fcn(state)
    growth_rate_observation = 0.0
    population_observation = 1.0 * state[2]
    observation = [growth_rate_observation, population_observation]
    return observation
end

#Now we need to set the observation noise. We already set the measurement noise
#for the population earlier. We'lll arbitrarily make the variance for the
#growth rate (it doesn't matter because we don't explicity observe it):
observation_noise_mat = diagm([1.0, measurement_noise_variance])

#Create instance of our EKF model
nonLinSSM = NonlinearGaussianSSM(process_fcn, process_noise_mat, observation_fcn, observation_noise_mat)
#End Section: Describe Extended Kalman Filter parameters
################################################################################

################################################################################
#Section: Set initial guess of the state
#-------------------------------------------------------------------------------
initial_guess = MvNormal([0.5, 10], diagm([1.0,20.0]))
################################################################################

################################################################################
#Section: Execute the Extended Kalman Filter
#-------------------------------------------------------------------------------
filtered_state = filter(nonLinSSM, measurements, initial_guess)
#End Section: Execute  the Extended Kalman Filter
################################################################################

################################################################################
#Section: Plot Filtered results
#-------------------------------------------------------------------------------
#Here we are plotting the filtered results with Gadfly. See the Gadfly
#documentation for information about how plotting works if you are unfamiliar.
#Website: http://gadflyjl.org/

x_data = 1:numObs
population_array = Vector{Float64}(numObs+1)
confidence_array = Vector{Float64}(numObs+1)
population_array[1] = initial_guess.μ[2]
confidence_array[1] = 2*sqrt(initial_guess.Σ.mat[2,2])
for i in x_data
    current_state = filtered_state.state[i]
    population_array[i+1] = current_state.μ[2]
    confidence_array[i+1] = 2*sqrt(current_state.Σ.mat[2,2])
end
df_fs = DataFrame(
    x = [0;x_data],
    y = population_array,
    ymin = population_array - confidence_array,
    ymax = population_array + confidence_array,
    f = "Filtered values"
    )

n = 3
getColors = distinguishable_colors(n, Color[LCHab(70, 60, 240)],
                                   transform=c -> deuteranopic(c, 0.5),
                                   lchoices=Float64[65, 70, 75, 80],
                                   cchoices=Float64[0, 50, 60, 70],
                                   hchoices=linspace(0, 330, 24))
population_state_plot = plot(
    layer(x=0:numObs, y=[p0;measurements[2,:]'], Geom.point, Theme(default_color=getColors[2])),
    layer(x=0:numObs, y=[p0;true_values], Geom.line, Theme(default_color=getColors[3])),
    layer(df_fs, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Measurement Number"), Guide.ylabel("Population"),
    Guide.manual_color_key("Colour Key",["Filtered Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Extended Kalman Filter Example")
    )
display(population_state_plot)
#End Section: Plot Filtered results
################################################################################

################################################################################
#Section: Plot Filtered results
#-------------------------------------------------------------------------------
#Here we are plotting the filtered results with Gadfly. See the Gadfly
#documentation for information about how plotting works if you are unfamiliar.
#Website: http://gadflyjl.org/

x_data = 1:numObs
growth_rate_array = Vector{Float64}(numObs+1)
confidence_array = Vector{Float64}(numObs+1)
growth_rate_array[1] = initial_guess.μ[1]
confidence_array[1] = initial_guess.Σ.mat[1,1]
for i in x_data
    current_state = filtered_state.state[i]
    growth_rate_array[i+1] = current_state.μ[1]
    confidence_array[i+1] = 2*sqrt(current_state.Σ.mat[1,1])
end
df_fs = DataFrame(
    x = [0;x_data],
    y = growth_rate_array,
    ymin = growth_rate_array - confidence_array,
    ymax = growth_rate_array + confidence_array,
    f = "Filtered values"
    )

growth_rate_state_plot = plot(
    layer(x=0:numObs, y=ones(numObs+1)*r, Geom.line, Theme(default_color=getColors[3])),
    layer(df_fs, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Measurement Number"), Guide.ylabel("Growth Rate"),
    Guide.manual_color_key("Colour Key",["Filtered Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Extended Kalman Filter Example")
    )
display(growth_rate_state_plot)
#End Section: Plot Filtered results
################################################################################


immutable Skellam <: DiscreteUnivariateDistribution
    μ1::Float64
    μ2::Float64

    function Skellam(μ1::Float64, μ2::Float64)
        μ1 > 0.0 && μ2 > 0.0 || error("μ1 and μ2 must be positive.")
        new(μ1, μ2)
    end

    @compat Skellam(μ1::Real, μ2::Real) = Skellam(Float64(μ1), Float64(μ2))

    Skellam(μ::Real) = Skellam(μ, μ)

    Skellam() = new(1.0, 1.0)
end

@distr_support Skellam -Inf Inf

### Parameters

params(d::Skellam) = (d.μ1, d.μ2)

### Statistics

mean(d::Skellam) = d.μ1 - d.μ2

var(d::Skellam) = d.μ1 + d.μ2

skewness(d::Skellam) = mean(d) / (var(d)^1.5)

kurtosis(d::Skellam) = 1.0 / var(d)

### Evaluation

function logpdf(d::Skellam, x::Int)
    μ1, μ2 = params(d)
    - (μ1 + μ2) + (x / 2.0) * log(μ1 / μ2) + log(besseli(x, 2.0 * sqrt(μ1) * sqrt(μ2)))
end

pdf(d::Skellam, x::Int) = exp(logpdf(d, x))

function mgf(d::Skellam, t::Real)
    μ1, μ2 = params(d)
    exp(-(μ1 + μ2) + μ1 * exp(t) + μ2 * exp(-t))
end

function cf(d::Skellam, t::Real)
    μ1, μ2 = params(d)
    exp(-(μ1 + μ2) + μ1 * cis(t) + μ2 * cis(-t))
end

### Sampling

rand(d::Skellam) = rand(Poisson(d.μ1)) - rand(Poisson(d.μ2))


function constantdiffusionproblem(stepper; nx=128, Lx=2π, kappa=1e-2, nsteps=1000, dev=CPU())
   τ = 1/kappa  # time-scale for diffusive decay
  dt = 1e-9 * τ # dynamics are resolved

  prob = Problem(nx=nx, Lx=Lx, kappa=kappa, dt=dt, stepper=stepper, dev=dev)
  g = prob.grid

  # a gaussian initial condition c(x, t=0)
  c0ampl, σ = 0.01, 0.2
  c0func(x) = @. c0ampl*exp(-x^2/(2σ^2))
  c0 = c0func.(g.x)

  # analytic solution for for 1D heat equation with constant κ
  tfinal = nsteps*dt
  σt = sqrt(2*kappa*tfinal + σ^2)
  cfinal = @. c0ampl*σ/σt * exp(-g.x^2/(2*σt^2))

  set_c!(prob, c0)
  tcomp = @elapsed stepforward!(prob, nsteps)
  updatevars!(prob)

  prob, c0, cfinal, nsteps, tcomp
end

function varyingdiffusionproblem(stepper; nx=128, Lx=2π, kappa=1e-2, nsteps=1000, dev=CPU())
   τ = 1/kappa  # time-scale for diffusive decay
  dt = 1e-9 * τ # dynamics are resolved

  kappa = kappa*ones(nx) # this is actually a constant diffusion but defining it
                         # as an array makes stepforward! call function calcN!
                         # instead of just the linear coefficients L*sol

  prob = Problem(nx=nx, Lx=Lx, kappa=kappa, dt=dt, stepper=stepper, dev=dev)
  g = prob.grid

  # a gaussian initial condition c(x, t=0)
  c0ampl, σ = 0.01, 0.2
  c0func(x) = @. c0ampl*exp(-x^2/(2σ^2))
  c0 = c0func.(g.x)

  # analytic solution for for 1D heat equation with constant κ
  tfinal = nsteps*dt
  σt = sqrt(2*kappa[1]*tfinal + σ^2)
  cfinal = @. c0ampl*σ/σt * exp(-g.x^2/(2*σt^2))

  set_c!(prob, c0)
  tcomp = @elapsed stepforward!(prob, nsteps)
  updatevars!(prob)

  prob, c0, cfinal, nsteps, tcomp
end

function constantdiffusiontest(stepper, dev::Device=CPU(); kwargs...)
  prob, c0, c1, nsteps, tcomp = constantdiffusionproblem(stepper; kwargs...)
  normmsg = "$stepper: relative error ="
  @printf("% 40s %.2e (%.3f s)\n", normmsg, norm(c1-prob.vars.c)/norm(c1), tcomp)
  isapprox(c1, prob.vars.c, rtol=nsteps*rtol_timesteppers)
end

function varyingdiffusiontest(stepper, dev::Device=CPU(); kwargs...)
  prob, c0, c1, nsteps, tcomp = varyingdiffusionproblem(stepper; kwargs...)
  normmsg = "$stepper: relative error ="
  @printf("% 40s %.2e (%.3f s)\n", normmsg, norm(c1-prob.vars.c)/norm(c1), tcomp)
  isapprox(c1, prob.vars.c, rtol=nsteps*rtol_timesteppers)
end


module Skew

mvnrnd(n,mu,C) = sqrtm(C)*randn(size(C,1),n) .+ mu

# Generate n random samples from SkewNormal(Omega,alpha).
#    Omega = d-by-d "correlation" matrix (a covariance matrix with ones on the diagonal).
#    alpha = length d vector of skew parameters.
# (This implements Proposition 1 of Azzalini & Capitanio (1999).)
function skewrndNd(n,Omega,alpha)
    delta = (Omega*alpha)/sqrt(1+dot(alpha,Omega*alpha))
    x = mvnrnd(n,0,[1 delta'; delta Omega])
    return sign(x[1,:]).*x[2:end,:]
end

# Generate n random samples from SkewNormal(xi,C,alpha).
#    xi = length d vector of location parameters (it is not necessarily the mean of the resulting samples).
#    C = d-by-d posdef matrix (it is not necessarily the covariance matrix of the resulting samples).
#    alpha = length d vector of skew parameters.
# (This implements the location-scale extension in Section 5.1 of Azzalini & Capitanio (1999).)
function skewrndNd(n,xi,C,alpha)
    s = sqrt(diag(C))
    Omega = (1.0./s) .* C .* (1.0./s)'
    return xi .+ s.*skewrndNd(n,Omega,alpha)
end

# Compute the mean of Z ~ SkewNormal(Omega,alpha).
# (See Equation (4) of Azzalini & Capitanio (1999).)
mu(Omega,alpha) = (delta = (Omega*alpha)/sqrt(1+dot(alpha,Omega*alpha)); sqrt(2/pi)*delta)

# Compute the covariance matrix of Z ~ SkewNormal(Omega,alpha).
# (See Equation (4) of Azzalini & Capitanio (1999).)
Cov(Omega,alpha) = (m = mu(Omega,alpha); Omega-m*m')

# Generate n random samples from SkewNormal(Omega,alpha) and transform them so that each entry has zero mean and unit variance.
skewrndNormalized(n,Omega,alpha) = (skewrndNd(n,Omega,alpha) .- mu(Omega,alpha))./sqrt(diag(Cov(Omega,alpha)))

end


### Edge and Gradient Related Image Operations ###

# Edge/gradient filters

function sobel()
    f = [ -1.0  0.0  1.0
          -2.0  0.0  2.0
          -1.0  0.0  1.0 ]
    return f', f
end

function prewitt()
    f = [ -1.0  0.0  1.0
          -1.0  0.0  1.0
          -1.0  0.0  1.0 ]
    return f', f
end

# Consistent Gradient Operators
# Ando Shigeru
# IEEE Trans. Pat. Anal. Mach. Int., vol. 22 no 3, March 2000
#
# TODO: These coefficients were taken from the paper It would be nice
#       to resolve the optimization problem and use higher precision
#       versions, which might allow better separable approximations of
#       ando4 and ando5.

function ando3()
    f = [ -0.112737  0.0  0.112737
          -0.274526  0.0  0.274526
          -0.112737  0.0  0.112737 ]
    return f', f
end

# Below, the ando4() and ando5() functions return filters with
# the published filter values.  The ando4_sep() and ando5_sep()
# functions return separable approximations to the corresponding
# filters, estimated using the projection of the actual values on the
# eigenvector corresponding to the largest eigenvalue of the SVD of
# the original filter.

function ando4()
    f = [ -0.022116 -0.025526  0.025526  0.022116
          -0.098381 -0.112984  0.112984  0.098381
          -0.098381 -0.112984  0.112984  0.098381
          -0.022116 -0.025526  0.025526  0.022116 ]
    return f', f
end

function ando4_sep()
    f = [-0.022175974729759376 -0.025473821998749126 0.025473821998749126 0.022175974729759376
         -0.09836750569692418  -0.11299599504060115  0.11299599504060115  0.09836750569692418
         -0.09836750569692418  -0.11299599504060115  0.11299599504060115  0.09836750569692418
         -0.022175974729759376 -0.025473821998749126 0.025473821998749126 0.022175974729759376]
    return f', f
end

function ando5()
    f = [ -0.003776 -0.010199  0.0  0.010199  0.003776
          -0.026786 -0.070844  0.0  0.070844  0.026786
          -0.046548 -0.122572  0.0  0.122572  0.046548
          -0.026786 -0.070844  0.0  0.070844  0.026786
          -0.003776 -0.010199  0.0  0.010199  0.003776 ]
    return f', f
end

function ando5_sep()
    f = [-0.0038543900766123762 -0.0101692999709622   0.0  0.0101692999709622   0.0038543900766123762
         -0.026843218687756566  -0.07082229291692607  0.0  0.07082229291692607  0.026843218687756566
         -0.046468878396946627  -0.12260200818803602  0.0  0.12260200818803602  0.046468878396946627
         -0.026843218687756566  -0.07082229291692607  0.0  0.07082229291692607  0.026843218687756566
         -0.0038543900766123762 -0.0101692999709622   0.0  0.0101692999709622   0.0038543900766123762]
    return f', f
end

# Image gradients in the X and Y direction
function imgradients(img::AbstractArray, method::String="ando3", border::String="replicate")
    sx,sy = spatialorder(img)[1] == "x" ? (1,2) : (2,1)
    s = (method == "sobel"     ? sobel() :
         method == "prewitt"   ? prewitt() :
         method == "ando3"     ? ando3() :
         method == "ando4"     ? ando4() :
         method == "ando5"     ? ando5() :
         method == "ando4_sep" ? ando4_sep() :
         method == "ando5_sep" ? ando5_sep() :
         error("Unknown gradient method: $method"))

    grad_x = imfilter(img, s[sx], border)
    grad_y = imfilter(img, s[sy], border)

    return grad_x, grad_y
end

# Magnitude of gradient, calculated from X and Y image gradients
magnitude(grad_x::AbstractArray, grad_y::AbstractArray) = hypot(grad_x, grad_y)

# Phase (angle of steepest gradient ascent), calculated from X and Y gradient images
function phase{T}(grad_x::AbstractArray{T}, grad_y::AbstractArray{T})
    EPS = sqrt(eps(eltype(T)))
    # Set phase to zero when both gradients are close to zero
    reshape([atan2(-grad_y[i], grad_x[i]) * ((abs(grad_x[i]) > EPS) | (abs(grad_y[i]) > EPS))
             for i=1:length(grad_x)], size(grad_x))
end

function phase(grad_x::AbstractImageDirect, grad_y::AbstractImageDirect)
    img = copy(grad_x, phase(data(grad_x), data(grad_y)))
    img["limits"] = (-float(pi),float(pi))
    img
end

# Orientation of the strongest edge at a point, calculated from X and Y gradient images
# Note that this is perpendicular to the phase at that point, except where
# both gradients are close to zero.

function orientation{T}(grad_x::AbstractArray{T}, grad_y::AbstractArray{T})
    EPS = sqrt(eps(eltype(T)))
    # Set orientation to zero when both gradients are close to zero
    # (grad_y[i] should probably be negated here, but isn't for consistency with earlier releases)
    reshape([atan2(grad_x[i], grad_y[i]) * ((abs(grad_x[i]) > EPS) | (abs(grad_y[i]) > EPS))
             for i=1:length(grad_x)], size(grad_x))
end

function orientation(grad_x::AbstractImageDirect, grad_y::AbstractImageDirect)
    img = copy(grad_x, orientation(data(grad_x), data(grad_y)))
    img["limits"] = (-float(pi),float(pi))
    img
end

# Return both the magnitude and phase in one call
magnitude_phase(grad_x::AbstractArray, grad_y::AbstractArray) = (magnitude(grad_x,grad_y), phase(grad_x,grad_y))

# Return the magnituded and phase of the gradients in an image
function magnitude_phase(img::AbstractArray, method::String="ando3", border::String="replicate")
    grad_x, grad_y = imgradients(img, method, border)
    return magnitude_phase(grad_x, grad_y)
end

# Return the x-y gradients and magnitude and phase of gradients in an image
function imedge(img::AbstractArray, method::String="ando3", border::String="replicate")
    grad_x, grad_y = imgradients(img, method, border)
    mag = magnitude(grad_x, grad_y)
    orient = orientation(grad_x, grad_y)
    return (grad_x, grad_y, mag, orient)
end

# Thin edges
thin_edges{T}(img::AbstractArray{T,2}, gradientangles::AbstractArray, border::String="replicate") =
    thin_edges_nonmaxsup(img, gradientangles, border)
thin_edges_subpix{T}(img::AbstractArray{T,2}, gradientangles::AbstractArray, border::String="replicate") =
    thin_edges_nonmaxsup_subpix(img, gradientangles, border)

# Code below is related to non-maximal suppression, and was ported to Julia from
# http://www.csse.uwa.edu.au/~pk/research/matlabfns/Spatial/nonmaxsup.m
# (Please conserve the original copyright below.)

# NONMAXSUP - Non-maxima suppression
#
# Usage:
#          (im,location) = nonmaxsup(img, gradientangles, radius);
#
# Function for performing non-maxima suppression on an image using
# gradient angles.  Gradient angles are assumed to be in radians.
#
# Input:
#   img - image to be non-maxima suppressed.
#
#   gradientangles - image containing gradient angles around each pixel in radians
#                    (-pi,pi)
#
#   radius  - Distance in pixel units to be looked at on each side of each
#             pixel when determining whether it is a local maxima or not.
#             This value cannot be less than 1.
#             (Suggested value about 1.2 - 1.5)
#
# Returns:
#   im        - Non maximally suppressed image.
#   location  - `Graphics.Point` image holding subpixel locations of edge
#               points.
#
# Notes:
#
# This function uses bilinear interpolation to estimate
# intensity values at ideal, real-valued pixel locations on each side of
# pixels to determine if they are local maxima.

# Copyright (c) 1996-2013 Peter Kovesi
# Centre for Exploration Targeting
# The University of Western Australia
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# The Software is provided "as is", without warranty of any kind.

# December  1996 - Original version
# September 2004 - Subpixel localization added
# August    2005 - Made Octave compatible
# October   2013 - Final thinning applied to binary image for Octave
#                  compatbility (Thanks to Chris Pudney)
# June      2014 - Ported (and modified significantly) to Julia (Kevin Squire)

import Base.Graphics.Point

if !applicable(zero, Point)
    import Base.zero
    zero(Point) = Point(0.0,0.0)
end

# Used to encode the sign, integral, and fractional components of
# an offset from a coordinate
immutable CoordOffset
    s::Int      # sign
    i::Int      # integer part
    f::Float64  # fractional part
end

CoordOffset(x::Float64) = ((frac,i) = modf(x); CoordOffset(sign(frac), int(i), abs(frac)))
(-)(off::CoordOffset) = CoordOffset(-off.s,-off.i, off.f)
(*)(x::Number, off::CoordOffset) = x*(off.i + off.s*off.f)
(*)(off::CoordOffset, x::Number) = x*(off.i + off.s*off.f)
(+)(x::Number, off::CoordOffset) = x + off.i + off.s*off.f
(+)(off::CoordOffset, x::Number) = x + off.i + off.s*off.f

# Precalculate x and y offsets relative to centre pixel for each orientation angle
function _calc_discrete_offsets(θ, radius, transposed)

    θ_count = iround(2π/θ)
    θ = 2π/θ_count
    angles = (0:θ_count)*θ

    # x and y offset of points at specified radius and angles
    # from each reference position.

    if transposed
        # θ′ = -π/2 - θ
        xoffs = [CoordOffset(-x) for x in  radius*sin(angles)]
        yoffs = [CoordOffset( y) for y in  radius*cos(angles)]
    else
        xoffs = [CoordOffset( x) for x in  radius*cos(angles)]
        yoffs = [CoordOffset(-y) for y in  radius*sin(angles)]
    end

    return θ, xoffs, yoffs
end

_discretize_angle(angle::FloatingPoint, invθ) =
    angle < 0 ? iround((angle + 2π)*invθ)+1 : iround(angle*invθ)+1

# Interpolate the value of an offset from a particular pixel
#
# Returns (interpolated value, min_value of adjacent pixels in direction of offset)
#
# The second value is made available to eliminate double edges; if the value at
# (x,y) is less than the value or values adjacent to it in the direction of the
# gradient (xoff,yoff), then it is not a local maximum

function _interp_offset(img::AbstractArray, x::Integer, y::Integer, xoff::CoordOffset, yoff::CoordOffset, Ix, Iy, pad)
    fx = Ix[x + xoff.i + pad]
    fy = Iy[y + yoff.i + pad]
    cx = Ix[x + xoff.i + xoff.s + pad]
    cy = Iy[y + yoff.i + yoff.s + pad]

    tl = img[fy,fx]    # Value at bottom left integer pixel location.
    tr = img[fy,cx]    # bottom right
    bl = img[cy,fx]    # top left
    br = img[cy,cx]    # top right

    upperavg = tl + xoff.f * (tr - tl)  # Now use bilinear interpolation to
    loweravg = bl + xoff.f * (br - bl)  # estimate value at x,y

    min_adjacent = (fx == x) & (fy == y) ? min(tr,bl) : tl

    return (upperavg + yoff.f * (loweravg - upperavg), min_adjacent)
end

# Core edge thinning algorithm using nonmaximal suppression
function thin_edges_nonmaxsup_core!{T}(out::AbstractArray{T,2}, location::AbstractArray{Point,2},
                                       img::AbstractArray{T,2}, gradientangles, radius, border, theta)
    calc_subpixel = !isempty(location)

    # Error checking
    size(img) == size(gradientangles) == size(out) || error("image, gradient angle, and output image must all be the same size")
    calc_subpixel && size(location) != size(img) && error("subpixel location has a different size than the input image")
    radius < 1.0 && error("radius must be >= 1")

    # Precalculate x and y offsets relative to centre pixel for each orientation angle
    transposed = spatialorder(img)[1] == "x"
    θ, xoffs, yoffs = _calc_discrete_offsets(theta, radius, transposed)
    iθ = 1/θ

    # Indexes to use for border handling
    pad = iceil(radius)
    Ix = Images.padindexes(img, 2, pad, pad, border)
    Iy = Images.padindexes(img, 1, pad, pad, border)

    # Now run through the image interpolating grey values on each side
    # of the centre pixel to be used for the non-maximal suppression.

    (height,width) = size(img)

    for x = 1:width, y = 1:height
        (c = img[y,x]) == 0 && continue  # For thresholded images

        or = _discretize_angle(gradientangles[y,x],iθ)   # Disretized orientation
        v1, n1 = _interp_offset(img, x, y, xoffs[or], yoffs[or], Ix, Iy, pad)

        if (c > v1) & (c >= n1) # We need to check the value on the other side...
            v2, n2 = _interp_offset(img, x, y, -xoffs[or], -yoffs[or], Ix, Iy, pad)

            if (c > v2) & (c >= n2)  # This is a local maximum.
                                     # Record value in the output image.
                if calc_subpixel
                    # Solve for coefficients of parabola that passes through
                    # [-1, v2]  [0, img] and [1, v1].
                    # v = a*r^2 + b*r + c

                    # c = img[y,x]
                    a = (v1 + v2)/2 - c
                    b = a + c - v2

                    # location where maxima of fitted parabola occurs
                    r = -b/2a
                    location[y,x] = transposed ? Point(y + r*yoffs[or], x + r*xoffs[or]) :
                                                 Point(x + r*xoffs[or], y + r*yoffs[or])

                    if T<:FloatingPoint
                        # Store the interpolated value
                        out[y,x] = a*r^2 + b*r + c
                    else
                        out[y,x] = c
                    end
                else
                    out[y,x] = c
                end
            end
        end
    end

    out
end

# Main function call when subpixel location of edges is not needed
function thin_edges_nonmaxsup!{A<:AbstractArray,B<:AbstractArray}(out::A, img::A, gradientangles::B, border::String="replicate";
                                                                  radius::Float64=1.35, theta=pi/180)
    properties(out) != properties(img) && error("Input and output arrays must have the same properties")
    thin_edges_nonmaxsup_core!(data(out), Array(Point,(0,0)), img, gradientangles, radius, border, theta)
    out
end

function thin_edges_nonmaxsup{T}(img::AbstractArray{T,2}, gradientangles::AbstractArray, border::String="replicate";
                                 radius::Float64=1.35, theta=pi/180)
    (height,width) = size(img)
    out = zeros(T, height, width)
    thin_edges_nonmaxsup_core!(out, Array(Point,(0,0)), img, gradientangles, radius, border, theta)
    copy(img, out)
end

# Main function call when subpixel location of edges is desired
function thin_edges_nonmaxsup_subpix!{A<:AbstractArray, B<:AbstractArray, C<:AbstractArray}(out::A, location::B, img::A, gradientangles::C,
                                     border::String="replicate"; radius::Float64=1.35, theta=pi/180)
    properties(out) != properties(img) && error("Input and output arrays must have the same properties")
    eltype(location) != Point && error("Preallocated subpixel location array/image must have element type Graphics.Point")

    thin_edges_nonmaxsup_core!(data(out), data(location), img, gradientangles, radius, border, theta)
    img, location
end

function thin_edges_nonmaxsup_subpix{T}(img::AbstractArray{T}, gradientangles::AbstractArray,
                                        border::String="replicate"; radius::Float64=1.35, theta=pi/180)
    (height,width) = size(img)
    out = zeros(T, height, width)
    location = zeros(Point, height, width)
    thin_edges_nonmaxsup_core!(out, location, img, gradientangles, radius, border, theta)

    copy(img, out), copy(img, location)
end


export pivotedqr

function pivotedqr(ψ::AbstractMatrix{T}, u::AbstractVector{T}; invert::Bool=true, verbose::Bool = true, ϵrel::Float64 = 1e-1, maxterms::Int64=typemax(Int64)) where {T}

    m, n = size(ψ)

    # Compute a pivoted QR decomposition of ψ, use the pivot for the sensor placement

    F = qr(ψ, Val(true))

    residue = copy(u)
    idxset = Int64[]
    dict = collect(1:n)
    ϵu = norm(u)
    ϵhist = [ϵu]
    ϵrel *= ϵu

    @inbounds for k=1:n
        # Update set of selected basis
        push!(idxset, F.p[k])

        # Update candidate dictionary
        filter!(x-> x != F.p[k], dict)

        # Compute residual
        c = view(ψ,:,idxset)\u
        residue = u - view(ψ,:,idxset)*c

        # Calculate stopping critera
        ϵ = norm(residue)
        if verbose == true
            push!(ϵhist, copy(ϵ))
        end
        if ϵ < ϵrel || k == maxterms
            break
        end

    end

    # Solve the system with the set of indices
    if verbose == true
        if invert == true
            c = zeros(length(idxset))
            c .= view(ψ,:, idxset)\u
            return idxset, c, ϵhist
        else
            return idxset, ϵhist
        end
    else
        if invert == true
            c = zeros(length(idxset))
            c .= view(ψ,:, idxset)\u
            return idxset, c
        else
            return idxset
        end
    end
end


### A Pluto.jl notebook ###
# v0.12.7

using Markdown
using InteractiveUtils

# ╔═╡ 6fd616f6-1ea1-11eb-3814-8bfb4a096c49
using Pkg, DrWatson

# ╔═╡ 6ffe2628-1ea1-11eb-24ea-57f985146a72
begin
	@quickactivate "StatisticsWithJuliaPlutoNotebooks"
	using Distributions, Random, StatsBase, StatsPlots
	Random.seed!(1)
end;

# ╔═╡ ed174bc4-1ea0-11eb-1e2f-a32874cec549
md"## Listing 3.11"

# ╔═╡ 70179b62-1ea1-11eb-13ce-7fb4d54bad4b
dist1 = TriangularDist(0,10,5)

# ╔═╡ 75c334f0-2134-11eb-132d-dfcb59e14474
dist2 = DiscreteUniform(1,5)

# ╔═╡ 75c37638-2134-11eb-150e-a5788c7f72a7
theorMean1, theorMean2 = mean(dist1), mean(dist2)

# ╔═╡ 75c3fa78-2134-11eb-0259-699e1c89a023
begin
	N = 10^6
	data1 = rand(dist1,N)
	data2 = rand(dist2,N)
	estMean1, estMean2 = mean(data1), mean(data2)
end

# ╔═╡ de66cfee-2134-11eb-2c7a-add4bee67341
density(data1, lab="Triangukar density")

# ╔═╡ a315bbee-2134-11eb-0249-17f3c6af3543
density!(data2, lab="Discrete Uniform density")

# ╔═╡ 75cedfbc-2134-11eb-3b3c-5bdc69e4735a
Text("Symmetric Triangular Distiribution on [0,10] has mean $theorMean1
	(estimated: $estMean1)")

# ╔═╡ 75cf627a-2134-11eb-0dc9-e5e3d869f4a8
Text("Discrete Uniform Distiribution on {1,2,3,4,5} has mean $theorMean2
	(estimated: $estMean2)")

# ╔═╡ 70182cd8-1ea1-11eb-094d-8d0d49cf15f3
md"## End of listing 3.11"

# ╔═╡ Cell order:
# ╟─ed174bc4-1ea0-11eb-1e2f-a32874cec549
# ╠═6fd616f6-1ea1-11eb-3814-8bfb4a096c49
# ╠═6ffe2628-1ea1-11eb-24ea-57f985146a72
# ╠═70179b62-1ea1-11eb-13ce-7fb4d54bad4b
# ╠═75c334f0-2134-11eb-132d-dfcb59e14474
# ╠═75c37638-2134-11eb-150e-a5788c7f72a7
# ╠═75c3fa78-2134-11eb-0259-699e1c89a023
# ╠═de66cfee-2134-11eb-2c7a-add4bee67341
# ╠═a315bbee-2134-11eb-0249-17f3c6af3543
# ╠═75cedfbc-2134-11eb-3b3c-5bdc69e4735a
# ╠═75cf627a-2134-11eb-0dc9-e5e3d869f4a8
# ╟─70182cd8-1ea1-11eb-094d-8d0d49cf15f3


function collatz_steps(n::Integer)
    n < 1 && throw(DomainError("n must be positive"))
    count = 0
    while n > 1
        count += 1
        n = isodd(n) ? 3n + 1 : n ÷ 2
    end
    count
end

# Recursion: hard to reason about and slow in Julia as there's no TCO.
function collatz_steps_slow(n::Integer)
    # Avoid checking n < 1 in every loop
    n < 1 && throw(DomainError("n must be positive"))
    _collatz_steps(n)
end

function _collatz_steps(n::Integer)
    n == 1 && return 0
    isodd(n) && return 2 + _collatz_steps((3n + 1) ÷ 2)
    1 + _collatz_steps(n÷2)
end


# This file is part of IntegerSequences.
# Copyright Peter Luschny. License is MIT.

(@__DIR__) ∉ LOAD_PATH && push!(LOAD_PATH, (@__DIR__))

module Polynomials

using Nemo
# import AbstractAlgebra.lead

export ModulePolynomials
export Poly, AltPoly, EgfPoly, AltEgfPoly
export Coeffs, CoeffSum, CoeffAltSum, CoeffConst, CoeffLeading, AltCoeffs
export Diagonal, Central, EgfCoeffs, AltEgfCoeffs, ReflectPoly

"""

Mostly convenient functions to deal with polynomials as often used in connection with ordinary and exponential generating functions. The naming scheme used is roughly described by:

```
   Poly       <-> Coeffs
   AltPoly    <-> Poly(AltCoeffs)
   EgfPoly    <-> Poly(EgfCoeffs)
   OgfPoly    <-> Poly(OgfCoeffs)
   AltEgfPoly <-> Poly(AltEgfCoeffs)
```

Here 'Alt' stands for alternating, 'Egf' for exponential generating function, 'Ogf' for ordinary generating function.

* Coeffs, CoeffSum, CoeffAltSum, CoeffConst, CoeffLeading, AltCoeffs, Diagonal, Central, EgfCoeffs, AltEgfCoeffs, Poly, AltPoly, EgfPoly, AltEgfPoly, ReflectPoly.
"""
const ModulePolynomials = ""

"""

Return the coefficients of the polynomial ``p``.
"""
Coeffs(p::fmpz_poly) = [coeff(p, k) for k ∈ 0:degree(p)]

"""

Return the coefficients of the polynomial ``p`` with alternating signs.
"""
AltCoeffs(p::fmpz_poly) = [(-1)^k * coeff(p, k) for k ∈ 0:degree(p)]

"""

Return the coefficients of the polynomial ``p`` multiplied by ``k!``.
"""
OgfCoeffs(p::fmpz_poly) = [factorial(k)*coeff(p, k) for k ∈ 0:degree(p)]

"""

Return the coefficients of the polynomial ``p`` divided by ``k!``. Note that integer division is used.
"""
EgfCoeffs(p::fmpz_poly) = [div(coeff(p, k), factorial(k)) for k ∈ 0:degree(p)]

"""

Return the coefficients of the polynomial ``p`` divided by ``(-1)^k k!``.
"""
AltEgfCoeffs(p::fmpz_poly) = [(-1)^k * div(coeff(p, k), factorial(k)) for k ∈ 0:degree(p)]

"""

Return the polynomial ``p`` with the coefficients C.
"""
function Poly(C)
    T, x = PolynomialRing(ZZ, "x")
    sum(c * x^k for (k, c) ∈ enumerate(C))
end

"""

Return the polynomial ``p`` with the coefficients C and alternating signs (i.e. with ``(-1)^k c[k] x^k)``.
"""
function AltPoly(C)
    T, x = PolynomialRing(ZZ, "x")
    sum((-1)^k * c * x^k for (k, c) ∈ enumerate(C))
end

"""

Return the polynomial ``p`` with the coefficients C used in the form ``c[k] x^k/k!``. Note that integer division is used.
"""
function EgfPoly(C)
    T, x = PolynomialRing(ZZ, "x")
    sum(div(c, factorial(k)) * x^k for (k, c) ∈ enumerate(C))
end

"""

Return the polynomial ``p`` with the coefficients C used in the form ``c[k] k! x^k``.
"""
function OgfPoly(C)
    T, x = PolynomialRing(ZZ, "x")
    sum(c * factorial(k) * x^k for (k, c) ∈ enumerate(C))
end

"""

Return the polynomial ``p`` with alternating signs attached to the coefficients .
"""
AltPoly(p::fmpz_poly) = Poly(AltCoeffs(p))
#    T, x = PolynomialRing(ZZ, "x")
#    sum((-1)^k*coeff(p, k)*x^k for k ∈ 0:degree(p))

"""

Return the polynomial ``p`` with coefficients in exponential form (i.e. with ``c[k] x^k/k!``).
"""
EgfPoly(p::fmpz_poly) = Poly(EgfCoeffs(p))
#    T, x = PolynomialRing(ZZ, "x")
#    sum(div(coeff(p, k), factorial(k))*x^k for k ∈ 0:degree(p))

"""

Return the polynomial ``p`` with coefficients in exponential form and alternating signs (i.e. with ``(-1)^k c[k] x^k/k!``).
"""
AltEgfPoly(p::fmpz_poly) = Poly(AltEgfCoeffs(p))
#    T, x = PolynomialRing(ZZ, "x")
#    sum((-1)^k*div(coeff(p, k), factorial(k))*x^k for k ∈ 0:degree(p))

"""

Return the list of the coefficients of the first ``len`` polynomials of the sequence of polynomials ``P`` as a triangle.
"""
Coeffs(P, len) = [[coeff(P(n), k) for k ∈ 0:degree(P(n))] for n ∈ 0:len-1]

"""

Return the sum of the coefficients of the polynomial ``p``.
"""
CoeffSum(p) = evaluate(p, 1)

"""

Return the sequence of the sum of coefficients of the sequence of polynomials ``P``.
"""
CoeffSum(P, len) = [CoeffSum(P(n)) for n ∈ 0:len-1]

"""

Return the alternating sum of the coefficients of the polynomial ``p``.
"""
CoeffAltSum(p) = evaluate(p, -1)

"""

Return the sequence of the alternating sums of the coefficients of the sequence of polynomials ``P``.
"""
CoeffAltSum(P, len) = [CoeffAltSum(P(n)) for n ∈ 0:len-1]

"""

Return the leading coefficient of the polynomial ``p``.
"""
CoeffLeading(p) = coeff(p, degree(p)) # lead(p)

"""

Return the sequence of the leading coefficient of the sequence of polynomials ``P``.
"""
Diagonal(P, len) = [CoeffLeading(P(n)) for n ∈ 0:len-1]

"""

Return the constant coefficient of the polynomial ``p``.
"""
CoeffConst(p) = coeff(p, 0)

"""

Return the sequence of the constant coefficients of the sequence of polynomials ``P``.
"""
CoeffConst(P, len) = [CoeffConst(P(n)) for n ∈ 0:len-1]

"""

Return the central column of the coefficients of the sequence of polynomials ``P``.
"""
Central(P, len) = [Coeffs(P(2n))[n+1] for n ∈ 0:len-1]

"""

Return the reflected polynomial of ``p``.
"""
function ReflectPoly(p::fmpz_poly)
    T, x = PolynomialRing(ZZ, "x")
    p(0) != 1 && throw(ValueError("Constant coefficient must be 1."))
    d = degree(p)
    x^d + sum(coeff(p, k) * x^(d - k) for k ∈ 1:d)
end

#START-TEST-########################################################

using Test, SeqUtils

function test()

    @testset "Polynomials" begin
        T, x = PolynomialRing(ZZ, "x")
        p = 63063000 * x^4 + 2702700 * x^3 + 16510 * x^2 + x
        @test Coeffs(p) == [0, 1, 16510, 2702700, 63063000]
        @test CoeffSum(p) == 65782211
        @test CoeffAltSum(p) == 60376809
        @test CoeffConst(p) == T(0)
        @test CoeffLeading(p) == 63063000

        q = x^2 - x - 1
        p = 1 - x - x^2
        r = ReflectPoly(p)
        @test r == q
    end

end

function demo()

    function P(m, n)
        R, x = PolynomialRing(ZZ, "x")
        function recP(m, n)
            n == 0 && return R(1)
            sum(binomial(m * n, m * k) * recP(m, n - k) * x for k ∈ 1:n)
        end
        recP(m, n)
    end

    # ... given a polynomial p:
    for m ∈ 0:4, n ∈ 0:5
        println("---> m: $m, n: $n")
        q = P(m, n)

        typeof(q) |> println
        q |> println
        AltPoly(q) |> println
        EgfPoly(q) |> println
        AltEgfPoly(q) |> println

        Coeffs(q) |> Println
        EgfCoeffs(q) |> Println
        AltEgfCoeffs(q) |> Println

        CoeffSum(q) |> println
        CoeffAltSum(q) |> println
        CoeffConst(q) |> println
        CoeffLeading(q) |> println
    end

    # ... given a sequence of polynomials P:
    for m ∈ 0:4
        println("---> m ", m)
        println("\nPolynomial:")
        Q(n) = P(m, n)
        println(Q(4))
        println("\nTriangle of coefficients:")
        Coeffs(Q, 7) |> Println
        println("\nSum of coefficients:")
        CoeffSum(Q, 7) |> Println
        println("\nAlternating sum of coefficients:")
        CoeffAltSum(Q, 7) |> Println
        println("\nLeading coefficients:")
        Diagonal(Q, 7) |> Println
        println("\nCentral column of coefficient triangle:")
        Central(Q, 7) |> Println
    end
end

function perf() end

function main()
    test()
    demo()
    perf()
end

main()

end # module


export SSDFun, HuberFun, MVFun, MVFunTotal

"""
	mis,dmis,d2mis = SSDFun(dc,dobs,Wd)

	Input:

		dc::Array   -  simulated data
		dobs::Array -  measured data
		Wd::Array   -  diagonal weighting

	Output:

		mis::Real   -  misfit, 0.5*|dc-dobs|_Wd^2
		dmis        -  gradient
		d2mis       -  diagonal of Hessian

"""
function SSDFun(dc::Union{Array{Float64},Array{Float32}},dobs::Union{Array{Float64},Array{Float32}},Wd::Union{Array{Float64},Array{Float32}})
	res   = vec(dc)-vec(dobs) # predicted - observed data
	Wd    = vec(Wd)
	mis   = .5*real(dot(Wd.*res,Wd.*res))  # data misfit
	dmis  = Wd.*(Wd.*res)
	d2mis = Wd.*Wd
	return mis, dmis, d2mis
end # function SSDFun

"""
	For complex data misfit is computed as 0.5*|real(dc)-(dobs)|_Wd^2 +  0.5*|complex(dc)-complex(dobs)|_W^2
"""
function SSDFun(dc::Array{ComplexF64},dobs::Array{ComplexF64},Wd::Array{ComplexF64})

	wdr   = vec(real(Wd)); wdi = vec(imag(Wd))
	# wdr.*dRe + im*wdi.*dIm
	res   = vec(dc)-vec(dobs)
	resw  = wdr.*real(res) + im*wdi.*imag(res)

	mis   = .5*real(dot(resw,resw))
	dmis  = sdiag(wdr.*wdr)*real(res) + 1im*sdiag(wdi.*wdi)*imag(res)
	d2mis =  wdr.*wdr + im*wdi.*wdi
	return mis,dmis,d2mis
end

"""
	mis,dmis,d2mis = HuberFun(dc,dobs,Wd,C)

	Computes misfit via

		misfit(dc,dobs) = sqrt(abs(Wd*res).^2 + eps)

	Input:
		dc::Array   -  simulated data
		dobs::Array -  measured data
		Wd::Array   -  diagional weighting
		eps         -  conditioning parameter (default=1e-3)

	Output:
		mis::Real   -  misfit
		dmis        -  gradient
		d2mis       -  diagonal of Hessian

"""
function HuberFun(dc::Array{Float64},dobs::Array{Float64},Wd::Array{Float64},eps=1e-3)
	# compute Huber distance
	res   = vec(dc-dobs)
	G     = sqrt.( abs.(Wd.*res).^2 .+ eps)
	mis   = sum(G)
	dmis  = sdiag(Wd./G)*(Wd.*res)
	d2mis = (Wd.*Wd)./G
	return mis,dmis,d2mis
end
# Old Huber fun
# function HuberFun(dc::Array{Float64},dobs::Array{Float64},Wd::Array{Float64},eps=1e-3)
# 	# compute Huber distance
# 	res   = dc-dobs
# 	G     = sqrt.( abs.(Wd.*res).^2 .+ eps)
# 	mis   = sum(G)
# 	dmis  = sdiag(Wd./G)*(Wd.*res)
# 	d2mis = (Wd.^Wd)./G
# 	return mis,dmis,d2mis
# end


"Tableau for explicit Euler method"
function getTableauExplicitEuler()
    a = zeros(Float64, 1, 1)
    b = [1.0]
    c = [0.0]
    o = 1

    TableauERK(:explicit_euler, o, a, b, c)
end

"Tableau for explicit midpoint method"
function getTableauExplicitMidpoint()
    a = [[0.0 0.0]
         [0.5 0.0]]
    b = [0.0, 1.0]
    c = [0.0, 0.5]
    o = 2

    TableauERK(:explicit_midpoint, o, a, b, c)
end

"Tableau for Runge's method"
function getTableauRunge()
    a = [[0.0 0.0]
         [1.0 0.0]]
    b = [0.5, 0.5]
    c = [0.0, 1.0]
    o = 2

    TableauERK(:runge, o, a, b, c)
end

"Tableau for Heun's method"
function getTableauHeun()
    a = [[0.0 0.0]
         [1.0 0.0]]
    b = [0.5, 0.5]
    c = [0.0, 1.0]
    o = 2

    TableauERK(:heun, o, a, b, c)
end

"Tableau for Kutta's method of order three"
function getTableauKutta()
    a = [[ 0.0 0.0 0.0]
         [ 0.5 0.0 0.0]
         [-1.0 2.0 0.0]]
    b = [1/6, 4/6, 1/6]
    c = [0.0, 0.5, 1.0]
    o = 3

    TableauERK(:kutta, o, a, b, c)
end

"Tableau for explicit Runge-Kutta method of order four (1/6 rule)"
function getTableauERK4()
    a = [[0.0 0.0 0.0 0.0]
         [0.5 0.0 0.0 0.0]
         [0.0 0.5 0.0 0.0]
         [0.0 0.0 1.0 0.0]]
    b = [1/6, 1/3, 1/3, 1/6]
    c = [0.0, 0.5, 0.5, 1.0]
    o = 4

    TableauERK(:erk4, o, a, b, c)
end

"Tableau for explicit Runge-Kutta method of order four (3/8 rule)"
function getTableauERK438()
    a = [[ 0.0  0.0  0.0  0.0]
         [ 1/3  0.0  0.0  0.0]
         [-1/3  1.0  0.0  0.0]
         [ 1.0 -1.0  1.0  0.0]]
    b = [1/8, 3/8, 3/8, 1/8]
    c = [0.0, 1/3, 2/3, 1.0]
    o = 4

    TableauERK(:erk438, o, a, b, c)
end

"Tableau for explicit Verner's method of order six"
function getTableauVerner()
    a = [[ 0.0          0.0     0.0           0.0       0.0          0.0  0.0         0.0]
         [ 1/6          0.0     0.0           0.0       0.0          0.0  0.0         0.0]
         [ 4/75         16/75   0.0           0.0       0.0          0.0  0.0         0.0]
         [ 5/6         -8/3     5/2           0.0       0.0          0.0  0.0         0.0]
         [-165/64       55/6   -425/64        85/96     0.0          0.0  0.0         0.0]
         [ 12/5        -8.0     4015/612     -11/36     88/255       0.0  0.0         0.0]
         [-8263/15000   124/75 -643/680      -81/250    2484/10625   0.0  0.0         0.0]
         [ 3501/1720   -300/43  297275/52632 -319/2322  24068/84065  0.0  3850/26703  0.0]]
    b = [3/40, 0.0, 875/2244, 23/72, 264/1955, 0.0, 125/11592, 43/616]
    c = [0.0, 1/6, 4/15, 2/3, 5/6, 1.0, 1/15, 1.0]
    o = 6

    TableauERK(:verner, o, a, b, c)
end


# using Distances

const start = time()

const DEBUG = length(ARGS) >=1 && ARGS[1] == "1"

function distance(solution::Array{Int32})
    cost = 0
    for i in 1:length(solution)-1
        cost += d[solution[i],solution[i+1]]
    end
    return cost
end

tic()
tic()
# file lines iterator object
# input = eachline(STDIN)

# state = start(input)
# read data size N
const n = parse(Int32, readline(STDIN))
const k = max(0.01*n, 25.0)

# 2xN node's coordinates matrix
data = Array{Tuple{Float64, Float64}}(n)

# read file data to Data matrix
for i in 1:n
    v = readline(STDIN)
    (i_v, x_v, y_v) = split(v)
    data[parse(Int32, i_v)] = (parse(Float64, x_v), parse(Float64, y_v))
end
# calculate nodes distances as NxN matrix

maxtime = parse(Float64, readline(STDIN))

# d = Array{Float64}(n, n)
# pairwise!(d, Euclidean(1e-12), data)

d = [hypot(x2-x1, y2-y1) for (x2, y2) in data, (x1, y1) in data]

currentsol = Array{Int32}(n+1)
currentsol[1] = currentsol[n+1] = Int32(1)

for i in 1:n
    d[i,i] = Inf
end

d1 = copy(d)
j = 1
for i in 2:length(currentsol)-1
    currentsol[i] = findfirst(d1[j,:].==min(d1[j,:]...))
    j = currentsol[i]
    d1[j,1] = Inf
    for visited_node in 1:i
        d1[j, currentsol[visited_node]] = Inf
    end
end

currentcost = distance(currentsol)

optimumsol = copy(currentsol)
optimumcost = currentcost

####### SETTINGS #######
# how many times to iterate (minimum)
const minItrs     = 10000000

# fraction of melting point for starting temperature
const meltPointF  = 0.7

# fraction of melting point for ending temperature
const targetTempF = 0.01

# fraction of stagnant minItrs allowed before reheating
const stagItrsF   = 0.1
########################

function iterate(t::Float64, xsol::Array{Int32}, xcost::Float64)
	ysol = copy(xsol)
    ycost = xcost

	t1 = rand(2:n-2)
	t2 = rand(t1+2:n)

    ycost = ycost -
    		d[ysol[t1-1], ysol[t1]] -
    		d[ysol[t1], ysol[t1+1]] +
    		d[ysol[t1-1], ysol[t2]] +
    		d[ysol[t2], ysol[t1+1]] -
    		d[ysol[t2-1], ysol[t2]] -
    		d[ysol[t2], ysol[t2+1]] +
    		d[ysol[t2-1], ysol[t1]] +
    		d[ysol[t1], ysol[t2+1]]

	ysol[t1], ysol[t2] = ysol[t2], ysol[t1]

	if ycost <= xcost
		return ysol, ycost
	elseif rand() < exp((xcost-ycost)/t)
		return ysol, ycost
	end
	return xsol, xcost
end

function iterateP(p::Float64, xsol::Array{Int32}, xcost::Float64)
	ysol = copy(xsol)
    ycost = xcost

	t1 = rand(2:n-2)
    t2Max = round(min(t1+k, n))
	t2 = rand(Int64(t1+2):Int64(t2Max))

    ycost = ycost -
    		d[ysol[t1-1], ysol[t1]] -
    		d[ysol[t1], ysol[t1+1]] +
    		d[ysol[t1-1], ysol[t2]] +
    		d[ysol[t2], ysol[t1+1]] -
    		d[ysol[t2-1], ysol[t2]] -
    		d[ysol[t2], ysol[t2+1]] +
    		d[ysol[t2-1], ysol[t1]] +
    		d[ysol[t1], ysol[t2+1]]

	ysol[t1], ysol[t2] = ysol[t2], ysol[t1]

	if ycost <= xcost
		return ysol, ycost
	elseif rand() < p
		return ysol, ycost
	end
	return xsol, xcost
end

const initialcost = currentcost
if DEBUG
    println(string("initial cost: ", initialcost))
    println(string("min iterations: ", minItrs))
    println(string("initial time: ", toq(), " seconds"))
end

testsol = copy(currentsol)
testcost = currentcost
minT = 10.0e10
maxT = 0

for i in 1:max(0.01*minItrs, 2.0)
    testsol, testcost = iterateP(0.001, testsol, testcost)

    minT = min(minT, testcost)
    maxT = max(maxT, testcost)
end

const meltPoint = (maxT - minT) * 10.0^(-1.0*log10(n))

const t0 = meltPoint * meltPointF

if DEBUG
    @printf("T0 = %.2f\n", t0)
end
t = t0

const tDecay = targetTempF ^ (1.0/minItrs)

itr = 1
optItr = 1
stagItrs = 0

function printresult()
    if DEBUG
        @printf("T = %.2f\n", t)
    	@printf("Optimum: %.5f\n", optimumcost)
    	@printf("%.2f%% improvement\n", 100.0-100*optimumcost/initialcost)
    	@printf("after %.2f%% of iterations\n", 100*optItr/itr)
    	@printf("done %.2f%% of minimum iterations\n", 100*itr/minItrs)
    	toc()
    else
        println(STDOUT, optimumcost)
        for city in optimumsol
            print(STDERR, city)
            print(STDERR, " ")
        end
        println(STDERR)
    end
end

atexit(printresult)

while t > t0*targetTempF

    if time() - start > maxtime - 1.0
        exit()
    end

	currentsol, currentcost = iterate(t, currentsol, currentcost)
	if currentcost < optimumcost
		optimumsol = copy(currentsol)
        optimumcost = currentcost
		optItr = itr
	else
		stagItrs += 1
	end

	if stagItrs == Int(stagItrsF*minItrs) && itr <= minItrs
		stagItrs = 0
        if DEBUG
            println("reheating")
        end
		t *= 1.0 + 0.7*(minItrs-itr)/minItrs
	end

	t *= tDecay

	if DEBUG && itr%(minItrs/5) == 0
		@printf("T = %.2f\n", t)
		@printf("cost: %.5f\n", currentcost)
		@printf("optimum: %.5f\n", optimumcost)
	end
    itr += 1
end


@doc raw"""
    Entropy <: AbstractImageBinarizationAlgorithm
    Entropy()

    binarize([T,] img, f::Entropy)
    binarize!([out,] img, f::Entropy)

An algorithm for finding the binarization threshold value using
the entropy of the image histogram.

# Output

Return the binarized image as an `Array{Gray{T}}` of size `size(img)`. If
`T` is not specified, it is inferred from `out` and `img`.

# Details

This algorithm uses the entropy of a one-dimensional histogram to produce a threshold
value.

Let ``f_1, f_2, \ldots, f_I`` be the frequencies in the various bins of the
histogram and ``I`` the number of bins. With ``N = \sum_{i=1}^{I}f_i``, let
``p_i = \frac{f_i}{N}`` (``i = 1, \ldots, I``) denote the probability
distribution of gray levels. From this distribution one derives two additional
distributions. The first defined for discrete values ``1`` to ``s`` and the
other, from ``s+1`` to ``I``. These distributions are

```math
A: \frac{p_1}{P_s}, \frac{p_2}{P_s}, \ldots, \frac{p_s}{P_s}
\quad \text{and} \quad
B: \frac{p_{s+1}}{1-P_s}, \ldots, \frac{p_n}{1-P_s}
\quad \text{where} \quad
P_s = \sum_{i=1}^{s}p_i.
```

The entropies associated with each distribution are as follows:

```math
H(A) = \ln(P_s) + \frac{H_s}{P_s}
```
```math
H(B) = \ln(1-P_s) + \frac{H_n-H_s}{1-P_s}
```
```math
\quad \text{where} \quad
H_s = -\sum_{i=1}^{s}p_i\ln{p_i}
\quad \text{and} \quad
H_n = -\sum_{i=1}^{I}p_i\ln{p_i}.
```

Combining these two entropy functions we have

```math
\psi(s) = \ln(P_s(1-P_s)) + \frac{H_s}{P_s} + \frac{H_n-H_s}{1-P_s}.
```
Finding the discrete value ``s`` which maximises the function ``\psi(s)`` produces
the sought-after threshold value (i.e. the bin which determines the threshold).

See Section 4 of [1] for more details on the derivation of the entropy.

# Arguments

The function argument is described in more detail below.

##  `img::AbstractArray`

The image that needs to be binarized. The image is automatically converted
to `Gray` in order to construct the requisite graylevel histogram.

# Example

Binarize the "cameraman" image in the `TestImages` package.

```julia
using TestImages, ImageBinarization

img = testimage("cameraman")
img_binary = binarize(img, Entropy())
```

# References
1. J. N. Kapur, P. K. Sahoo, and A. K. C. Wong, “A new method for gray-level picture thresholding using the entropy of the histogram,” *Computer Vision, Graphics, and Image Processing*, vol. 29, no. 1, p. 140, Jan. 1985.[doi:10.1016/s0734-189x(85)90156-2](https://doi.org/10.1016/s0734-189x%2885%2990156-2)
"""
struct Entropy <: AbstractImageBinarizationAlgorithm end

function (f::Entropy)(out::GenericGrayImage, img::GenericGrayImage)
    edges, counts = build_histogram(img,  256)
    t = find_threshold(HistogramThresholding.Entropy(), counts[1:end], edges)
    @simd for i in CartesianIndices(img)
      out[i] = img[i] < t ? 0 : 1
    end
    out
end

(f::Entropy)(out::GenericGrayImage, img::AbstractArray{<:Color3}) =
    f(out, of_eltype(Gray, img))


"""
```julia
nanstd(A; dims=:, mean=nothing, corrected=true)
```
Compute the variance of all non-`NaN` elements in `A`, optionally over dimensions specified by `dims`.
As `Statistics.var`, but ignoring `NaN`s.

A precomputed `mean` may optionally be provided, which results in a somewhat faster
calculation. If `corrected` is `true`, then _Bessel's correction_ is applied, such
that the sum is divided by `n-1` rather than `n`.

As an alternative to `dims`, `nanstd` also supports the `dim` keyword, which
behaves identically to `dims`, but also drops any singleton dimensions that have
been reduced over (as is the convention in some other languages).

## Examples
```julia
julia> using NaNStatistics

julia> A = [1 2; 3 4]
2×2 Matrix{Int64}:
 1  2
 3  4

julia> nanstd(A, dims=1)
1×2 Matrix{Float64}:
 1.41421  1.41421

julia> nanstd(A, dims=2)
2×1 Matrix{Float64}:
 0.7071067811865476
 0.7071067811865476
```
"""
nanstd(A; dims=:, dim=:, mean=nothing, corrected=true) = sqrt!(__nanvar(mean, corrected, A, dims, dim))
export nanstd

sqrt!(x::Number) = sqrt(x)
function sqrt!(A::AbstractArray)
    @turbo for i ∈ eachindex(A)
        A[i] = sqrt(A[i])
    end
    return A
end


#=Copyright (c) 2017 Gabriel Goh

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.=#

λ = 1
Rf(β, α) = [   β        λ;
            -α*β (1 - α*λ)]

α = 0.05
β = 0.4

ϵ = 8
S = inv([ 1  0 ;
          α  1 ])

function err(k)
    R = Rf(β, α)
    sum( ( ((R^(k-i))*([ϵ; -α*ϵ]) )[2] )^2 for i = 1:k )
end

function err2(k, α, β)
    R = Rf(β, α)
    x = [ϵ; -α*ϵ]
    s = 0
    z = []
    for i = 1:k
      s = s + x[2]^2
      x = R*x
      push!(z, s)
    end
    return z
end

err2(100)[50] - err(50)

function geosumgen(R)
  Λ, U = eig(R)
  (b,k) -> begin;
    Λsum = (1./(1 - Λ)).*(1 - Λ.^k);
    real(U*(Λsum.*(U\b)));
  end
end

function runmomentum(A,b,k, ϵ)
  z = zeros(length(b))
  w = 3*ones(length(b))
  for i = 1:k
    z = β*z + (A*w - b) + ϵ*randn(1)
    w = w - α*z
  end
  return (z[1],w[1])
end

function runmomentum2(A,b,k, ϵ)
  z = zeros(length(b))
  w = 3*ones(length(b))
  fx = []
  for i = 1:k
    z = β*z + (A*w - b) + ϵ*randn(1)
    w = w - α*z
    push!(fx, w[1]*w[1])
  end
  return fx
end

k = 150
d = mean([runmomentum2(1,0, k, ϵ) for i = 1:100])
e = err2(k)
d2 = [runmomentum(1,0, i, 0.)[2]^2 for i = 1:k]

plot(d,".")
plot(d2+e)
#plot(d2)

plot(err2(200, 0.01, 0))


module Stepwise

    using DataFrames
    using StatsBase
    using ..DataMod, ..Utils

    export stepwise

    # The code for `step` and `stepwise` below is inspired by the code by Bogumił Kamiński, found at
    # https://stackoverflow.com/questions/49794476/backward-elimination-forward-selection-in-multilinear-regression-in-julia

    function multibic(data::Data, rhs::Vector{Symbol})::Tuple{Float64, Vector{RegressionModel}}
        models = get_models(data, rhs)::Vector{RegressionModel}
        return (mean(bic.(models))::Float64, models)
    end
    function multibic(models::Vector{RegressionModel})::Float64
        return mean(bic.(models))
    end

    function step(data::Data, rhs::Vector{Symbol}, forward::Bool,
                  prevbest::Float64, prevmodel::Vector{RegressionModel},
                  verbose::Bool, debug::Bool)::Tuple{Vector{Symbol}, Bool, Float64, Vector{RegressionModel}}
        options = forward ? setdiff(data.xs, rhs) : rhs
        if isempty(options)
            return (rhs, false, prevbest, prevmodel)
        end
        bestbic = prevbest
        bestrhs = rhs
        bestmodel = prevmodel
        improved = false
        for opt in options
            thisrhs = forward ? [rhs; opt] : setdiff(rhs, opt)
            thisbic, thismodel = multibic(data, thisrhs)
            if debug
                print(opt); print(" - "); println(thisbic)
            end
            if thisbic < bestbic
                bestbic = thisbic
                bestrhs = thisrhs
                bestmodel = thismodel
                improved = true
            end
        end
        if verbose
            if improved
                println("$(forward ? "forward" : "backward") step - new rhs: $bestrhs\nwith mean BIC $bestbic")
            else
                println("$(forward ? "forward" : "backward") step could not improve BIC")
            end
        end
        return (bestrhs, improved, bestbic, bestmodel)
    end

    function stepwise(data::Data; verbose::Bool = true, debug::Bool = false)::Vector{RegressionModel}
        rhs = Symbol[]
        model = get_models(data, Symbol[])
        bestbic = multibic(model)
        verbose && println("BIC empty model: $bestbic")
        while true
            verbose && println("step")
            rhs, improvedfwd, bestbic, model = step(data, rhs, true, bestbic, model, verbose, debug)
            rhs, improvedbwd, bestbic, model = step(data, rhs, false, bestbic, model, verbose, debug)
            if !improvedfwd && !improvedbwd
                sort!(rhs)
                verbose && println("final rhs: $rhs")
                return model
            end
        end
    end

end

using .Stepwise
export stepwise


# This file is a part of AstroLib.jl. License is MIT "Expat".
# Copyright (C) 2016 Mosè Giordano.

function _posang{T<:AbstractFloat}(units::Integer, ra1::T, dec1::T, ra2::T, dec2::T)
    # Convert all quantities to radians.
    if units == 0
        # All radians
        ra1_rad  = ra1
        ra2_rad  = ra2
        dec1_rad = dec1
        dec2_rad = dec2
    elseif units == 1
        # Right ascensions are in hours, declinations in degrees.
        ra1_rad  = ra1*pi/12.0
        ra2_rad  = ra2*pi/12.0
        dec1_rad = deg2rad(dec1)
        dec2_rad = deg2rad(dec2)
    elseif units == 2
        # Right ascensions and declinations are in degrees.
        ra1_rad  = deg2rad(ra1)
        ra2_rad  = deg2rad(ra2)
        dec1_rad = deg2rad(dec1)
        dec2_rad = deg2rad(dec2)
    else
        # In any other case throw an error.
        error("units must be 0 (radians), 1 (hours, degrees) or 2 (degrees)")
    end
    radif = ra2_rad - ra1_rad
    angle = atan2(sin(radif), cos(dec1_rad)*tan(dec2_rad) -
                  sin(dec1_rad)*cos(radif))
    if units == 0
        return angle
    else
        return rad2deg(angle)
    end
end

"""
    posang(units, ra1, dec1, ra2, dec2) -> angular_distance

### Purpose ###

Compute rigorous position angle of point 2 relative to point 1.

### Explanation ###

Computes the rigorous position angle of point 2 (with given right ascension and
declination) using point 1 (with given right ascension and declination) as the
center.

### Arguments ###

* `units`: integer, can be either 0, or 1, or 2.  Describes units of inputs and
 output:
    * 0: everything (input right ascensions and declinations, and output
      distance) is radians
    * 1: right ascensions are in decimal hours, declinations in decimal degrees,
      output distance in degrees
    * 2: right ascensions and declinations are in degrees, output distance in
      degrees
* `ra1`:  right ascension or longitude of point 1
* `dec1`: declination or latitude of point 1
* `ra2`: right ascension or longitude of point 2
* `dec2`: declination or latitude of point 2

Both `ra1` and `dec1`, and `ra2` and `dec2` can be given as 2-tuples `(ra1,
dec1)` and `(ra2, dec2)`.

### Output ###

Angle of the great circle containing `[ra2, dec2]` from the meridian containing
`[ra1, dec1]`, in the sense north through east rotating about `[ra1, dec1]`.
See `units` argument above for units.

### Method ###

The "four-parts formula" from spherical trigonometry (p. 12 of Smart's Spherical
Astronomy or p. 12 of Green' Spherical Astronomy).

### Example ###

Mizar has coordinates (ra, dec) = (13h 23m 55.5s, +54° 55' 31'').  Its
companion, Alcor, has coordinates (ra, dec) = (13h 25m 13.5s, +54° 59' 17'').
Find the position angle of Alcor with respect to Mizar.

``` julia
posang(1, ten(13, 25, 13.5), ten(54, 59, 17), ten(13, 23, 55.5), ten(54, 55, 31))
# => -108.46011246802047
```

### Notes ###

* If `ra1`, `dec1` are scalars, and `ra2`, `dec2` are vectors, then the output
 is a vector giving the distance of each element of `ra2`, `dec2` to `ra1`,
 `dec1`.  Similarly, if `ra1`,`de1` are vectors, and `ra2`,` dec2` are scalars,
 then the output is a vector giving the distance of each element of `ra1`,
 `dec1` to `ra2`, `dec2`.  If both `ra1`, `dec1` and `ra2`, `dec2` are vectors
 then the output is a vector giving the distance of each element of `ra1`,
 `dec1` to the corresponding element of `ra2`, `dec2`.
* The function `sphdist` provides an alternate method of computing a spherical
 distance.
* Note that `posang` is not commutative: the position angle between A and B is
  \$\\theta\$, then the position angle between B and A is \$180 + \\theta\$.

Code of this function is based on IDL Astronomy User's Library.
"""
posang(units::Integer, ra1::Real, dec1::Real, ra2::Real, dec2::Real) =
    _posang(units, promote(float(ra1), float(dec1), float(ra2), float(dec2))...)

function posang{R1<:Real, D1<:Real}(units::Integer,
                                   ra1::AbstractArray{R1},
                                   dec1::AbstractArray{D1},
                                   ra2::Real,
                                   dec2::Real)
    @assert length(ra1) == length(dec1)
    dist = similar(ra1, typeof(float(one(R1))))
    for i in eachindex(ra1)
        dist[i] = posang(units, ra1[i], dec1[i], ra2, dec2)
    end
    return dist
end

function posang{R2<:Real, D2<:Real}(units::Integer,
                                   ra1::Real,
                                   dec1::Real,
                                   ra2::AbstractArray{R2},
                                   dec2::AbstractArray{D2})
    @assert length(ra2) == length(dec2)
    dist = similar(ra2, typeof(float(one(R2))))
    for i in eachindex(ra2)
        dist[i] = posang(units, ra1, dec1, ra2[i], dec2[i])
    end
    return dist
end

function posang{R1<:Real, D1<:Real, R2<:Real, D2<:Real}(units::Integer,
                                                       ra1::AbstractArray{R1},
                                                       dec1::AbstractArray{D1},
                                                       ra2::AbstractArray{R2},
                                                       dec2::AbstractArray{D2})
    @assert length(ra1) == length(dec1) == length(ra2) == length(dec2)
    dist = similar(ra1, typeof(float(one(R1))))
    for i in eachindex(ra1)
        dist[i] = posang(units, ra1[i], dec1[i], ra2[i], dec2[i])
    end
    return dist
end

### Tuples input
posang(units::Integer, radec1::Tuple{Real, Real}, ra2::Real, dec2::Real) =
    posang(units, radec1..., ra2, dec2)

posang(units::Integer, ra1::Real, dec1::Real, radec2::Tuple{Real, Real}) =
    posang(units, ra1, dec1, radec2...)

posang(units::Integer, radec1::Tuple{Real, Real}, radec2::Tuple{Real, Real}) =
    posang(units, radec1..., radec2...)


@doc raw"""
    Yen <: AbstractImageBinarizationAlgorithm
    Yen()

    binarize([T,] img, f::Yen)
    binarize!([out,] img, f::Yen)

Computes the binarization threshold value using Yen's maximum correlation criterion for
bilevel thresholding.

# Output

Return the binarized image as an `Array{Gray{T}}` of size `size(img)`. If
`T` is not specified, it is inferred from `out` and `img`.

# Details

This algorithm uses the concept of *entropic correlation* of a gray level histogram to produce a threshold
value.

Let ``f_1, f_2, \ldots, f_I`` be the frequencies in the various bins of the
histogram and ``I`` the number of bins. With ``N = \sum_{i=1}^{I}f_i``, let
``p_i = \frac{f_i}{N}`` (``i = 1, \ldots, I``) denote the probability
distribution of gray levels. From this distribution one derives two additional
distributions. The first defined for discrete values ``1`` to ``s`` and the
other, from ``s+1`` to ``I``. These distributions are

```math
A: \frac{p_1}{P_s}, \frac{p_2}{P_s}, \ldots, \frac{p_s}{P_s}
\quad \text{and} \quad
B: \frac{p_{s+1}}{1-P_s}, \ldots, \frac{p_n}{1-P_s}
\quad \text{where} \quad
P_s = \sum_{i=1}^{s}p_i.
```
The entropic correlations associated with each distribution are

```math
C(A) = -\ln \sum_{i=1}^{s} \left( \frac{p_i}{P_s} \right)^2 \quad \text{and} \quad C(B) = -\ln \sum_{i=s+1}^{I} \left( \frac{p_i}{1 - P_s} \right)^2.
```

Combining these two entropic correlation functions we have

```math
\psi(s) = -\ln \sum_{i=1}^{s} \left( \frac{p_i}{P_s} \right)^2 -\ln \sum_{i=s+1}^{I} \left( \frac{p_i}{1 - P_s} \right)^2.
```
Finding the discrete value ``s`` which maximises the function ``\psi(s)`` produces
the sought-after threshold value (i.e. the bin which determines the threshold).

# Arguments

The function argument is described in more detail below.

##  `img::AbstractArray`

The image that needs to be binarized. The image is automatically converted
to `Gray` in order to construct the requisite graylevel histogram.

# Example

Binarize the "cameraman" image in the `TestImages` package.

```julia
using TestImages, ImageBinarization

img = testimage("cameraman")
img_binary = binarize(img, Yen())
```

# Reference

1. Yen JC, Chang FJ, Chang S (1995), “A New Criterion for Automatic Multilevel Thresholding”, IEEE Trans. on Image Processing 4 (3): 370-378, [doi:10.1109/83.366472](https://doi.org/10.1109/83.366472)
"""
struct Yen <: AbstractImageBinarizationAlgorithm end

function (f::Yen)(out::GenericGrayImage, img::GenericGrayImage)
    edges, counts = build_histogram(img,  256)
    t = find_threshold(HistogramThresholding.Yen(), counts[1:end], edges)
    @simd for i in CartesianIndices(img)
        out[i] = img[i] < t ? 0 : 1
    end
    out
end

(f::Yen)(out::GenericGrayImage, img::AbstractArray{<:Color3}) =
    f(out, of_eltype(Gray, img))


#=

Author: Benjamin Hemingway

This code adjusts the square matrix in the case it is not positive semi definite.

This code is based on the Matlab package nearestSPD version 1.1.0.0 by John D'Errico
=#
function spdMatrix(M::Array{T,2}) where T<:Real
  # symmetrize A into B
  B = (M + M')/2;
  # Compute the symmetric polar factor of B. Call it H.
  # Clearly H is itself SPD.
  out = LinearAlgebra.svd(B)
  H = out.Vt'*Diagonal(out.S)*out.Vt
  # get Ahat in the above formula
  Ahat = (B+H)/2
  # ensure symmetry
  Ahat = (Ahat + Ahat')/2;
  # test that Ahat is in fact PD. if it is not so, then tweak it just a bit.
  p = isposdef(Ahat)
  k = 0;
  while p==false
    k+=1
    # Ahat failed the chol test. It must have been just a hair off,
    # due to floating point trash, so it is simplest now just to
    # tweak by adding a tiny multiple of an identity matrix.
    mineig = minimum(eigvals(Ahat))
    Ahat += (-mineig*k.^2 + eps(mineig))*Matrix{Float64}(I,size(Ahat))
    p = isposdef(Ahat)
    if k>99
      error("Could not create Positive Definite Matrix")
    end
  end
  return Ahat
end


function sphere_vertex(x, y, z, scale)
    length = sqrt(x^2 + y^2 + z^2)
    return [(i * scale) / length for i in (x, y, z)]
end
function sphere_vertex(a::Vector{T}, scale) where T
    @assert length(a) == 3
    x, y, z = a[1], a[2], a[3]
    return sphere_vertex(x, y, z, scale)
end

function middle_point!(point_1, point_2, middle_point_cache, verts, scale)
    # We check if we have already cut this edge first
    # to avoid duplicated verts
    smaller_index = min(point_1, point_2)
    greater_index = max(point_1, point_2)
    key = "$smaller_index, $greater_index"
    if haskey(middle_point_cache, key)
        return middle_point_cache[key]
    end
    vert_1 = verts[point_1]
    vert_2 = verts[point_2]
    middle = [sum(i) / 2 for i in zip(vert_1, vert_2)]
    append!(verts, [sphere_vertex(middle, scale)])
    index = length(verts)
    middle_point_cache[key] = index
    return index
end # function

"""
Create icosphere
"""
function icosphere(subdiv = 3, scale = 1.0)
    PHI = (1. + sqrt(5.)) / 2.
    verts = [(sphere_vertex(-1., PHI, 0., scale)),
        (sphere_vertex(1., PHI, 0., scale)),
        (sphere_vertex(-1., -PHI, 0., scale)),
        (sphere_vertex(1., -PHI, 0., scale)),
        (sphere_vertex(0., -1., PHI, scale)),
        (sphere_vertex(0., 1., PHI, scale)),
        (sphere_vertex(0., -1., -PHI, scale)),
        (sphere_vertex(0., 1., -PHI, scale)),
        (sphere_vertex(PHI, 0., -1., scale)),
        (sphere_vertex(PHI, 0., 1., scale)),
        (sphere_vertex(-PHI, 0., -1., scale)),
        (sphere_vertex(-PHI, 0., 1., scale))]
    faces = [[1, 12, 6], # 5 faces around point 0
            [1, 6, 2],
            [1, 2, 8],
            [1, 8, 11],
            [1, 11, 12],
            [2, 6, 10], # Adjacent faces
            [6, 12, 5],
            [12, 11, 3],
            [11, 8, 7],
            [8, 2, 9],
            [4, 10, 5],  # 5 faces around 3
            [4, 5, 3],
            [4, 3, 7],
            [4, 7, 9],
            [4, 9, 10],
            [5, 10, 6],# Adjacent faces
            [3, 5, 12],
            [7, 3, 11],
            [9, 7, 8],
            [10, 9, 2]]

    middle_point_cache = Dict{String,Int}()
    for i in 1:subdiv
        faces_subdiv = Array{Int64,1}[]
        for tri in faces
            v1 = middle_point!(tri[1], tri[2], middle_point_cache, verts, scale)
            v2 = middle_point!(tri[2], tri[3], middle_point_cache, verts, scale)
            v3 = middle_point!(tri[3], tri[1], middle_point_cache, verts, scale)
            append!(faces_subdiv, [[tri[1], v1, v3]])
            append!(faces_subdiv, [[tri[2], v2, v1]])
            append!(faces_subdiv, [[tri[3], v3, v2]])
            append!(faces_subdiv, [[v1, v2, v3]])
            faces = faces_subdiv
        end
    end
    v = Vector{Point3f0}(verts)
    f = Vector{TriangleFace{Int}}(faces)
    sphere = normal_mesh(Mesh(v, f))
    return sphere
end


"""
    SomeVertexInCircumsphere(simplex1, r2, c2)

Decides whether some vertices of simplex1 lies inside the circumsphere of simplex2
(defined by the radius and centroid of simplex1, r2 and c2). Each column of simplex1 is a vertex.

Arguments
---------
c2::Vector{Float64} Column vector. Centroid of simplex2.
r2::Float64 Radius of simplex 2

Returns either 0 or 1; 1 if some of the vertices of simplex1 are contained in the circumsphere
of simplex 2, 0 otherwise.
"""
function SomeVertexInCircumsphere(simplex1, r2, c2)
    # The dimension
    n = size(simplex1, 1)

    i = 1
    some_vertex_in_circumsphere = false

    while i <= (n + 1) && !some_vertex_in_circumsphere
        # Difference between the i-th vertex of simplex1 and centroid of simplex2
        ith_vertex = simplex1[:, i] - c2
        tmp = heaviside0(r2^2 - transpose(ith_vertex) * ith_vertex) # Radius times norm

        if tmp == 1
            some_vertex_in_circumsphere = true
        end

        i = i + 1
    end

    return some_vertex_in_circumsphere
end


## Class shape transformation method
#==========================================================================================#

# Basic shape function
function shape_function(x, basis_func, coeffs, coeff_LE = 0)
    n     = length(coeffs)
    terms = basis_func.(x, n - 1, 0:n-1)
    dot(coeffs, terms) + coeff_LE * (x^0.5) * (1 - x)^(n - 0.5)
end

# Computing coordinates
CST_coordinates(class_func, basis_func, x, alphas, dz, coeff_LE, args...) = class_func(x) * shape_function(x, basis_func, alphas, coeff_LE) + x * dz

## Bernstein basis
#==========================================================================================#

bernstein_class(x, N1, N2) = x^N1 * (1 - x)^N2
bernstein_basis(x, n, k)   = binomial(n, k) * bernstein_class(x, k, n - k)

"""
    kulfan_CST(alpha_u, alpha_l,
               (Δz_u, Δz_l) = (0., 0.),
               (LE_u, LE_l) = (0., 0.),
               n            = 40)

Define a cosine-spaced foil with ``2n`` points using the Class Shape Transformation method on a Bernstein polynomial basis for the upper and lower coordinates.

The foil is defined by arrays of coefficients ``(α_u,~ α_l)`` for the upper and lower surfaces (not necessarily of the same lengths), trailing-edge displacement values ``(Δz_u,~ Δz_l)``, and coefficients for leading edge modifications on the upper and lower surfaces at the nose.
"""
function kulfan_CST(alpha_u, alpha_l, (dz_u, dz_l) = (0., 0.), (LE_u, LE_l) = (0., 0.), n :: Integer = 40, N1 = 0.5, N2 = 1.)
    # Cosine spacing for airfoil of unit chord length
    xs = cosine_spacing(0.5, 1, n)

    # λ-function for Bernstein polynomials
    bernie(x, alphas, dz, LE) = CST_coordinates(y -> bernstein_class(y, N1, N2), bernstein_basis, x, alphas, dz, LE)

    # Upper and lower surface generation
    upper_surf = [ bernie(x, alpha_u, dz_u, LE_u) for x ∈ xs ]
    lower_surf = [ bernie(x, alpha_l, dz_l, LE_l) for x ∈ xs ]

    # Counter-clockwise ordering
    @views Foil([ xs[end:-1:2] upper_surf[end:-1:2] ;
                  xs           lower_surf           ], "Kulfan CST")
end

"""
    camber_CST(α_c, α_t,
               (Δz_u, Δz_l) :: NTuple{2, Real},
               coeff_LE = 0.,
               n :: Integer = 40)

Define a cosine-spaced foil with ``2n`` points using the Class Shape Transformation method on a Bernstein polynomial basis for the camber and thickness coordinates.

The foil is defined by arrays of coefficients ``(α_c,~ α_t)`` for the upper and lower surfaces, trailing-edge spacing values ``(Δz_u,~Δz_l)``, and a coefficient for the leading edge modifications at the nose.
"""
function camber_CST(α_cam, α_thicc, dz_thicc = 0., coeff_LE = 0, n :: Integer = 40, N1 = 0.5, N2 = 1.)
    # Cosine spacing for airfoil of unit chord length
    xs = cosine_spacing(0.5, 1, n)

    # λ-function for Bernstein polynomials
    bernie(x, αs, dz = 0.) = CST_coordinates(y -> bernstein_class(y, N1, N2), bernstein_basis, x, αs, dz, coeff_LE)

    # Upper and lower surface generation
    cam   = [ bernie(x, α_cam) for x ∈ xs ]
    thicc = [ bernie(x, α_thicc, dz_thicc) for x ∈ xs ]

    Foil(camber_thickness_to_coordinates(xs, cam, thicc), "Camber-Thickness CST")
end

"""
    coordinates_to_CST(coords, num_dvs)

Convert coordinates to a specified number of CST variables by performing a least-squares solution.
"""
function coordinates_to_CST(coords, num_dvs)
    xs       = @views coords[:,1]
    S_matrix = reduce(hcat, @. bernstein_class(xs, 0.5, 1.0) * bernstein_basis(xs, num_dvs - 1, i) for i in 0:num_dvs - 1)
    alphas   = @views S_matrix \ coords[:,2]
end

"""
    camber_thickness_to_CST(coords, num_dvs)

Convert camber-thickness coordinates to a specified number of CST variables by performing a least-squares solution.
"""
function camber_thickness_to_CST(coords, num_dvs)
    xs, camber, thickness = (columns ∘ coordinates_to_camber_thickness)(coords)

    alpha_cam   = coordinates_to_CST([ xs camber ], num_dvs)
    alpha_thick = coordinates_to_CST([ xs thickness ], num_dvs)

    alpha_cam, alpha_thick
end