"""
    monte_carlo_return

Perform a monte_carlo_return using the provided environment, gvf, and start_state. For an environment to be compatible it must
implement `reset!(env, state)`

The interface is limited in that it can't do rollouts on compositional GVFs.
"""
function monte_carlo_return(env,
                            gvf,
                            start_state,
                            num_returns,
                            γ_thresh=1e-6,
                            max_steps=Int(1e6),
                            rng=Random.GLOBAL_RNG)

    returns = zeros(num_returns)

    for ret in 1:num_returns
        step = 0
        term = false
        cumulative_gamma = 1.0

        cur_state = start!(env, cur_state)
        next_action = StatsBase.sample(rng, policy(gvf), cur_state)

        while cumulative_gamma > γ_thresh &&
            step < max_steps &&
            term == false

            # Take action
            action = next_action
            next_state, r, term = step!(env, cur_state, action; rng=rng)

            # Get next action for GVFs
            next_action = StatsBase.sample(rng, policy(gvf), cur_state)

            # Update Return
            c, γ, pi_prob = get(gvf, cur_state, action, next_state, next_action, nothing)
            returns[ret] += cumulative_gamma*(1 - term)*c
            cumulative_gamma *= γ

            cur_state = next_state
            step += 1
        end
    end

    return returns
end

monte_carlo_returns(env, gvf, state_states, num_returns, γ_thresh, max_steps=Int(1e6), rng=Random.GLOBAL_RNG) =
    [monte_carlo_return(env, gvf, st, num_returns, γ_thres, max_steps=Int(1e6), rng=Random.GLOBAL_RNG) for st in start_states]


include("header.jl")

@testset "getindex" begin

    for T in (Float32, Float64)
        @testset "Array" begin
            a = rand(T,3,3)
            for i in (:, 1, 1:2, 1:2:3, [1,2], [2,2], [])
                #@show i
                @test gradcheck(getindex, a, i; args=1)
                for j in (:, 1, 1:2, 1:2:3, [1,2], [2,2], [])
                    #@show i,j
                    @test gradcheck(getindex, a, i, j; args=1)
                end
            end
            j = [true,false,true]
            for i in (:, 1, 1:2, 1:2:3, [1,2], [2,2], [])
                #@show i,j
                @test gradcheck(getindex, a, i, j; args=1)
                #@show j,i
                @test gradcheck(getindex, a, j, i; args=1)
            end
            @test gradcheck(getindex, a, [1 2;1 2]; args=1)
            @test gradcheck(getindex, a, a.>0.5; args=1)
            @test gradcheck(getindex, a, CartesianIndex(1,2); args=1)
        end

        @testset "Tuple" begin
            a = Any[rand(T,3)...]
            t = tuple(a...)
            f(a,i)=(ai=a[i];s=0;for j=1:length(ai); s+=j*ai[j]; end; s)
            g = grad(f)
            for i in (1,1:2,1:2:3,[1,2],[2,2],[],[true,false,true])
                #@show i
                @test gradcheck(getindex, t, i; args=1)
                @test g(t,i)==g(a,i)==nothing || g(t,i) == tuple(g(a,i)...)
            end
        end

        @testset "Dict" begin
            g = grad(getindex)
            d = Dict(1=>rand(T), 2=>rand(T), 3=> rand(T))
            #@show d
            @test gradcheck(getindex, d, 1; args=1)
            @test collect(g(d,2)) == Any[(2=>1.0)]
        end

        @testset "size" begin
            #@show size
            f0(x)=(p=size(x); p[1]*sum(abs2,x))
            @test grad(f0)(ones(3)) == fill(6, 3)

            f1(x)=(p=size(x, 1); p*sum(abs2,x))
            @test grad(f1)(ones(3, 3)) == fill(6, 3, 3)

            # Issue #18: ambiguity error in size(rec, dims...)
            f2(x)=(p=(size(x, 1), size(x, 2)); p[1]*sum(abs2,x))
            @test grad(f2)(ones(3, 3)) == fill(6, 3, 3)
        end

        @testset "indexing" begin

            # info("Test indexing...")

            a1 = rand(T,3)
            t1 = (a1...,)
            d1 = Dict(); for i=1:length(a1); d1[i]=a1[i]; end

            s0(x)=x[1]^2+x[2]^2
            s1 = grad(s0)
            s1sum(x)=(y=s1(x);y[1]+y[2])
            s2 = grad(s1sum)

            @test gradcheck(s0,a1)
            @test gradcheck(s0,t1)
            @test gradcheck(s0,d1)
            @test gradcheck(s1sum,a1)
            @test gradcheck(s1sum,t1)
            @test gradcheck(s1sum,d1)

            f0(x)=(a=0;for i=1:length(x);a+=x[i]^2;end;a)
            f1=grad(f0)
            f1sum(x)=sum(values(f1(x)))
            f2=grad(f1sum)
            @test gradcheck(f0,a1)
            @test gradcheck(f0,t1)
            @test gradcheck(f0,d1)
            @test gradcheck(f1sum,a1)
            @test gradcheck(f1sum,t1)
            @test gradcheck(f1sum,d1)

            r0(x)=(s=0; for i=2:length(x); s+=(1-x[i-1])^2 + 2*(x[i]-x[i-1]^2)^2; end; s)
            r1 = grad(r0)
            r1sum(x)=sum(values(r1(x)))
            r2 = grad(r1sum)
            @test gradcheck(r0,a1)
            @test gradcheck(r0,t1)
            @test gradcheck(r0,d1)
            @test gradcheck(r1sum,a1)
            @test gradcheck(r1sum,t1)
            @test gradcheck(r1sum,d1)

        end

        @testset "view" begin
            a = rand(T,3,3)
            g1 = grad(x->sum(x[1:2,1:2].^2.5))
            g2 = grad(x->sum(view(x,1:2,1:2).^2.5))
            @test g1(a) == g2(a)

            g1 = grad(x->sum(x[:,1:2].^2.0))
            g2 = grad(x->sum(view(x,:,1:2).^2.0))
            g3 = grad(x->sum(selectdim(x,2,1:2).^2.0))
            @test g1(a) == g2(a) == g3(a)
        end
    end

    # Issue #73: incorrect gradient when indexing into a matrix of vectors
    a = Matrix{Array{Float32}}(undef, 2, 1)
    a[1] = [2,3,4]
    a[2] = [4,5,6]
    g1 = grad(x->x[1]' * x[2])
    g2 = grad(x->x[1, 1]' * x[2, 1])
    @test g1(a) == g2(a)

end

nothing


function inser-sort!(A)
    if length(A) < 1 # check if A is empty
        return "A < 1"
    end
    for i in 2:length(A) # Julia uses 1 based indexing, no point at starting at 1 wchich has 1 element
        key = A[i] # Set key equal to the element we're currently checking
        j = i-1 # j will be the index checking the elements before i
        while j > 0 && key < A[j] # loop until value is less than the current element
            A[j+1] = A[j]
            j = j - 1
        end
        A[j+1] = key # set equal to the last index that was bigger than current
    end
end

A = [0,1,2,4,2]
#test push


# # Global stress objective example
#-
#md # !!! tip
#md #     This example is also available as a Jupyter notebook:
#md #     [`global_stress.ipynb`](@__NBVIEWER_ROOT_URL__/examples/global_stress.ipynb)
#-
# ## Commented Program
#
# What follows is a program spliced with comments.
#md # The full program, without comments, can be found in the next [section](@ref global-stress-plain-program).

using TopOpt, LinearAlgebra

# ### Define the problem
E = 1.0 # Young’s modulus
v = 0.3 # Poisson’s ratio
f = 1.0 # downward force
rmin = 3.0 # filter radius

problems = Any[
    PointLoadCantilever(Val{:Linear}, (60, 20), (1.0, 1.0), E, v, f),
    HalfMBB(Val{:Linear}, (60, 20), (1.0, 1.0), E, v, f),
]
problem_names = [
    "Cantilever beam",
    "Half MBB beam",
    "L-beam",
    "Tie-beam",
]

i = 1
println(problem_names[i])
problem = problems[i]

# ### Parameter settings
V = 0.5 # volume fraction
xmin = 0.001 # minimum density
steps = 40 # maximum number of penalty steps, delta_p0 = 0.1
convcriteria = Nonconvex.KKTCriteria()
penalty = TopOpt.PowerPenalty(1.0)

# ### Define a finite element solver
solver = FEASolver(
    Displacement, Direct, problem, xmin = xmin, penalty = penalty,
)

# ### Define **stress** objective
# Notice that gradient is derived automatically by automatic differentiation (Zygote.jl)!
stress = TopOpt.MicroVonMisesStress(solver)
filter = if problem isa TopOptProblems.TieBeam
    identity
else
    DensityFilter(solver, rmin = rmin)
end
volfrac = TopOpt.Volume(problem, solver)

obj = x -> volfrac(filter(x))
constr = x -> norm(stress(filter(x)), 5) - 1.0
options = Nonconvex.MMAOptions(
    maxiter=2000, tol = Nonconvex.Tolerance(kkt = 1e-4),
)

# ### Define subproblem optimizer
x0 = fill(1.0, length(solver.vars))
optimizer = Optimizer(
    obj, constr, x0, Nonconvex.MMA87(),
    options = options, convcriteria = convcriteria,
)

# ### Define continuation SIMP optimizer
simp = SIMP(optimizer, solver, 3.0)

# ### Solve
result = simp(x0)

@show result.convstate
@show optimizer.workspace.iter
@show result.objval

# ### (Optional) Visualize the result using Makie.jl
# Need to run `using Pkg; Pkg.add(Makie)` first
# ```julia
# using TopOpt.TopOptProblems.Visualization: visualize
# fig = visualize(problem; topology = result.topology)
# Makie.display(fig)
# ```

#md # ## [Plain Program](@id global-stress-plain-program)
#md #
#md # Below follows a version of the program without any comments.
#md # The file is also available here: [global-stress.jl](global_stress.jl)
#md #
#md # ```julia
#md # @__CODE__
#md # ```


#=--------------------------------------------------------------------

utilities - Various utility functions for computer viison

Copyright (c) 2015-2017 Peter Kovesi
peterkovesi.com

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.

PK April 2016 Initial version
   April 2017 Updates
   September 2018 update for v0.7/v1.0

---------------------------------------------------------------------=#

export derivative3, derivative5, derivative7
export nonmaxsuppts
export dilate1d, erode1d, imdilate, imerode, circularstruct
export gaussfilt
export imgnormalise, imgnormalize, histtruncate
export floatyx
export matchbycorrelation
export medfilt2, stdfilt2
export grey2census, grey2lbp, grey2lbp!
export keypause

using LinearAlgebra, Statistics
import DSP, Images, ImageFiltering, PyPlot

#----------------------------------------------------------------------
"""
derivative3 - 3-Tap discrete derivative filters

This function computes 1st derivatives of an image using the 3-tap
coefficients given by Farid and Simoncelli.  The results are significantly
more accurate than simple gradient functions on edges that are at angles
other than vertical or horizontal. This, in turn, improves gradient orientation
estimation enormously.  If you are after extreme accuracy try using derivative7().
```
Usage:  (gx, gy) = derivative3(img, derivative specifiers)

Arguments:
                     img - Image to compute derivatives from. ::Array{T,2}
   derivative specifiers - A tuple of character strings
                           that can be any of "x" or "y"
                           These can be in any order, the order of the
                           computed output arguments will match the order
                           of the derivative specifier strings.
Returns:
 Function returns requested derivatives which can be:
    gx, gy   - 1st derivative in x and y

 Example:
   Compute 1st derivatives in x and y
   (gx, gy) = derivative3(img, ("x", "y"))
```
See also: derivative5(), derivative7()
"""
function derivative3(img::Array{T,2}, spec) where T <: Real
    # Reference: Hany Farid and Eero Simoncelli "Differentiation of Discrete
    # Multi-Dimensional Signals" IEEE Trans. Image Processing. 13(4): 496-508 (2004)

    # Convert to float if needed
    if isa(img[1,1], Integer)
        fimg = float(img)
    else
        fimg = img
    end

    # 3-Tap filters
    p = [0.229879,  0.540242,  0.229879]

    # ?? Coefficients seem too small added the division...??
    d1 =[0.425287,  0.000000, -0.425287]/0.8506

    # Compute derivatives.  Note that in the 1st call below conv
    # function performs a 1D convolution down the columns using p then a 1D
    # convolution along the rows using d1. etc etc.
    G = Array{Array}(undef, length(spec))

    for n = 1:length(spec)
      if spec[n] == "x"
          G[n] = DSP.conv(p, d1, fimg)[2:end-1, 2:end-1]
      elseif spec[n] == "y"
          G[n] = DSP.conv(d1, p, fimg)[2:end-1, 2:end-1]
      else
          badspec = spec[n]
          error(" $badspec is an unrecognized derivative option")
      end
    end

    return tuple(G...)
end

#----------------------------------------------------------------------
"""
derivative5 - 5-Tap 1st and 2nd discrete derivatives

This function computes 1st and 2nd derivatives of an image using the 5-tap
coefficients given by Farid and Simoncelli.  The results are significantly
more accurate than simple gradient functions on edges that are at angles
other than vertical or horizontal. This, in turn, improves gradient orientation
estimation enormously.  If you are after extreme accuracy try using derivative7().
```
Usage:  (gx, gy, gxx, gyy, gxy) = derivative5(img, derivative_specifiers)

Arguments:
                     img - Image to compute derivatives from. ::Array{T,2}
   derivative specifiers - A tuple of character strings
                           that can be any of "x", "y", "xx", "yy" or "xy"
                           These can be in any order, the order of the
                           computed output arguments will match the order
                           of the derivative specifier strings.
Returns:
 Function returns requested derivatives which can be:
    gx, gy   - 1st derivative in x and y
    gxx, gyy - 2nd derivative in x and y
    gxy      - 1st derivative in y of 1st derivative in x

 Examples:
   Just compute 1st derivatives in x and y
   (gx, gy) = derivative5(img, ("x", "y"))

   Compute 2nd derivative in x, 1st derivative in y and 2nd derivative in y
   (gxx, gy, gyy) = derivative5(img, ("xx", "y", "yy"))
```
See also: derivative7()
"""
function derivative5(img::Array{T,2}, spec) where T <: Real
    # Reference: Hany Farid and Eero Simoncelli "Differentiation of Discrete
    # Multi-Dimensional Signals" IEEE Trans. Image Processing. 13(4): 496-508 (2004)

    # Convert to float if needed
    if isa(img[1,1], Integer)
        fimg = float(img)
    else
        fimg = img
    end

    # Check if we are just computing 1st derivatives.  If so use the
    # interpolant and derivative filters optimized for 1st derivatives, else
    # use 2nd derivative filters and interpolant coefficients.
    # Detection is done by seeing if any of the derivative specifier
    # arguments is longer than 1 char, this implies 2nd derivative needed.
    secondDeriv = false
    for n = 1:length(spec)
        if length(spec[n]) > 1
            secondDeriv = true
            break
        end
    end

    if !secondDeriv
        # 5 tap 1st derivative cofficients.  These are optimal if you are just
        # seeking the 1st deriavtives
        p = [0.037659,  0.249153,  0.426375,  0.249153,  0.037659]
        d1 =[0.109604,  0.276691,  0.000000, -0.276691, -0.109604]
    else
        # 5-tap 2nd derivative coefficients. The associated 1st derivative
        # coefficients are not quite as optimal as the ones above but are
        # consistent with the 2nd derivative interpolator p and thus are
        # appropriate to use if you are after both 1st and 2nd derivatives.
        p  = [0.030320,  0.249724,  0.439911,  0.249724,  0.030320]
        d1 = [0.104550,  0.292315,  0.000000, -0.292315, -0.104550]
        d2 = [0.232905,  0.002668, -0.471147,  0.002668,  0.232905]
    end

    # Compute derivatives.  Note that in the 1st call below conv
    # function performs a 1D convolution down the columns using p then
    # a 1D convolution along the rows using d1. etc etc.
    # Note that the result has to be trimmed becasue conv expands the
    # result due to size of the filter
    G = Array{Array}(undef,length(spec))

    for n = 1:length(spec)
      if spec[n] == "x"
          G[n] = DSP.conv(p, d1, fimg)[3:end-2, 3:end-2]
      elseif spec[n] == "y"
          G[n] = DSP.conv(d1, p, fimg)[3:end-2, 3:end-2]
      elseif spec[n] == "xx"
          G[n] = DSP.conv(p, d2, fimg)[3:end-2, 3:end-2]
      elseif spec[n] == "yy"
          G[n] = DSP.conv(d2, p, fimg)[3:end-2, 3:end-2]
      elseif spec[n] == "xy" || spec[n] == "yx"
          G[n] = DSP.conv(d1, d1, fimg)[3:end-2, 3:end-2]
      else
          badspec = spec[n]
          error(" $badspec is an unrecognized derivative option")
      end
    end

    return tuple(G...)
end

#----------------------------------------------------------------------
"""
derivative7 - 7-Tap 1st and 2nd discrete derivatives

This function computes 1st and 2nd derivatives of an image using the 7-tap
coefficients given by Farid and Simoncelli.  The results are significantly
more accurate than simple gradient functions on edges that are at angles
other than vertical or horizontal. This, in turn, improves gradient orientation
estimation enormously.
```
Usage:  (gx, gy, gxx, gyy, gxy) = derivative7(img, derivative specifiers)

Arguments:
                      im - Image to compute derivatives from. ::Array{T,2}
   derivative specifiers - A tuple of character strings
                           that can be any of "x", "y", "xx", "yy" or "xy"
                           These can be in any order, the order of the
                           computed output arguments will match the order
                           of the derivative specifier strings.
Returns:
 Function returns requested derivatives which can be:
    gx, gy   - 1st derivative in x and y
    gxx, gyy - 2nd derivative in x and y
    gxy      - 1st derivative in y of 1st derivative in x

 Examples:
   Just compute 1st derivatives in x and y
   (gx, gy) = derivative7(img, ("x", "y"))

   Compute 2nd derivative in x, 1st derivative in y and 2nd derivative in y
   (gxx, gy, gyy) = derivative7(img, ("xx", "y", "yy"))
```
See also: derivative5()
"""
function derivative7(img::Array{T,2}, spec) where T <: Real
    # Reference: Hany Farid and Eero Simoncelli "Differentiation of Discrete
    # Multi-Dimensional Signals" IEEE Trans. Image Processing. 13(4): 496-508 (2004)

    # Convert to float if needed
    if isa(img[1,1], Integer)
        fimg = float(img)
    else
        fimg = img
    end

    # 7-tap interpolant and 1st and 2nd derivative coefficients
    p  = [ 0.004711,  0.069321,  0.245410,  0.361117,  0.245410,  0.069321,  0.004711]
    d1 = [ 0.018708,  0.125376,  0.193091,  0.000000, -0.193091, -0.125376, -0.018708]
    d2 = [ 0.055336,  0.137778, -0.056554, -0.273118, -0.056554,  0.137778,  0.055336]

    # Compute derivatives.  Note that in the 1st call below conv
    # function performs a 1D convolution down the columns using p then a 1D
    # convolution along the rows using d1. etc etc.
    # Note that the result has to be trimmed becasue conv expands the
    # result due to size of the filter
    G = Array{Array}(undef, length(spec))

    for n = 1:length(spec)
      if spec[n] == "x"
          G[n] = DSP.conv(p, d1, fimg)[4:end-3, 4:end-3]
      elseif spec[n] == "y"
          G[n] = DSP.conv(d1, p, fimg)[4:end-3, 4:end-3]
      elseif spec[n] == "xx"
          G[n] = DSP.conv(p, d2, fimg)[4:end-3, 4:end-3]
      elseif spec[n] == "yy"
          G[n] = DSP.conv(d2, p, fimg)[4:end-3, 4:end-3]
      elseif spec[n] == "xy" || spec[n] == "yx"
          G[n] = DSP.conv(d1, d1, fimg)[4:end-3, 4:end-3]
      else
          badspec = spec[n]
          error(" $badspec is an unrecognized derivative option")
      end
    end

    return tuple(G...)
end

#-------------------------------------------------------------------
"""
nonmaxsuppts - Non-maximal suppression for features/corners

Non maxima suppression and thresholding for points generated by a feature
or corner detector.
```
Usage:   (r,c) = nonmaxsuppts(cimg; radius=1, thresh=0, N=Inf, subpix=false, img=[], fig=nothing)

Argument:
           cimg   - corner strength image. ::Array{T,2}

Keyword Arguments:
           radius - radius of region considered in non-maximal
                    suppression. Typical values to use might
                    be 1-3 pixels. Default (and minimum value) is 1.
           thresh - Threshold, only features with value greater than
                    threshold are returned. Default is 0.
                N - Maximum number of features to return.  In this case the
                    N strongest features with value above 'thresh' are
                    returned. Default is Inf.
         subpixel - If set to true features are localised to subpixel
                    precision. Default is false.
             img  - Optional image data.  If an image is supplied the
                    detected corners are overlayed on this image. This can
                    be useful for parameter tuning. Default is [].
              fig - Optional figure number to display image and corner points.
                    If not specified a new figure is generated.

Returns:
           r,c    - row and column coordinates of corner points.
```

Example of use:
```
 > hes = hessianfeatures(img, 1)   # Compute Hessian feature image in image img
```
Find the 1000 strongest features to subpixel precision using a non-maximal
suppression radius of 2 and overlay the detected corners on the origin image.
```
 > (r,c) = nonmaxsuppts(abs(hes), radius=2, N=1000, img=img, subpixel=true)
```
Note: An issue with integer valued images is that if there are multiple pixels
all with the same value within distance 2*radius of each other then they will
all be marked as local maxima.

See also: harris(), noble(), shi_tomasi(), hessianfeatures()
"""
function nonmaxsuppts(cimg::Array{T,2}; radius::Real=1, thresh::Real=0,
                      N::Int=typemax(Int), subpixel::Bool=false, img=[], fig = nothing) where T <: Real

    #=
    September 2003  Original MATLAB version
    August    2015  Ported to Julia
    January   2016  Reworked to allow the N strongest features to be returned.
    =#

    if radius < 1
        error("Radius must be 1 or greater")
    end

    (rows,cols) = size(cimg)

    # Extract local maxima by performing a grey scale morphological
    # dilation and then finding points in the corner strength image that
    # match the dilated image and are also greater than the threshold.
#    mx = imdilate(cimg, circularstruct(radius))
    mx = imdilate(cimg, "OCT", radius) # Much faster

    # Make mask to exclude points within radius of the image boundary.
    bordermask = zeros(Bool, size(cimg))
    bordermask[radius+1:end-radius, radius+1:end-radius] .= true

    # Find maxima, threshold, and apply bordermask
    cimgmx = (cimg.==mx) .& (cimg.>thresh) .& bordermask

    # Get row, col coords of points.  The following is a clumsy replacement of
    # ind2sub.  It would be nice to make the function return an array of
    # CartesianIndicies however this would not allow maxima to be returned with
    # sub-pixel accuracy.
#    (r, c) = ind2sub(size(cimgmx),findall(cimgmx))
    ci = findall(cimgmx);  # CartesianIndices of maxima
    r = [ci[n][1] for n = 1:length(ci)]
    c = [ci[n][2] for n = 1:length(ci)]

    # Check if we want to limit the number of maxima
    if isfinite(N)
        mxval = cimg[cimgmx]       # Get values of maxima and sort them.
        ind = sortperm(mxval,rev=true) # Permutation order of sorted values

        r = r[ind]                 # Reorder r and c arrays
        c = c[ind]                 # to match.

        if length(r) > N           # Grab the N strongest features.
            r = r[1:N]
            c = c[1:N]
        end
    end

    if isempty(r)
        warn("No maxima above threshold found")
        r = zeros(Int,0)
        c = zeros(Int,0)
        return r, c
    end

    # If requested compute local maxima to sub pixel accuracy.  We fit
    # a 1D quadratic vertically and horizontally to the image data
    # using the pixels above and below and left and right of the
    # detected maxima and adjust the location of the maxima to match
    # maxima of the fitted quadartic.  Note that since the nonmaxima
    # radius is >= 1 we do not need to check whether we step outside
    # the image bounds when fitting the quadratics.
    if subpixel
        rsubpix = float(r)
        csubpix = float(c)

        for n = 1:length(r)
            # Solve for quadratic vertically
            cy = cimg[r[n],c[n]]
            ay = (cimg[r[n]-1,c[n]] + cimg[r[n]+1,c[n]])/2 - cy
            by = ay + cy - cimg[r[n]-1,c[n]]
            if abs(ay) > eps()
                rsubpix[n] += -by/(2*ay)  # Maxima of quadradic
            end

            # Solve for quadratic across columns
            cx = cimg[r[n],c[n]]
            ax = (cimg[r[n],c[n]-1] + cimg[r[n],c[n]+1])/2 - cx
            bx = ax + cx - cimg[r[n],c[n]-1]
            if abs(ax) > eps()
                csubpix[n] += -bx/(2*ax)  # Maxima of quadradic
            end

        end
    end

    # If an image has been supplied display it and overlay corners.
    if !isempty(img)
        PyPlot.figure(fig)
        PyPlot.clf()
        PyPlot.imshow(img)
        PyPlot.set_cmap(PyPlot.ColorMap("gray"))
        if subpixel
            PyPlot.plot(csubpix,rsubpix,"r+")
        else
            PyPlot.plot(c,r,"r+")
        end
        PyPlot.axis([1,cols,rows,1])
        PyPlot.title("Corners detected")
    end

    if subpixel
        return rsubpix, csubpix
    else
        return r, c
    end
end

#---------------------------------------------------------------------
"""
erode1d - 1D morphological erosion of a signal.
```
Usage:  ef = erode1d(f, k)

Arguments:  f - 1D array of values to be eroded
            k - Size of erosion kernel

Returns:   ef - Array of eroded values
```
Note that if the size of the structuring element is even then the centre pixel
is taken to be the integer pixel location to the right of the ideal.

This function uses Marcel van Herk's algorithm to run in linear time with
respect to the length of the signal, irrespective of the structing element
size.

See also: dilate1d(), imerode(), imdilate()
"""
function erode1d(f::Array{T,1}, ki::Integer) where T <: Real
    #=
    Reference: Marcel van Herk, "A fast algorithm for local minimum and maximum
    filters on rectangular and octagonal kernels".  Pattern Recognition Letters 13
    (1992) pp 517-521
    =#
    # Two separate functions here because I am having trouble forming the
    # function template using a parameterised Union as follows:
    # function erode1d{T<:Real}(f::Union{Array{T,1}, BitArray{1}}, ki::Real)

    return erode_dilate1d(f, ki, "ERODE")
end

function erode1d(f::BitArray{1}, ki::Integer)
    return erode_dilate1d(f, ki, "ERODE")
end

#---------------------------------------------------------------------
"""
dilate1d - 1D morphological dilation of a signal.
```
Usage:  df = dilate1d(f, k)

Arguments:  f - 1D array of values to be dilated
            k - Size of dilation kernel

Returns:   df - Array of dilated values
```
Note that if the size of the structuring element is even then the centre pixel
is taken to be the integer pixel location to the right of the ideal.

This function uses Marcel van Herk's algorithm to run in linear time with
respect to the length of the signal, irrespective of the structing element
size.

See also: erode1d(), imdilate(), imerode()
"""
function dilate1d(f::Array{T,1}, ki::Integer) where T <: Real
    return erode_dilate1d(f, ki, "DILATE")
end

function dilate1d(f::BitArray{1}, ki::Integer)
    return erode_dilate1d(f, ki, "DILATE")
end

#---------------------------------------------------------------------
"""
Unexported function that performs 1D erosion or dilation

```
Usage: df = erode_dilate1d(f, k::Integer, erode_dilate::String)

Arguments:
            f - Array of values to be eroded or dilated
            k - Size/length of 1D structuring element
 erode_dilate - "erode" or "dilate"

Returns:
          fd - The eroded/dilated array.
```

See also: erode_dilate1d!()

"""
function erode_dilate1d(f, k::Integer, erode_dilate::String)
#function erode_dilate1d{T<:Real}(f::Array{T,1}, ki::Real,
#                           erode_dilate::String)

    if uppercase(erode_dilate) == "ERODE"
        min_max = min
        gt_lt = >
        typemin_max = typemax
    elseif  uppercase(erode_dilate) == "DILATE"
        min_max = max
        gt_lt = <
        typemin_max = typemin
    else
        error("Option must be 'erode' or 'dilate' ")
    end

    df = zeros(eltype(f), size(f))
    Nf = length(f)

    # Determine the 'radius' of structuring element.  If the size is odd then
    # there is a centre pixel and the 'radius' is symmetric.  If the size is
    # even then the centre pixel is taken to be the integer pixel location to
    # the right of the ideal. In this case we have a 'left radius', rad1 and
    # a 'right radius' rad2.
    if isodd(k)
        rad1 = round(Int, (k-1)/2)
        rad2 = rad1
    else
        rad1 = round(Int, k/2)
        rad2 = rad1-1
    end

    # Pad ends of f with rad values of +-typemin/max and ensure overall length of padded
    # array is a multiple of k
    extrapad = round(Int, mod(Nf + rad1+rad2, k))
    padval = typemin_max(typeof(f[1]));
    g = [repeat([padval], rad1); f[:]; repeat([padval], rad2+extrapad)]
    h = copy(g)
    Npad = length(g)

    # Generate the intermediate arrays
    rng2tok = 2:k           # Precompute ranges for a small gain in speed
    rngkm1to1 = (k-1):-1:1

    for o in 0:k:(Npad-k)
        op1 = o+1
        om1 = o-1
        for n in rng2tok
            if gt_lt(g[o+n], g[om1+n])
                g[o+n] = g[om1+n]
            end
        end

        for n in rngkm1to1
            if gt_lt(h[o+n], h[op1+n])
                h[o+n] = h[op1+n]
            end
        end
    end

    # Combine the intermediate arrays g and h to obtain the dilation.  Note
    # that for even sized structuring elements there is a wasted min() or max()
    # operation every k steps.  However in the interests of keeping the code
    # clean and simple this is accepted.
    for n in rad1+1:rad1+Nf
        # If eroding we want the minimum of g[n+rad2] and h[n-rad1]
        # also note that if eroding gt_lt is >().  Vice-versa for dilation

        if gt_lt(g[n + rad2], h[n - rad1])
            df[n - rad1] = h[n - rad1]
        else
            df[n - rad1] = g[n + rad2]
        end

    end

    return df
end

#---------------------------------------------------------------------
"""

Parameterised struct for storing erosion/dilation parameters and data buffers
for the implementation of Marcel van Herk's morphological algorithm.
The parameterisation is awkward due to the need to accommodate BitArrays

If you are processing a BitArray T1 should be Bool and T2 is BitArray{1}
If you are processing an Array T1 should be set to the element type and T2 set to Array{eltype, 1}

"""
struct erode_dilate_data{T1,T2}
    min_max  # min() or max()
    gt_lt
    k::Int64     # length of 1D structuring element
    rad1::Int64  # radius 1 of structuring element
    rad2::Int64  # radius 2 of structuring element
    N::Int64     # Length of input array
    Npad::Int64  # Length of padded buffer arrays
    padval::T1
    g::T2        # buffer
    h::T2        # buffer
end

"""
Set up buffers etc for erode_dilate!()

See `erode_dilate_data`
"""
function erode_dilate1d!_setup(f::Union{BitArray{1}, Array{T,1}}, k::Integer, erode_dilate::String) where T<:Real

    if uppercase(erode_dilate) == "ERODE"
        min_max = min
        gt_lt = >
        typemin_max = typemax
    elseif  uppercase(erode_dilate) == "DILATE"
        min_max = max
        gt_lt = <
        typemin_max = typemin
    else
        error("Option must be 'erode' or 'dilate' ")
    end

    # Determine the 'radius' of structuring element.  If the size is odd then
    # there is a centre pixel and the 'radius' is symmetric.  If the size is
    # even then the centre pixel is taken to be the integer pixel location to
    # the right of the ideal. In this case we have a 'left radius', rad1 and
    # a 'right radius' rad2.
    if isodd(k)
        rad1 = rad2 = (k-1)÷2
    else
        rad1 = k÷2
        rad2 = rad1-1
    end

    # The buffers need to accommodate f + padding of rad1 + rad2 plus extra to
    # ensure overall length of padded array is a multiple of k
    Nf = length(f)
    extrapad = round(Int64, mod(Nf + rad1+rad2, k))
    padval = typemin_max(eltype(f))

    Npad = rad1 + Nf + rad2 + extrapad

    if eltype(f) == Bool   # Use BitArrays, faster and less memory
        g = BitArray(undef, Npad)
        h = BitArray(undef, Npad)
        return erode_dilate_data{Bool, BitArray{1}}(min_max, gt_lt,  k, rad1, rad2, Nf, Npad, padval, g, h)
    else
        g = zeros(eltype(f), Npad)
        h = zeros(eltype(f), Npad)
        return erode_dilate_data{eltype(f), Array{eltype(f),1}}(min_max, gt_lt,  k, rad1, rad2, Nf, Npad, padval, g, h)
    end

end

#---------------------------------------------------------------------
"""

Inplace version of erode_dilate1d()

Usage: erode_dilate1d!(df, f, D::erode_dilate_data, N = D.N)

Arguments:

    N - Optional argument indicating length of valid elements in f to use

"""
function erode_dilate1d!(df, f, D::erode_dilate_data, N = D.N)

    # Fill buffers D.g and D.h with padding and the input array f
    for n in 1:D.rad1
        D.g[n] = D.padval
    end

    ind = 1
    for n in (D.rad1+1):(D.rad1 + N)
        D.g[n] = f[ind]
        ind += 1
    end

    for n in (D.rad1 + N + 1):D.Npad
        D.g[n] = D.padval
    end

    for n in 1:D.Npad
        D.h[n] = D.g[n]
    end

    # Generate the intermediate arrays
    rng2tok = 2:D.k           # Precompute ranges for a small gain in speed
    rngkm1to1 = (D.k-1):-1:1

    for o in 0:D.k:(D.Npad-D.k)
        om1 = o - 1
        op1 = o + 1
        for n in rng2tok
            if D.gt_lt(D.g[o+n], D.g[om1+n])
                D.g[o+n] = D.g[om1+n]
            end
        end

        for n in rngkm1to1
            if D.gt_lt(D.h[o+n], D.h[op1+n])
                D.h[o+n] = D.h[op1+n]
            end
        end
    end

    # Combine the intermediate arrays g and h to obtain the dilation.  Note
    # that for even sized structuring elements there is a wasted min() or max()
    # operation every k steps.  However in the interests of keeping the code
    # clean and simple this is accepted.
    for n in (D.rad1+1):(D.rad1 + N)
        # If eroding we want the minimum of g[n+D.rad2] and h[n-D.rad1]
        # also, gt_lt is >().  Vice-versa for dilation

        if D.gt_lt(D.g[n + D.rad2], D.h[n - D.rad1])
            df[n - D.rad1] = D.h[n - D.rad1]
        else
            df[n - D.rad1] = D.g[n + D.rad2]
        end

    end

    return nothing
end

#---------------------------------------------------------------------
"""
imdilate - 2D morphological dilation with rectangular or octagonal structing element
```
Usage: dimg = imdilate(img, seType, seSize)

Arguments: img - Image to be dilated
        seType - String either "rectangle" or "octagon" specifying the
                 structuring element type. Can be shortened to "rect" or
                 "oct".
        seSize - Structuring element size.
                 If the seType is 'rect' seSize can be a 2 element vector
                 indicating the size of the rectangular structuring element
                 [rows, cols], or if it is a single value the structuring
                 element is square [seSize x seSize]
                 If seType is 'oct' then seSize is the nominal radius of
                 the octagonal structuring element.

Returns:  dimg - The dilated image.
```
Note that the radius of the octagonal structuring element is somewhat nominal
due to discrete approximations.  If anything the structuring element may end
up with a slightly larger radius than specified rather than being smaller.

This function uses Marcel van Herk's algorithm to run in linear time with
respect to the size of the image, irrespective of the structing element size.

See also imerode(), erode1d(), dilate1d()
"""
function imdilate(img::Array{T,2}, seType::String,
                  seSize::Union{Array{T2,1},T2}) where {T<:Real, T2<:Integer}
    imerode_dilate(img, seType, seSize, "DILATE")
end

function imdilate(img::BitArray{2}, seType::String,
                  seSize::Union{Array{T2,1},T2}) where T2 <: Integer
    imerode_dilate(img, seType, seSize, "DILATE")
end

#---------------------------------------------------------------------
"""
imerode - 2D morphological erosion with rectangular or octagonal structing element
```
Usage: eimg = imerode(img, seType, seSize)

Arguments: img - Image to be eroded
        seType - String either "rectangle" or "octagon" specifying the
                 structuring element type. Can be shortened to "rect" or
                 "oct".
        seSize - Structuring element size.
                 If the seType is 'rect' seSize can be a 2 element vector
                 indicating the size of the rectangular structuring element
                 [rows, cols], or if it is a single value the structuring
                 element is square [seSize x seSize]
                 If seType is 'oct' then seSize is the nominal radius of
                 the octagonal structuring element.

Returns:  eimg - The eroded image.
```
Note that the radius of the octagonal structuring element is somewhat nominal
due to discrete approximations.  If anything the structuring element may end
up with a slightly larger radius than specified rather than being smaller.

This function uses Marcel van Herk's algorithm to run in linear time with
respect to the size of the image, irrespective of the structing element size.

See also imdilate(), erode1d(), dilate1d()
"""
function imerode(img::Array{T,2}, seType::String,
                 seSize::Union{Array{T2,1},T2}) where {T<:Real, T2<:Integer}
    imerode_dilate(img, seType, seSize, "ERODE")
end

function imerode(img::BitArray{2}, seType::String,
                 seSize::Union{Array{T2,1},T2}) where T2<:Integer
    imerode_dilate(img, seType, seSize, "ERODE")
end

#------------------------------------------------------------------------

# Unexported function that performs erosion or dilation

#=
Reference: Marcel van Herk, "A fast algorithm for local minimum and maximum
filters on rectangular and octagonal kernels".  Pattern Recognition Letters 13
(1992) pp 517-521

PK April 2016

Code is ugly and should eventually be restructured to accept a structuring
element defined via decomposition into a set of linear dilations.
=#

#function imdilate{T<:Real}(img::Union{Array{T,2}, BitArray{2}}, seType::String, seSize)
function imerode_dilate(img, seType::String, seSize, erode_dilate::String)

    if ndims(img) == 3
        error("Image must be binary or greyscale")
    end

    if !(uppercase(erode_dilate) == "ERODE"  || uppercase(erode_dilate) == "DILATE")
        error("Option must be 'erode' or 'dilate' ")
    end

    (rows,cols) = size(img)

    # Rectangular structuring element
    if uppercase(seType[1:3]) == "REC"
        if length(seSize) == 1
            k = round.(Int, [seSize, seSize])
        else
            k = round.(Int, seSize)
        end

        dimg = copy(img)
#=
        # Old code which was memory intensive
        for c = 1:cols
            dimg[:,c] = erode_dilate1d(img[:,c], k[1], erode_dilate)
        end

        for r = 1:rows
            dimg[r,:] = erode_dilate1d(dimg[r,:], k[2], erode_dilate)
        end

=#
        # Inplace code

        # Operate on columns
        data = erode_dilate1d!_setup(img[:,1], k[1], erode_dilate)
        for c in 1:cols
            erode_dilate1d!(view(dimg,:,c), img[:,c], data)
        end

        # Process rows of result
        data = erode_dilate1d!_setup(dimg[1,:], k[2], erode_dilate)
        for r in 1:rows
            erode_dilate1d!(view(dimg,r,:), dimg[r,:], data)
        end

    # Octagonal structuring element
    elseif uppercase(seType[1:3]) == "OCT"

        # The size of the linear structuring element for the
        # vertical and horizontal dilations matches the desired radius.
        k = round(Int, seSize)
        # The size of the linear structuring element for the dilations in the
        # diagonal direction is 1/sqrt(2) of k
        dk = round(Int, k/sqrt(2))
        # Ensure the diagonal structuring element size is odd so that there is a centre
        # pixel and the resulting octagon is symmetric
        if iseven(dk); dk = dk+1; end

        # First do 2D square dilation
        dimg = imerode_dilate(img, "rect", k, erode_dilate)

        # Extract diagonal lines of pixels from dimg and perform 1d dilation on
        # them

dirn = (1,2,3,4)

if 1 in dirn
        # NE lines, emanating from left edge of image
        # Max no of elements is cols
        data = erode_dilate1d!_setup(dimg[1,:], dk, erode_dilate)
        l = dimg[1,:]  # buffers for storing input and output
        dl = copy(l)

        for r = 1:rows
            cmax = min(r,cols)

            for c = 1:cmax
                l[c] = dimg[r-c+1, c]
            end

            erode_dilate1d!(dl, l, data, cmax)

            for c = 1:cmax
                dimg[r-c+1,c] = dl[c]
            end
        end
end
if 2 in dirn
        # NE lines, emanating from bottom of image, max no of elements is rows
        data = erode_dilate1d!_setup(dimg[:,1], dk, erode_dilate)
        l = dimg[:,1]
        dl = copy(l)

        for c = 2:cols#-1
            rmin = max(1, rows-(cols-c))
            nelements = rows - rmin + 1

            for r = rows:-1:rmin
                l[r-rmin+1] = dimg[r,c+rows-r]
            end

            erode_dilate1d!(dl, l, data, nelements)

            for r = rows:-1:rmin
                dimg[r,c+rows-r] = dl[r-rmin+1]
            end
        end
end
if 3 in dirn
        # SE lines, emanating from left edge of image
        data = erode_dilate1d!_setup(dimg[1,:], dk, erode_dilate)
        l = dimg[1,:]
        dl = copy(l)

        for r = 1:rows
            cmax = min(rows-r+1,cols)

            for c = 1:cmax
                l[c] = dimg[r+c-1, c]
            end

            erode_dilate1d!(dl, l, data, cmax)

            for c = 1:cmax
                dimg[r+c-1,c] = dl[c]
            end
        end
end
if 4 in dirn
        # SE lines, emanating from top of image
        data = erode_dilate1d!_setup(dimg[:,1], dk, erode_dilate)
        l = dimg[:,1]
        dl = copy(l)

        for c = 2:cols
            rmax = min(rows, cols-c+1)
            for r = 1:rmax
                l[r] = dimg[r,c+r-1]
            end

            erode_dilate1d!(dl, l, data, rmax)

            for r = 1:rmax
                dimg[r,c+r-1] = dl[r]
            end
        end
end
    else
        error("Structure element type must be 'rect' or 'oct' ")
    end

    return dimg
end

#------------------------------------------------------------------------
"""
imerode - 2D morpholgical erosion with arbitrary structuring element
```
Usage:   eimg = imerode(img, se)

Arguments:
          img - Image to be eroded.  May be greyscale or binary
                ::Array{T,2} or ::BitArray{2}
           se - Structuring element defined by a 2D Bool array

Returns:
         eimg - eroded image
```
See also: imdilate(), circularstruct()
"""
function imerode(img::Array{T,2}, se) where T <: Real
    return imerode_dilate(img, se, "ERODE")
end

function imerode(img::BitArray{2}, se)
    return imerode_dilate(img, se, "ERODE")
end

#------------------------------------------------------------------------
"""
imdilate - 2D morpholgical dilation with arbitrary structuring element
```
Usage:   dimg = imdilate(img, se)

Arguments:
          img - Image to be dilated.  May be greyscale or binary
                ::Array{T,2} or ::BitArray{2}
           se - Structuring element defined by a 2D Bool array

Returns:
         dimg - dilated image
```
See also: imerode(), circularstruct()
"""
function imdilate(img::Array{T,2}, se) where T <: Real
    return imerode_dilate(img, se, "DILATE")
end

function imdilate(img::BitArray{2}, se)
    return imerode_dilate(img, se, "DILATE")
end

#------------------------------------------------------------------
# Unexported function to perform image erosion or dilation depending
# on the whether minimum() or maximum() functions are passed for min_max

# Inefficient code!
# April 2016

function imerode_dilate(img, se, erode_dilate::String)

    if uppercase(erode_dilate) == "ERODE"
        min_max = minimum
    elseif  uppercase(erode_dilate) == "DILATE"
        min_max = maximum
    else
        error("Option must be 'erode' or 'dilate' ")
    end

    (rows,cols) = size(img)
    (sr,sc) = size(se)
    roff = round(Int, (sr-1)/2)
    coff = round(Int, (sc-1)/2)
    dimg = zeros(size(img))

    for r = 1:rows-sr, c = 1:cols-sc
        dimg[r+roff, c+coff] = min_max(img[r:r+sr-1,  c:c+sc-1].*se)
    end

    return dimg
end

#------------------------------------------------------------------------
"""
circularstruct - Generate circular structuring element for morphological operations
```
Usage:  se = circularstruct(radius)

Argument:  radius - Desired radius of structuring element
Returns:       se - Bool structuring element of size (2*radius+1)^2

```
See also: imdilate(), imerode()
"""
function circularstruct(radius::Real)
    se = [ sqrt(x^2+y^2) .<= radius  for x = -radius:radius, y = -radius:radius ]
end

#----------------------------------------------------------------------
"""
gaussfilt -  Small wrapper function for convenient Gaussian filtering
```
Usage:  smimg = gaussfilt(img, sigma)

Arguments:  img - Image to be smoothed, can be multi-channel.
                  ::Array{T,2}  or ::Array{T,3}
          sigma - Standard deviation of Gaussian filter.

Returns:  smimg - Smoothed image.
```
If called with sigma = 0 the function immediately returns with img assigned
to smimg

"""
function gaussfilt(img::Array, sigma::Real)

    if sigma < eps()
        return copy(img)   # ?? Return img or copy(img) ?
    end

    h = ImageFiltering.Kernel.gaussian(sigma)
#    h = Images.KernelFactors.IIRGaussian(sigma)

    if ndims(img) == 2      # Greyscale image
        return Images.imfilter(img,h)

    elseif ndims(img) == 3  # Multi-channel image
        nchan = size(img,3)
        smimg = zeros(size(img))

        for n = 1:nchan
            smimg[:,:,n] = Images.imfilter(img[:,:,n],h)
        end

        return smimg
    end
end

#----------------------------------------------------------------------
"""
floatyx - Convert 2D ImageMeta to 2D float array with y x spatial order
```
Usage:  (fimg, prop) = floatyx(img)

Argument:  img - ::ImageMeta{T,2}

Returns:  fimg - ::Array{Float64,2} in "y" "x" spatial order.
          prop - A copy of the properties dictionary of the input image
                 with "spatialorder" adjusted (if this was needed).
```
Most image processing functions expect 2D arrays in (row, column)
format and most feature localisation functions return corner and edge
coordinates in terms of (row, column) coordinates.

This convenience function takes a 2D ImageMeta, extracts the data,
converts it to a 2D Float64 array and, if necessary, transposes the data
so that it is in "y" "x" (row, column) spatial order.  The
ImageMeta spatial order property is set to ["y","x"] and the
properties returned.
"""
function floatyx(img::Images.ImageMeta{T,2}) where T <: Real

    fimg = float(Images.data(img))
    prop = copy(img.properties)

    if prop["spatialorder"] == ["x","y"]
        fimg = fimg'
        prop["spatialorder"] = ["y","x"]
    elseif img.properties["spatialorder"] != ["y","x"]
        error("Unable to handle the spatial order of this image")
    end

    return fimg, prop
end

#----------------------------------------------------------------------
"""
imgnormalise/imgnormalize - Normalises image values to 0-1, or to desired mean and variance

```
Usage 1:      nimg = imgnormalise(img)
```
Offsets and rescales image so that the minimum value is 0
and the maximum value is 1.

```
Usage 2:      nimg = imgnormalise(img, reqmean, reqvar)

Arguments:  img     - A grey-level input image.
            reqmean - The required mean value of the image.
            reqvar  - The required variance of the image.
```
Offsets and rescales image so that nimg has mean reqmean and variance
reqvar.
"""
function imgnormalise(img::Array) # Normalise 0 - 1
    n = img .- minimum(img)
    return n = n/maximum(n)
end

# Normalise to desired mean and variance
function imgnormalise(img::Array, reqmean::Real, reqvar::Real)
    n = img .- mean(img)
    n /= std(img)      # Zero mean, unit std dev
    return n = reqmean .+ n*sqrt(reqvar)
end

# For those who spell normalise with a 'z'
"""
imgnormalize - Normalizes image values to 0-1, or to desired mean and variance
```
Usage 1:      nimg = imgnormalize(img)
```
Offsets and rescales image so that the minimum value is 0
and the maximum value is 1.
```
Usage 2:      nimg = imgnormalize(img, reqmean, reqvar)

Arguments:  img     - A grey-level input image.
            reqmean - The required mean value of the image.
            reqvar  - The required variance of the image.
```
Offsets and rescales image so that nimg has mean reqmean and variance
reqvar.
"""
function imgnormalize(img::Array)
    return imgnormalise(img)
end

function imgnormalize(img::Array, reqmean::Real, reqvar::Real)
    return imgnormalise(img, reqmean, reqvar)
end

#----------------------------------------------------------------------
"""
histtruncate - Truncates ends of an image histogram.

Function truncates a specified percentage of the lower and
upper ends of an image histogram.

This operation allows grey levels to be distributed across
the primary part of the histogram.  This solves the problem
when one has, say, a few very bright values in the image which
have the overall effect of darkening the rest of the image after
rescaling.

```
Usage:
1)   newimg = histtruncate(img, lHistCut, uHistCut)
2)   newimg = histtruncate(img, HistCut)

Arguments:
 Usage 1)
   img         -  Image to be processed.
   lHistCut    -  Percentage of the lower end of the histogram
                  to saturate.
   uHistCut    -  Percentage of the upper end of the histogram
                  to saturate.  If omitted or empty defaults to the value
                  for lHistCut.
 Usage 2)
   HistCut     -  Percentage of upper and lower ends of the histogram to cut.

Returns:
   newimg      -  Image with values clipped at the specified histogram
                  fraction values.  If the input image was colour the
                  lightness values are clipped and stretched to the range
                  0-1.  If the input image is greyscale no stretching is
                  applied. You may want to use imgnormalise() to achieve this.
```
See also: imgnormalise()
"""
function  histtruncate(img::Array, lHistCut::Real, uHistCut::Real)

    if lHistCut < 0 || lHistCut > 100 || uHistCut < 0 || uHistCut > 100
	error("Histogram truncation values must be between 0 and 100")
    end

    if ndims(img) > 2
	error("histtruncate only defined for grey scale images")
    end

    newimg = copy(img)
    sortv = sort(newimg[:])   # Generate a sorted array of pixel values.

    # Any NaN values will end up at the end of the sorted list. We
    # need to ignore these.
#    N = sum(.!isnan.(sortv))  # Number of non NaN values. v0.6
    N = sum(broadcast(!,isnan.(sortv)))  # compatibity for v0.5 and v0.6

    # Compute indicies corresponding to specified upper and lower fractions
    # of the histogram.
    lind = floor(Int, 1 + N*lHistCut/100)
    hind =  ceil(Int, N - N*uHistCut/100)

    low_val  = sortv[lind]
    high_val = sortv[hind]

    # Adjust image
    newimg[newimg .< low_val] .= low_val
    newimg[newimg .> high_val] .= high_val

    return newimg
end

function  histtruncate(img::Array, HistCut::Real)
    return histtruncate(img, HistCut, HistCut)
end

#----------------------------------------------------------------------
"""
matchbycorrelation - Match image feature points by correlation

Function generates putative matches between previously detected
feature points in two images by looking for points that are maximally
correlated with each other within windows surrounding each point.
Only points that correlate most strongly with each other in both
directions are returned.

This implements normalised cross correlation in its most basic form.
There is no attempt to deal with scale or orientation differences
between the images and thus it will only work if the images are not
too dissimilar.

This is a simple-minded N^2 comparison.

```
Usage: (m1, m2, p1ind, p2ind, cormat) =
                matchbycorrelation(img1, p1, img2, p2, w, dmax)

Arguments:
      img1, img2 - Images containing points that we wish to match.
        p1, p2   - Coordinates of feature pointed detected in img1 and
                   img2 respectively using a corner detector (say Harris
                   or phasecong3).  p1 and p2 are [2 x npts] arrays though
                   p1 and p2 are not expected to have the same number
                   of points.  The first row of p1 and p2 gives the row
                   coordinate of each feature point, the second row
                   gives the column of each point.
        w        - Window size (in pixels) over which the correlation
                   around each feature point is performed.  This should
                   be an odd number.
        dmax     - (Optional) Maximum search radius for matching
                   points.  Used to improve speed when there is little
                   disparity between images. Even setting it to a generous
                   value of 1/4 of the image size gives a useful
                   speedup. If this parameter is omitted it defaults to Inf.

Returns:
        m1, m2   - Coordinates of points selected from p1 and p2
                   respectively such that (putatively) m1[:,i] matches
                   m2[:,i]. m1 and m2 are [2 x npts] arrays defining the
                   points in each of the images in the form [row;col].
  p1ind, p2ind   - Indices of points in p1 and p2 that form a match.  Thus,
                   m1 = p1[:,p1ind] and m2 = p2[:,p2ind]
        cormat   - Correlation matrix; rows correspond to points in p1,
                   columns correspond to points in p2
```
This function is slow as mud! Needs some attention.
"""
function matchbycorrelation(img1i, p1, img2i, p2, w, dmax=Inf)
    # Probably needs devectorisation to improve speed
    #=
    February 2004    - Original version
    May      2004    - Speed improvements + constraint on search radius for
    additional speed
    August   2004    - Vectorized distance calculation for more speed
    (thanks to Daniel Wedge)
    December 2009    - Added return of indices of matching points from original
    point arrays
    May 2016         - Ported to Julia
    =#

    #-------------------------------------------------------------------------
    # Function that does the work.  This function builds a correlation matrix
    # that holds the correlation strength of every point relative to every
    # other point.  While this seems a bit wasteful we need all this data if
    # we want to find pairs of points that correlate maximally in both
    # directions.
    #
    # This code assumes img1 and img2 have zero mean.  This speeds the
    # calculation of the normalised correlation measure.

    function correlationmatrix(img1, p1, img2, p2, w, dmax)

        if iseven(w)
            error("Window size should be odd")
        end

        (rows1, npts1) = size(p1)
        (rows2, npts2) = size(p2)
        if rows1 != 2 || rows2 != 2
            error("Feature points must be specified in 2xN arrays")
        end

        # Initialize correlation matrix values to -infinty
        cormat = -Inf*ones(npts1,npts2)

        (im1rows, im1cols) = size(img1)
        (im2rows, im2cols) = size(img2)

        r = (w-1)÷2   # 'radius' of correlation window

        # For every feature point in the first image extract a window of data
        # and correlate with a window corresponding to every feature point in
        # the other image.  Any feature point less than distance 'r' from the
        # boundary of an image is not considered.

        # Find indices of points that are distance 'r' or greater from
        # boundary on image1 and image2;
        n1ind = find((p1[1,:].>r) .& (p1[1,:].<im1rows+1-r) .& (p1[2,:].>r) .& (p1[2,:].<im1cols+1-r))
        n2ind = find((p2[1,:].>r) .& (p2[1,:].<im2rows+1-r) .& (p2[2,:].>r) .& (p2[2,:].<im2cols+1-r))

        dists2 = zeros(length(n2ind))

        for n1 = n1ind
            # Generate window in 1st image
            w1 = vec(img1[p1[1,n1]-r:p1[1,n1]+r, p1[2,n1]-r:p1[2,n1]+r])
            # Pre-normalise w1 to a unit vector.
            w1 = w1/sqrt(w1'*w1)

            # Identify the indices of points in p2 that we need to consider.
            if dmax == Inf
	        n2indmod = n2ind # We have to consider all of n2ind

            else     # Compute distances from p1[:,n1] to all available p2.
                ind = 0
                for i = n2ind
                    ind += 1
                    dists2[ind] = sum((p1[:,n1]-p2[:,i]).^2)
                end
	        n2indmod = n2ind[findall(dists2 .< dmax^2)]
            end

            # Calculate normalised correlation measure.  Note this gives
            # significantly better matches than the unnormalised one.
            for n2 = n2indmod
                # Generate window in 2nd image
                w2 = vec(img2[p2[1,n2]-r:p2[1,n2]+r, p2[2,n2]-r:p2[2,n2]+r])
                cormat[n1,n2] = (w1'*w2/sqrt(w2'*w2))[1]  # [1] to create scalar
            end
        end

        return cormat
    end
    #-----------------------------------------------------------------
    # The main function

    img1 = float(img1i)
    img2 = float(img2i)

    # Subtract image smoothed with an Gaussian filter with sigma = w/3 from
    # each of the images.  This attempts to compensate for brightness
    # differences in each image.  Doing it now allows faster correlation
    # calculation.
    sigma = w/3   # A guess as to what might be appropriate...
    img1 .-= gaussfilt(img1, sigma)
    img2 .-= gaussfilt(img2, sigma)

    # Generate correlation matrix
    cormat = correlationmatrix(img1, p1, img2, p2, w, dmax)
    (corrows,corcols) = size(cormat)

    # Find max along rows give strongest match in p2 for each p1
    mp2forp1 = zeros(corrows)
    colp2forp1 = zeros(Int,corrows)
    for r = 1:corrows
        (mp2forp1[r], colp2forp1[r]) = findmax(cormat[r,:])
    end
    # Find max down cols give strongest match in p1 for each p2
    mp1forp2 = zeros(corcols)
    rowp1forp2 = zeros(Int,corcols)
    for c = 1:corcols
        (mp1forp2[c], rowp1forp2[c]) = findmax(cormat[:,c])
    end

    # Now find matches that were consistent in both directions
    p1ind = zeros(Int, size(p1,2))  # Arrays for storing matched indices
    p2ind = zeros(Int, size(p2,2))
    indcount = 0
    for n = 1:corrows
        if rowp1forp2[colp2forp1[n]] == n  # consistent both ways
            indcount += 1
            p1ind[indcount] = n
            p2ind[indcount] = colp2forp1[n]
        end
    end

    # Trim arrays of indices of matched points
    p1ind = p1ind[1:indcount]
    p2ind = p2ind[1:indcount]

    # Extract matched points from original arrays
    m1 = p1[:,p1ind]
    m2 = p2[:,p2ind]

    return (m1, m2, p1ind, p2ind, cormat)

end # of matchbycorrelation

#----------------------------------------------------------------------
"""
grey2census - Convert image grey scale values to census values
```
Usage:  cimg = grey2census(img, window)

Arguments:
          img - greyscale image to be processed
       window - Optional 2-vector [rows,cols] specifying the size of the
                window to be considered in computing the census
                transform. Defaults to [7, 9].  The values must be odd and
                their product must not be greater than 64.
Returns:
         cimg - Census encoded UInt64 image.
```
Each pixel is encoded with a bit pattern formed by comparing the pixel with
the pixels in the window around it. If a window pixel is less than the centre
pixel its corresponding bit in the census encoding is set.  This provides an
encoding that describes a pixel in terms of its surrounding pixels in a way
that is invariant to lighting variations.  Note, however, that the encoding is
dependent on the image orientation.

Use the Hamming distance to compare encoded pixel values when matching.
```
  hammingDist = count_ones(img1[r,c] \$ img2[r,c])
```
"""
function grey2census(img::Array{T,2}; window::Vector{T2}=[7,9], medianref = true) where {T<:Real,T2<:Integer}
    # Reference: Ramin Zabih and John Woodfill. Non-parametric Local
    # Transforms for Computing Visual Correspondence. European Conference
    # on Computer Vision, Stockholm, Sweden, May 1994, pages 151-158
    # ** There is a type instability that makes this slower than it should
    # ** be and the array slicing must be costly.

    (rows,cols) = size(img)

    nBits = window[1]*window[2]
    if nBits > 64
        error("Window must not have more than 64 elements")
    end

    if iseven(window[1]) || iseven(window[2])
        error("Window sizes must be odd")
    end

    rrad = round(Int,(window[1]-1)/2)
    crad = round(Int,(window[2]-1)/2)

    # Define the reference values.  If we use the median of the window we
    # apply a median filter to obtain the reference values, else use the
    # original image
    if medianref
        imref = medfilt2(img, window)
    else
        imref = copy(img)
    end

    # Build table of values corresponding to each bit
    bitval = UInt64[2^(b-1) for b=1:nBits]

    cimg = zeros(UInt64, rows, cols)

    # Build meshgrid of coordinates corresponding to all the offsets/bits
    coff = [c for r = -rrad:rrad, c = -crad:crad]
    roff = [r for r = -rrad:rrad, c = -crad:crad]

    for n = 1:nBits
        cimg[rrad+1:rows-rrad, crad+1:cols-crad] =
        cimg[rrad+1:rows-rrad, crad+1:cols-crad] +
#=
        bitval[n]*(
                img[rrad+1+roff[n]:rows-rrad+roff[n], crad+1+coff[n]:cols-crad+coff[n]] .<
                img[rrad+1:rows-rrad, crad+1:cols-crad])
=#
        bitval[n]*(
                view(img,rrad+1+roff[n]:rows-rrad+roff[n], crad+1+coff[n]:cols-crad+coff[n]) .<
                view(img,rrad+1:rows-rrad, crad+1:cols-crad))

    end

    return cimg
end

#-----------------------------------------------------------------
"""
briefcoords - Compute BRIEF descriptor sampling coordinates within a patch.

```
Usage:  rc = briefcoords(S, nPairs, UorG; disp=false)

Arguments:  S - An odd integer specifying the patch size (S x S).
       nPairs - Number of point pairs required.  Typically this is a power
                of 2, 128, 256, 512 for packing the descriptor values into
                a bit string.
         UorG - Character 'U' or 'G' indicating whether a uniform or
                gaussian distribution of point pairings is formed within
                the patch.
         disp - Optional boolean flag indicating whether a plot of the point
                pairings should be displayed.

Returns:   rc - [2 x 2*nPairs] array of integer (row; col) coordinates
                with all values in the range -(S-1)/2..(S-1)/2.  Each
                successive pair of columns is intended to provide a pair of
                points for comparing image grey values against each other
                for forming a BRIEF descriptor of a patch about some
                central feature location.
```

Use of the Gaussian distribution of point pairings corresponds to the approach
suggested by Calonder et al in their original paper.  Note, however that for
small patches and a large number of pairings one will get repeated pairings
which reduces the degrees of freedom in the final descriptor.  The uniformly
distributed option does not result in repeated pairings making it the
preferred option for small patch sizes.

See also: grey2lbp(), grey2lbp!()
"""
function briefcoords(S, nPairs, UorG; disp=false)
    # Reference:
    # Michael Calonder, Vincent Lepetit, Christoph Strecha, and Pascal Fua.
    # BRIEF: Binary Robust Independent Elementary Features
    #
    # To do: Constrain point selection to be within a circular region
    # around the centre pixel, rather than a square one.  This will make
    # the size and localisation invariant to orientation of the underlying
    # pixel data.

    if iseven(S)
        error("Patch size must be an odd integer")
    end

    # Fix the initialisation of the random number generator so that we get
    # repeatable sampling point locations.
    rng = MersenneTwister(1)

    R = round(Int, (S-1)/2)    # 'Radius' of patch

    # Option that generates a uniformly distributed pairings of
    # coordinates within the patch.
    if uppercase(UorG[1]) == 'U'
        rc = rand(rng, -R:R, (2, 2*nPairs))

    # Option that generates a set of normally distributed points within the
    # patch and forms pairings between them as suggested by the orginal paper.
    elseif uppercase(UorG[1]) == 'G'
        sigma = S/5  # Standard deviation suggested by Calonder et al.
        rc = round(Int, sigma * randn(rng, 2, 2*nPairs))

        # Clamp coordinates to patch bounds
        rc[rc .> R] = R
        rc[rc .< -R] = -R

    else
        error("Option must be 'U'niform or 'G'aussian")
    end

    # Diagnostic display
    if disp
        figure(200); clf
        R = (S-1)/2 + 1
        axis([-R, R, -R, R])

        for n = 1:nPairs
            plot(rc[2, 2*n-1:2*n], rc[1, 2*n-1:2*n], color = rand(3),
                 linewidth = 3)
            hold(true)
        end

        axis("equal")
        hold(false)

        # Determine distances between pairs and display histogram
        sdist = zeros(nPairs)
        for n = 1:nPairs
            sdist[n] = norm(rc[:,2*n-1] - rc[:,2*n])
        end

        (e, counts) = hist(sdist, 0:.5:S)
        figure(30); clf()
        PyPlot.bar(left=e[1:end-1], height = counts, width=e[2]-e[1])
        title("Histogram of distances between pairs")
    end

    return rc
end

#-----------------------------------------------------------------
"""
grey2lbp - Convert image grey scale values to local binary pattern.

```
Usage:  lbpim = grey2lbp(img, rc, window)

Arguments:
          img - greyscale image to be processed
           rc - [2 x 2*nPairs] array of integer (row;col) coordinates.
                Each successive pair of columns provides a pair of
                points for comparing image grey values against each other
                for forming a binary descriptor of a patch about some
                central feature location. Array rc must not have more than
                128 columns corresponding to an encoding of 64 bits,
       window - 2-vector specifying the size of the window to be considered
                in computing the local standard deviation for determining
                regions to be encoded with a 0.  The window size values
                should match the range of values in rc and be odd.

Returns:
        lbpim - Local Binary Pattern encoded UInt64 image.
```

Each pixel is encoded with a bit pattern formed by comparing pairs of pixels.
If the difference between pixels is -ve they are are encoded with 1, else 0.
This provides an encoding that describes a pixel in terms of its surrounding
pixels in a way that is invariant to lighting variations.  Note, however, that
the encoding is dependent on the image orientation.   The encoding is also
sensitive to noise on near constant regions.

Use the Hamming distance to compare encoded pixel values when matching.

See also: grey2lbp!(), grey2census(), briefcoords()
"""
function grey2lbp(img::Array, rc::Array, window::Vector)

    if ndims(img) == 3
        error("Image must be grey scale")
    end

    (rows,cols) = size(img)
    lbpim = zeros(UInt64, rows, cols)

    nBits = div(size(rc,2),2)       # No of coordiante pairs / No of bits

    if nBits > 64
        error("Array 'rc' must not have more than 128 columns")
    end

    if iseven(window[1]) || iseven(window[2])
        error("Window sizes must be odd")
    end

    rrad = div((window[1]-1),2)
    crad = div((window[2]-1),2)

    # Build table of values corresponding to each bit.
    bitval = [UInt64(1) << n for n = 0:63]

    # Perform encoding
    # Jumping around in img is not cache friendly
    for b = 1:nBits
        twob = 2*b
        twobm1 = 2*b-1
        roff1 = rc[1,twob]
        roff2 = rc[1,twobm1]
        for c = crad+1:cols-crad
            c1 = c+rc[2,twob]
            c2 = c+rc[2,twobm1]

            for r = rrad+1:rows-rrad
                if img[r+roff1, c1] < img[r+roff2, c2]
                    lbpim[r,c] |= bitval[b]
                end
            end
        end
    end

    return lbpim
end

#----------------------------
# Inplace version
"""
grey2lbp! - Convert image grey scale values to local binary pattern.

```
Usage:   grey2lbp!(lbpim, img, rc, window)

Arguments:
        lbpim - Buffer for Local Binary Pattern encoded image.
                Must be of type Array{UInt64,2}.
          img - greyscale image to be processed
           rc - [2 x 2*nPairs] array of integer (row;col) coordinates.
                Each successive pair of columns provides a pair of
                points for comparing image grey values against each other
                for forming a binary descriptor of a patch about some
                central feature location. Array rc must not have more than
                128 columns corresponding to an encoding of 64 bits,
       window - 2-vector specifying the size of the window to be considered
                in computing the local standard deviation for determining
                regions to be encoded with a 0.  The window size values
                should match the range of values in rc and be odd.

Returns:  nothing

```

Each pixel is encoded with a bit pattern formed by comparing pairs of pixels.
If the difference between pixels is -ve they are are encoded with 1, else 0.
This provides an encoding that describes a pixel in terms of its surrounding
pixels in a way that is invariant to lighting variations.  Note, however, that
the encoding is dependent on the image orientation.   The encoding is also
sensitive to noise on near constant regions.

Use the Hamming distance to compare encoded pixel values when matching.

See also: grey2lbp(), grey2census(), briefcoords()
"""
function grey2lbp!(lbpim::Array{UInt64,2}, img::Array, rc::Array, window::Vector)

    if ndims(img) == 3
        error("Image must be grey scale")
    end

    (rows,cols) = size(img)
    fill!(lbpim, zero(UInt64))

    nBits = div(size(rc,2),2)       # No of coordiante pairs / No of bits

    if nBits > 64
        error("Array 'rc' must not have more than 128 columns")
    end

    if iseven(window[1]) || iseven(window[2])
        error("Window sizes must be odd")
    end

    rrad = div((window[1]-1),2)
    crad = div((window[2]-1),2)

    # Build table of values corresponding to each bit.
    bitval = [UInt64(1) << n for n = 0:63]

    rrange = rrad+1:rows-rrad
    crange = crad+1:cols-crad

    nrows = length(rrange)

    # Perform encoding
    # Jumping around in img is not cache friendly
    @inbounds begin
        for b = 1:nBits
            twob = 2*b
            twobm1 = twob-1
            roff1 = rc[1,twob]
            roff2 = rc[1,twobm1]

            for c = crange
                c1 = c+rc[2,twob]
                c2 = c+rc[2,twobm1]

                # This loop is the killer for time, what can one do?
                for r = rrange
                    if img[r+roff1, c1] < img[r+roff2, c2]
                        lbpim[r,c] |= bitval[b]
                    end
                end
            end
        end
    end # @inbounds

    return nothing
end

#------------------------------------------------------------------
"""
medfilt2 - Convenience wrapper for median filtering.

```
Usage:  medimg = medfilt2(img, h::Int, w::Int)
        medimg = medfilt2(img, h::Int)
        medimg = medfilt2(img, hw::Tuple)

Arguments:   img - Image to be processed, Array{T,2}
            h, w - Height and width of rectangular window over which the
                   median is to be computed. Values must be odd.
                   h and w may be specified as a size tuple, or as a
                   single value in which case w and h are made equal.

Returns:  medimg - Median filtered image.

```

"""
function medfilt2(img::Array{T,2}, h::Int, w::Int) where T <: Real
    # ** To be rewritten using Huang's algorithm or Perreault and Hebert's
    # ** algorithm

    if iseven(h) || iseven(w)
        error("Median filter region size must be odd")
    end

    return mapwindow(median!, img, (h,w))
end

function medfilt2(img::Array{T,2}, sze::Tuple{Int, Int}) where T <: Real
    return medfilt(img, sze[1], sze[2])
end

function medfilt2(img::Array{T,2}, sze::Int) where T <: Real
    medfilt2(img, sze, sze)
end

#------------------------------------------------------------------------------
"""
stdfilt2 - Compute local standard deviation across an image.

```
Usage:  stdimg = stdfilt2(img, h::Int, w::Int)
        stdimg = stdfilt2(img, h::Int)
        stdimg = stdfilt2(img, hw::Tuple)

Arguments:   img - Image to be processed, Array{T,2}
            h, w - Height and width of rectangular window over which the
                   standard deviation is to be computed. Values must be odd.
                   h and w may be specified as a size tuple, or as a
                   single value in which case w and h are made equal.

Returns:  stdimg - Standard deviation image.

```

"""
function stdfilt2(img::Array, h::Int, w::Int)

    if ndims(img) != 2
        error("Image must be 2D")
    end

    # Get mean image
    kern = Images.centered(Images.imaverage((h,w)))
    meanimg::Array{Float64,2} = Images.imfilter(img, kern)

    # Subtract the original image from the mean image and square
    meanimg .= (meanimg .- img).^2

    # Perform averaging to get local variance
    stdimg::Array{Float64,2} = Images.imfilter(meanimg, kern)

    # Take sqrt to get standard deviation
    stdimg .= sqrt.(stdimg)

    return stdimg
end

function stdfilt2(img::Array, sze::Tuple{Int, Int})
    return stdfilt(img, sze[1], sze[2])
end

function stdfilt2(img::Array, sze::Int)
    return stdfilt(img, sze, sze)
end

#----------------------------------------------------------------------
"""
keypause - Wait for user to hit return before continuing.

```
Usage:  keypause()
```

"""
function keypause()
    println("Hit return to continue, or 'x' to exit")
    a = readline()
    if isempty(a)
        return
    elseif a[1] == 'x'
        error("Exiting")  # Should be a nice way to do this
    else
        return
    end
end

#----------------------------------------------------------------------

function testinput(img::Union{T, Array{T,2}, BitArray{2},Vector{T}}) where T<:Real
println(typeof(img))
end


using DataFrames, Dates, SpecialFunctions
using Distributions, Extremes
using Test
using LinearAlgebra, Random
using Mamba
using Statistics

# Set the seed for reproductible test results
Random.seed!(12)

@testset "Extremes.jl" begin
    include("data_test.jl")
    include("parameterestimation_test.jl")
    include("reproducingColesResults.jl")
    include("structures_test.jl")
    include("utils_test.jl")
    include("validationplots_tests.jl")
end;


function secret_handshake(code)
    mask = 1
    ans = String[]
    for (index, value) in enumerate(("wink", "double blink", "close your eyes", "jump"))
        if (code & mask << (index - 1)) != 0
            push!(ans, value)
        end
    end
    if (code & mask << 4) != 0
        reverse!(ans)
    end
    ans
end

function secret_handshake3(code::Integer)
    handshake = String[]
    (code >> 0) & 1 == 1 && push!(handshake, "wink")
    (code >> 1) & 1 == 1 && push!(handshake, "double blink")
    (code >> 2) & 1 == 1 && push!(handshake, "close your eyes")
    (code >> 3) & 1 == 1 && push!(handshake, "jump")
    (code >> 4) & 1 == 1 && reverse!(handshake)
    return handshake
end


modifier(ability) = div(ability - 10, 2, RoundDown)

ability() = sum(sort!([roll() for _ in 1:4])[2:end])

mutable struct DNDCharacter
    strength::Int
    dexterity::Int
    intelligence::Int
    wisdom::Int
    charisma::Int
    constitution::Int
    hitpoints::Int
    function DNDCharacter()
        rolls = [ability() for _ in 1:6]
        new(rolls...,10 + modifier(rolls[end]))
    end
end

roll(n, d) = sum(rand(Base.OneTo(d),n))
roll(d) = roll(1,d)
roll() = roll(6)

function str2roll(str)
    try
        (n, d) = parse.(Int,split(str,'d';limit=2))
        roll(n,d)
    catch(e)
        isa(e,ArgumentError) && throw(ArgumentError("invalid dice roll format"))
        throw(e)
    end
end

""" `roll"4d20"` style rolling, just for fun"""
macro roll_str(str) :(str2roll($str)) end


"""
This maps (v, C, log_α) ↦ u = log.(α) - log.(Kb) = -log.(Kb./α) = log.(α./Kb)
"""
function sinkhorn_step_log!(u::AbstractArray{T}, v, C, εinv, log_α, temp) where T
    for i in eachindex(u)
        for j in eachindex(v)          # sum over j
            temp[j] = C[i,j]
        end
        rmul!(temp, -εinv)
        temp .+= v .- log_α[i]
        u[i] = -logsumexp(temp)
    end
    return u
end

"""
wasserstein_distance_log(x, y, log_α, log_β, c, u₀, v₀, log_d₁₀, log_d₂₀, SP, caches)

Differentiable by x only!
"""
function sinkhorn_dvg_particles(    x::AbstractMatrix{T},
                                    y::AbstractMatrix{V},
                                    log_α::Vector{V},
                                    log_β::Vector{V},
                                    c::Base.Callable,
                                    u₀, v₀,
                                    log_d₁₀, log_d₂₀,
                                    SP, caches   ) where {T,V}

    VC = caches.VC
    εinv = 1/SP.ε

    C_xy = ForwardDiff.value.( LazyCost(x, y, c) )
    C_xx = ForwardDiff.value.( LazyCost(x, x, c) )
    C_yy = LazyCost(y, y, c)

    VC[:u,V] .= u₀
    VC[:v,V] .= v₀
    if SP.debias
        VC[:log_d₁,V] .= log_d₁₀
        VC[:log_d₂,V] .= log_d₂₀
    end

    for l in 1:SP.L
        sinkhorn_step_log!(VC[:u,V], VC[:v,V], C_xy, εinv, log_α, VC[:t1,V]) # u = log.(α./Kb)
        sinkhorn_step_log!(VC[:v,V], VC[:u,V], C_xy', εinv, log_β, VC[:t1,V]) # v = log.(β./Ka) assuming K = Kᵀ

        if SP.debias # && (l%2==0 || l==SP.L)
            VC[:t3,V] .= VC[:log_d₁,V]
            VC[:t2,V] .= VC[:log_d₁,V] .+ log_α
            sinkhorn_step_log!(VC[:log_d₁,V], VC[:t3,V], C_xx, εinv, VC[:t2,V], VC[:t1,V]) .*= 0.5

            VC[:t3,V] .= VC[:log_d₂,V]
            VC[:t2,V] .= VC[:log_d₂,V] .+ log_β
            sinkhorn_step_log!(VC[:log_d₂,V], VC[:t3,V], C_yy, εinv, VC[:t2,V], VC[:t1,V]) .*= 0.5
        end
    end

    C_xy = LazyCost(x, y, c)
    C_xx = LazyCost(x, x, c)

    S_ε = 0

    # calculate ε⟨α,u-d₁⟩ + ε⟨β,v-d₂⟩ - ε ∑ e^(u⊕v-C/ε) + 0.5ε ∑ e^(d₁⊕d₁-C/ε) + 0.5ε ∑ e^(d₂⊕d₂-C/ε)

    for i in eachindex(log_α)
        S_ε += SP.ε * exp(log_α[i]) * VC[:u,V][i]
        if SP.debias
            S_ε -= SP.ε * exp(log_α[i]) * VC[:log_d₁,V][i]
            for i_ in eachindex(log_α)
                S_ε += 0.5*SP.ε * exp( VC[:log_d₁,V][i] + VC[:log_d₁,V][i_] - C_xx[i,i_]*εinv )
            end
        end
        for j in eachindex(log_β)
            S_ε -= SP.ε * exp( VC[:u,V][i] + VC[:v,V][j] - C_xy[i,j] * εinv )
        end
    end

    for j in eachindex(log_β)
        S_ε += SP.ε * exp(log_β[j]) * VC[:v,V][j]
        if SP.debias
            S_ε -= SP.ε * exp(log_β[j]) * VC[:log_d₂,V][j]
            for j_ in eachindex(log_β)
                S_ε += 0.5*SP.ε * exp( VC[:log_d₂,V][j] + VC[:log_d₂,V][j_] - C_yy[j,j_]*εinv )
            end
        end
    end

    if SP.update_potentials
        u₀ .= VC[:u,V]
        v₀ .= VC[:v,V]
        if SP.debias
            log_d₁₀ .= VC[:log_d₁,V]
            log_d₂₀ .= VC[:log_d₂,V]
        end
    end

    return S_ε
end


module TestTwoInertiasAndIdealGearWithUnitsAndUncertainties

using TinyModia
using DifferentialEquations
using ModiaPlot
using Measurements
using Unitful

TwoInertiasAndIdealGear = Model(
    J1 = 0.0025u"kg*m^2",
    J2 = (150.0 ± 20.0)u"kg*m^2",
    r  = 105.0,
    tau_max = 1u"N*m",

    phi2 = Var(start = (0.5 ± 0.05)u"rad"),
    w2   = Var(start = 0.0u"rad/s"),
    tau2 = Var(start = 0u"N*m"),

    equations = :[
        tau = if time < 1u"s"; tau_max elseif time < 2u"s"; 0u"N*m" elseif time < 3u"s"; -tau_max else 0u"N*m" end,

        # inertia1
        w1 = der(phi1),
        J1*der(w1) = tau - tau1,

        # gear
        phi1   = r*phi2,
        r*tau1 = tau2,

        # inertia2
        w2 = der(phi2),
        J2*der(w2) = tau2
    ]
)

twoInertiasAndIdealGear = @instantiateModel(TwoInertiasAndIdealGear, FloatType = Measurement{Float64})

simulate!(twoInertiasAndIdealGear, Tsit5(), stopTime = 4.0, logParameters=true, logStates=true)

plot(twoInertiasAndIdealGear, ["phi2", "w2", "der(w2)"])

end


# homogeneous velocity and density model
nz = 101; nx = 201;
vel = 3000 * ones(nz, nx);  # m/s
vel[51:end,:] .= 3500;  # m/s
rho = 2000 * ones(nz, nx);  # kg/m^3

# vertical and horizontal grid size
dz = 10; dx = 10;

# time step size and maximum modelling length
dt = 0.001; tmax = 1.0;  # use second as unit

# source location
isx1 = floor(Int64, nx/2); isz1 = 2;
isx2 = collect(5:40:nx-5); ns=length(isx2); isz2 = 2*ones(ns);

test_type = [(isz1, isx1, true , Float32, 1.0e-5 ),
             (isz1, isx1, true , Float64, 1.0e-10),
             (isz1, isx1, false, Float32, 1.0e-5 ),
             (isz1, isx1, false, Float64, 1.0e-10),
             (isz2, isx2, true , Float32, 1.0e-5 ),
             (isz2, isx2, true , Float64, 1.0e-10),
             (isz2, isx2, false, Float32, 1.0e-5 ),
             (isz2, isx2, false, Float64, 1.0e-10),
            ]
@testset "forward modelling and pressure field reconstruction (forward and backward)" begin

    for (isz, isx, free_surface, data_format, tol) in test_type

        # generate finite difference stencil
        params = TdParams(rho, vel, free_surface, dz, dx, dt, tmax;
                 data_format=data_format, fd_flag="taylor", order=3, npml=20, apml=900.);

        # create source term
        ns = length(isz)
        if ns == 1
           source_term = Source(isz, isx, params; ot=0.0, fdom=15.0, amp=100000);

        elseif ns > 1
           source_term = get_multi_sources(isz, isx, params; ot=0.3*rand(ns), fdom=15.0, amp=100000);
        end

        # save the pressure field and boundary value
        path_pre  = joinpath(homedir(), "Desktop/pressure.rsf");
        path_bnd  = joinpath(homedir(), "Desktop/boundary_value.rsf");
        path_lwfd = joinpath(homedir(), "Desktop/last_wfd.rsf");

        # forward modeling of simultaneous sources
        multi_step_forward(source_term, params; path_pre=path_pre, path_bnd=path_bnd, path_lwfd=path_lwfd);
        (hdr, pre1) = read_RSdata(path_pre);

        # reconstruct pressure field using boundary infomation
        pre2 = pressure_reconstruct_forward(path_bnd, source_term, params);

        # reconstruct pressure field using boundary infomation
        pre3 = pressure_reconstruct_backward(path_bnd, path_lwfd, source_term, params);

        # compute the difference
        @test sqrt(sum((pre1 .- pre2) .^ 2)) / sqrt(sum(pre1 .^ 2)) < tol
        @test sqrt(sum((pre1 .- pre3) .^ 2)) / sqrt(sum(pre1 .^ 2)) < tol
    end
end


using Test
using StaticArrays
using MieSeries

# cross product
mycross(a,b) = SVector(a[2]*b[3]-a[3]*b[2],
                       a[3]*b[1]-a[1]*b[3],
                       a[1]*b[2]-a[2]*b[1])
# norm
mynorm(a) = (b=abs.(a); sqrt(b[1]^2+b[2]^2+b[3]^2))

@testset "Test boundary condition" begin
    TOL = 1e-7     # tolerance
    npoints = 10   # points per dimension in mesh
    n_terms = 80   # number of terms in Mie series
    for a in [1, 5]
        # spherical mesh
        θrange = range(0, 2π, length=npoints)[1:end-1]
        ϕrange = range(0, π, length=npoints)[2:end-1]
        normals = [SVector(sin(ϕ)*cos(θ),
                        sin(ϕ)*sin(θ),
                        cos(ϕ)) for ϕ in ϕrange for θ in θrange]
        mesh = a.*normals
        for k in [1,5,10]
            Es = mieseries(mesh; k, a, n_terms)  # scattered field
            Ei = incident_planewave.(mesh; k)     # incident field
            Et = Es + Ei     # total field
            # test boundary condition
            # n × Eᵗ = 0  on Γ
            for i in eachindex(Es)
                n = normals[i]
                @test mynorm(mycross(n, Et[i])) < TOL
            end
            # test conjugation
            @test conj(Es) == mieseries(mesh; k, a, n_terms, expjω=true)
            @test conj(Ei) == incident_planewave.(mesh; k, expjω=true)
        end
    end
end

@testset "Far field: monostatic RCS" begin
    TOL = 1e-3     # tolerance
    n_terms = 80   # number of terms in Mie series
    a = 0.1        # sphere radius

    obs_point = SVector(1e-7, 1e-7, -1.)          # observation point
    radius_in_λ = range(0.01, 1.6, length=50)
    λ_list = a ./ radius_in_λ                     # wavelengths
    for λ in λ_list
        k = 2π/λ       # wavenumber
        E_far = MieSeries.mieseries(obs_point; k, a, n_terms, farfield=true)
        E_far_norm2 = sum(abs2.(E_far))
        σ_mono = 4*π*E_far_norm2
        @test abs(σ_mono-MieSeries.sphere_monostatic_rcs(;k, a, n_terms)) < TOL
    end
end

@testset "Mie series debug mode" begin
    a = 1          # sphere radius
    n_terms = 80   # number of terms in Mie series
    k = 5.5        # wavenumber
    point = SVector(3.0, -5.0, 6.0)

    Es = mieseries(point; k, a, n_terms)
    Ex_terms, Ey_terms, Ez_terms = MieSeries.mieseries_debug(point; k, a, n_terms)
    @test length(Ex_terms) == n_terms
    @test length(Ey_terms) == n_terms
    @test length(Ez_terms) == n_terms
    @test sum(Ex_terms) ≈ Es[1]
    @test sum(Ey_terms) ≈ Es[2]
    @test sum(Ez_terms) ≈ Es[3]
end


function solveKnapHeur(data::KnapData)
    perm = sortperm(data.items, by = x -> x.profit / x.weight)

    solution = Int64[]
    best_val = 0
    capacity = data.capacity
    for index in perm
        if data.items[index].weight <= capacity
            push!(solution, index)
            best_val += data.items[index].profit
            capacity -= data.items[index].weight
        end
    end

    return best_val, solution
end


using Plots

include("src/pwl_approx.jl")

## Define a submodular cardinality based function via Combined gadget
k = 2
w = random_scb_function(k::Int64)

w = 10*w/maximum(w)
pl = scatter(0:k,w,legend = false, xticks = 0:k)
epsi = 10.0
z0, zk, a, b, cgf = SubCardFun_to_CGF_weights(w,epsi,false)
Jtrue = length(a) + 1
J = Jtrue
xs = 0:0.01:k
ys = cgf(xs)
plot!(pl,xs,ys,color= :blue)

# Refine to get better approximation
z0l, zkl, al, bl, cgfl, best_eps = Refined_SCB_to_CGF(w,epsi)
xs = 0:0.01:k
ys = cgfl(xs)
plot!(pl,xs,ys,color= :red)


function part1(input)
    weights = reverse(parse.(Int, eachline(input)))
    best = [length(weights) + 1, 0]
    find_best_partition(weights, best, sum(weights) ÷ 3)
    return last(best)
end

function part2(input)
    weights = reverse(parse.(Int, eachline(input)))
    best = [length(weights) + 1, 0]
    find_best_partition(weights, best, sum(weights) ÷ 4)
    return last(best)
end

function find_best_partition(weights, best, missing_weight, next = 1,
                             placed = zeros(Int, length(weights)))
    for i = next:length(weights)
        placed[i] != 0 && continue
        if weights[i] == missing_weight
            n = sum(placed) + 1
            qe = prod(weights[placed .== 1]) * weights[i]
            if [n, qe] < best
                best .= [n, qe]
            end
            return
        end
        weights[i] > missing_weight && continue
        weights[i] * (first(best) - sum(placed)) < missing_weight && continue
        placed[i] = 1
        find_best_partition(weights, best, missing_weight - weights[i],
                            i + 1, placed)
        placed[i] = 0
    end
end


"""
    TriangularDist(a,b,c)

The *triangular distribution* with lower limit `a`, upper limit `b` and mode `c` has probability density function

```math
f(x; a, b, c)= \\begin{cases}
        0 & \\mathrm{for\\ } x < a, \\\\
        \\frac{2(x-a)}{(b-a)(c-a)} & \\mathrm{for\\ } a \\le x \\leq c, \\\\[4pt]
        \\frac{2(b-x)}{(b-a)(b-c)} & \\mathrm{for\\ } c < x \\le b, \\\\[4pt]
        0 & \\mathrm{for\\ } b < x,
        \\end{cases}
```

```julia
TriangularDist(a, b)        # Triangular distribution with lower limit a, upper limit b, and mode (a+b)/2
TriangularDist(a, b, c)     # Triangular distribution with lower limit a, upper limit b, and mode c

params(d)       # Get the parameters, i.e. (a, b, c)
minimum(d)      # Get the lower bound, i.e. a
maximum(d)      # Get the upper bound, i.e. b
mode(d)         # Get the mode, i.e. c
```

External links

* [Triangular distribution on Wikipedia](http://en.wikipedia.org/wiki/Triangular_distribution)

"""
struct TriangularDist{T<:Real} <: ContinuousUnivariateDistribution
    a::T
    b::T
    c::T
    TriangularDist{T}(a::T, b::T, c::T) where {T <: Real} = new{T}(a, b, c)
end

function TriangularDist(a::T, b::T, c::T; check_args=true) where {T <: Real}
    check_args && @check_args(TriangularDist, a <= c <= b)
    return TriangularDist{T}(a, b, c)
end

TriangularDist(a::T, b::T) where {T <: Real} = TriangularDist(a, b, middle(a, b))

TriangularDist(a::Real, b::Real, c::Real) = TriangularDist(promote(a, b, c)...)
TriangularDist(a::Integer, b::Integer, c::Integer) = TriangularDist(float(a), float(b), float(c))
TriangularDist(a::Real, b::Real) = TriangularDist(promote(a, b)...)
TriangularDist(a::Integer, b::Integer) = TriangularDist(float(a), float(b))

@distr_support TriangularDist d.a d.b

#### Conversions
convert(::Type{TriangularDist{T}}, a::Real, b::Real, c::Real) where {T<:Real} = TriangularDist(T(a), T(b), T(c))
convert(::Type{TriangularDist{T}}, d::TriangularDist{S}) where {T<:Real, S<:Real} = TriangularDist(T(d.a), T(d.b), T(d.c), check_args=false)

#### Parameters

params(d::TriangularDist) = (d.a, d.b, d.c)
partype(::TriangularDist{T}) where {T<:Real} = T

#### Statistics

mode(d::TriangularDist) = d.c

mean(d::TriangularDist) = (d.a + d.b + d.c) / 3

function median(d::TriangularDist)
    (a, b, c) = params(d)
    m = middle(a, b)
    c >= m ? a + sqrt((b - a) * (c - a)/2) :
             b - sqrt((b - a) * (b - c)/2)
end

_pretvar(a::Real, b::Real, c::Real) = a*a + b*b + c*c - a*b - a*c - b*c

function var(d::TriangularDist)
    (a, b, c) = params(d)
    _pretvar(a, b, c) / 18
end

function skewness(d::TriangularDist{T}) where T<:Real
    (a, b, c) = params(d)
    sqrt2 * (a + b - 2c) * (2a - b - c) * (a - 2b + c) / ( 5 * _pretvar(a, b, c)^(T(3)/2) )
end

kurtosis(d::TriangularDist{T}) where {T<:Real} = T(-3)/5

entropy(d::TriangularDist{T}) where {T<:Real} = one(T)/2 + log((d.b - d.a) / 2)

#### Evaluation

function pdf(d::TriangularDist{T}, x::Real) where T<:Real
    (a, b, c) = params(d)
    x <= a ? zero(T) :
    x <  c ? 2 * (x - a) / ((b - a) * (c - a)) :
    x == c ? 2 / (b - a) :
    x <= b ? 2 * (b - x) / ((b - a) * (b - c)) : zero(T)
end

function cdf(d::TriangularDist{T}, x::Real) where T<:Real
    (a, b, c) = params(d)
    x <= a ? zero(T) :
    x <  c ? (x - a)^2 / ((b - a) * (c - a)) :
    x == c ? (c - a) / (b - a) :
    x <= b ? 1 - (b - x)^2 / ((b - a) * (b - c)) : one(T)
end

function quantile(d::TriangularDist, p::Real)
    (a, b, c) = params(d)
    c_m_a = c - a
    b_m_a = b - a
    rl = c_m_a / b_m_a
    p <= rl ? a + sqrt(b_m_a * c_m_a * p) :
              b - sqrt(b_m_a * (b - c) * (1 - p))
end

function mgf(d::TriangularDist{T}, t::Real) where T<:Real
    if t == zero(t)
        return one(T)
    else
        (a, b, c) = params(d)
        u = (b - c) * exp(a * t) - (b - a) * exp(c * t) + (c - a) * exp(b * t)
        v = (b - a) * (c - a) * (b - c) * t^2
        return 2u / v
    end
end

function cf(d::TriangularDist{T}, t::Real) where T<:Real
    # Is this correct?
    if t == zero(t)
        return one(Complex{T})
    else
        (a, b, c) = params(d)
        u = (b - c) * cis(a * t) - (b - a) * cis(c * t) + (c - a) * cis(b * t)
        v = (b - a) * (c - a) * (b - c) * t^2
        return -2u / v
    end
end

#### Sampling

function rand(rng::AbstractRNG, d::TriangularDist)
    (a, b, c) = params(d)
    b_m_a = b - a
    u = rand(rng)
    b_m_a * u < (c - a) ? d.a + sqrt(u * b_m_a * (c - a)) :
                          d.b - sqrt((1 - u) * b_m_a * (b - c))
end