# This file contains functions/methods related to images/image generation.

"""
    contraststretch!(image::Matrix{Float64};
                     minval::Float64 = 0.0, maxval::Float64 = 1.0)

Stretch the histogram of `image` to fill the range `minval` to `maxval`.

# Inputs
-`image`: Image to be contrast stretched.
-`minval`: Minimum value of pixels in `image` once stretched.
-`maxval`: Maximum value of pixels in `image` once stretched.
"""
function contraststretch!(image::Matrix{Float64};
    minval::Float64 = 0.0, maxval::Float64 = 1.0)
    # Stretch the image to the range [0.0, 1.0].
    image .-= StatsBase.minimum(image)
    image ./= StatsBase.maximum(image)
    image = (maxval - minval) * image .+ minval

    return image
end

"""
    outimage = contraststretch(image::Matrix{Float64};
                            minval::Float64 = 0.0, maxval::Float64 = 1.0)

Stretch the histogram of `image` to fill the range `minval` to `maxval`.

# Inputs
-`image`: Image to be contrast stretched.
-`minval`: Minimum value of pixels in `image` once stretched.
-`maxval`: Maximum value of pixels in `image` once stretched.

# Outputs
-`outimage`: Contrast stretched copy of input `image`.
"""
function contraststretch(image::Matrix{Float64};
    minval::Float64 = 0.0, maxval::Float64 = 1.0)
    return SMLMData.contraststretch!(deepcopy(image);
        minval = minval, maxval = maxval)
end

"""
    image = makecircleim(coords::Matrix{Float64},
                         σ::Vector{Float64},
                         datasize::Vector{Int},
                         mag::Float64 = 20.0)

Make a circle image of the localizations in `coords`.

# Description
This function creates an image of the localizations in `coords` by adding a
circle centered at the locations `coords` with radii `σ`.

# Inputs
-`coords`: Localization coordinates. ([y x])
-`σ`: Standard error of localizations in `coords`. (nlocx1)
-`datasize`: Size of the data image. ([ysize xsize])
-`mag`: Approximate magnfication from data coordinates to SR coordinates.
        (Default = 20.0)

# Outputs
-`image`: Matrix{Float64} histogram image of localizations.
"""
function makecircleim(coords::Matrix{Float64},
    σ::Vector{Float64},
    datasize::Vector{Int},
    mag::Float64 = 20.0)
    # Rescale the coordinates based on `mag`.
    coords = mag * (coords .- 0.5) .+ 0.5
    σ *= mag

    # Loop through localizations and add them to our output image.
    imagesize = Int.(round.(datasize * mag))
    image = zeros(Float64, imagesize[1], imagesize[2])
    for nn = 1:size(coords, 1)
        # If σ[nn] isn't positive, skip this localization.
        if !(σ[nn] > 0.0)
            continue
        end

        # Define the pixel locations that fall along the circle.
        # NOTE: The extra factor of 4 improves circle appearance.
        θ = range(0, 2 * pi, length = max(4, Int(ceil(4 * (2 * pi * σ[nn])))))
        rows = Int.(round.(coords[nn, 1] .+ σ[nn] * sin.(θ)))
        cols = Int.(round.(coords[nn, 2] .+ σ[nn] * cos.(θ)))
        validind = findall((rows .>= 1) .* (rows .< imagesize[1]) .*
                           (cols .>= 1) .* (cols .< imagesize[1]))

        # Set the pixels of the output image to 1.0 wherever met by the circle.
        for ii in validind
            image[rows[ii], cols[ii]] = 1.0
        end
    end

    return image
end

"""
    image = makecircleim(smld::SMLMData.SMLD2D, mag::Float64 = 20.0)

Make a circle image of the localizations in `smld`.

# Description
This function creates an image of the localizations in `smld` by adding a
circle for each localization.

# Inputs
-`smld`: SMLMData.SMLD2D data structure containing localizations.
-`mag`: Approximate magnfication from data coordinates to SR coordinates.
        (Default = 20.0)

# Outputs
-`image`: Matrix{Float64} circle image of localizations.
"""
function makecircleim(smld::SMLMData.SMLD2D, mag::Float64 = 20.0)
    coords = [smld.y smld.x]
    σ = vec(mean([smld.σ_y smld.σ_x], dims = 2))
    return SMLMData.makecircleim(coords, σ, smld.datasize, mag)
end

"""
    image = circleim(smld::SMLMData.SMLD2D, pxsize::Float64;
                     pxsize_out::Float64 = 0.005)

Generate and save a circle image.

# Description
This method is a wrapper for makecircleim() with modified inputs.

# Inputs
-`smld`: SMLMData.SMLD2D data structure containing localizations.
-`pxsize`: Pixel size of localizations in `smld`. (micrometers)
-`pxsize_out`: Desired output pixel size. (micrometers)
"""
function circleim(smld::SMLMData.SMLD2D, pxsize::Float64;
    pxsize_out::Float64 = 0.005)
    # Determine the requested magnfication factor.
    mag = pxsize / pxsize_out

    # Generate the circle image.
    image = SMLMData.makecircleim(smld, mag)

    # Perform a contrast stretch so that the pixels fills the range [0, 1].
    SMLMData.contraststretch!(image)

    return image
end

"""
    image = circleim(smld::SMLMData.SMLD2D, pxsize::Float64, filename::String;
                     pxsize_out::Float64 = 0.005)

Generate and save a circle image.

# Description
This method is a wrapper for makecircleim() which uses different inputs
arguments and saves the image.

# Inputs
-`smld`: SMLMData.SMLD2D data structure containing localizations.
-`pxsize`: Pixel size of localizations in `smld`. (micrometers)
-`pxsize_out`: Desired output pixel size. (micrometers)
"""
function circleim(smld::SMLMData.SMLD2D, pxsize::Float64, filename::String;
    pxsize_out::Float64 = 0.005)
    # Generate the circle image.
    image = SMLMData.circleim(smld, pxsize; pxsize_out = pxsize_out)

    # Save the image.
    save(filename, Images.colorview(Gray, image))

    return image
end

"""
    image = makegaussim(μ::Matrix{Float64},
                        σ_μ::Matrix{Float64},
                        datasize::Vector{Int};
                        mag::Float64 = 20.0,
                        nsigma::Float64 = 5.0)

Make a Gaussian image of the localizations in `μ`.

# Description
This function creates an image of the localizations defined by `μ` and `σ_μ`
in which Gaussians with standard deviations `σ_μ` are added at positions `μ`.

# Inputs
-`μ`: Positions of the Gaussians. (pixels)([y x])
-`σ_μ`: Standard errors of `μ` estimates. (pixels)([y x])
-`datasize`: Size of the region of data collection. (pixels)([y; x])
-`mag`: Approximate magnfication from data coordinates to SR coordinates.
        (Default = 20.0)
-`nsigma`: Number of standard deviations from the localization coordinate at
           which we truncate the Gaussian. (Default = 5.0)

# Outputs
-`image`: Matrix{Float64} Gaussian image in which each localization in `smld`
          is plotted as a Gaussian.
"""
function makegaussim(μ::Matrix{Float64},
    σ_μ::Matrix{Float64},
    datasize::Vector{Int};
    mag::Float64 = 20.0,
    nsigma::Float64 = 5.0)
    # Loop through emitters and add them to our output Gaussian image.
    imagesize = Int.(round.(datasize * mag))
    image = zeros(Float64, imagesize[1], imagesize[2])
    for nn = 1:size(μ, 1)
        # Prepare a normal distribution for this emitter.
        if !all(σ_μ[nn, :] .> 0.0)
            continue
        end
        Σ = [σ_μ[nn, 1]^2 0.0; 0.0 σ_μ[nn, 2]^2]
        distrib = Distributions.MvNormal(μ[nn, :], Σ)

        # Loop through pixels of the image and add this emitter.
        ystart = max(1,
            Int(round(mag * (μ[nn, 1] - nsigma * σ_μ[nn, 2] - 0.5))))
        yend = min(imagesize[1],
            Int(round(mag * (μ[nn, 1] + nsigma * σ_μ[nn, 2]))))
        xstart = max(1,
            Int(round(mag * (μ[nn, 2] - nsigma * σ_μ[nn, 1] - 0.5))))
        xend = min(imagesize[2],
            Int(round(mag * (μ[nn, 2] + nsigma * σ_μ[nn, 1]))))
        for ii = ystart:yend, jj = xstart:xend
            image[ii, jj] +=
                Distributions.pdf(distrib, ([ii; jj] .- 0.5) / mag .+ 0.5)
        end
    end

    # Normalize the image to sum to 1.0.  If any image values are NaN, set them
    # to 0.0.
    nanpixels = isnan.(image)
    if any(nanpixels)
        image[nanpixles] .= 0.0
    end
    image = image ./ sum(image)
    if !isapprox(sum(image), 1.0)
        @warn "Image is non-normalizable!  Returning flat image."
        image = ones(Float64, imagesize[1], imagesize[2]) ./ prod(imagesize)
    end

    return image
end

"""
    image = makegaussim(smld::SMLMData.SMLD2D;
                        mag::Float64 = 20.0,
                        nsigma::Float64 = 5.0)

Make a Gaussian image of the localizations in `smld`.

# Description
This function creates an image of the localizations in `smld` by placing a
Gaussian truncated to `nsigma` at the localization coordinates, where the
standard deviation is given by `smld.σ_x` and `smld.σ_y`.  The image is then
normalized such that it sums to 1.0.  The background signal is not accounted
for in this method.

# Inputs
-`smld`: SMLMData.SMLD2D data structure containing localizations.
-`mag`: Approximate magnfication from data coordinates to SR coordinates.
        (Default = 20.0)
-`nsigma`: Number of standard deviations from the localization coordinate at
           which we truncate the Gaussian. (Default = 5.0)

# Outputs
-`image`: Matrix{Float64} Gaussian image in which each localization in `smld`
          is plotted as a Gaussian.
"""
function makegaussim(smld::SMLMData.SMLD2D;
    mag::Float64 = 20.0,
    nsigma::Float64 = 5.0)
    return SMLMData.makegaussim([smld.y smld.x], [smld.σ_y smld.σ_x],
        smld.datasize; mag = mag, nsigma = nsigma)
end

"""
    image = gaussim(smld::SMLMData.SMLD2D, pxsize::Float64;
                    pxsize_out::Float64 = 0.005,
                    prctileceiling::Float64 = 99.5,
                    nsigma::Float64 = 5.0)

Generate a user-friendly (i.e., scaled and thresholded) Gaussian image.

# Description
This method is a wrapper for makegaussim() which generates a more user-friendly
Gaussian image.  That is, this method calls makegaussim(), applies a percentile
ceiling to the results, and then performs a contrast stretch so its pixels
occupy the range [0.0, 1.0].

# Inputs
-`smld`: SMLMData.SMLD2D data structure containing localizations.
-`pxsize`: Pixel size of localizations in `smld`. (micrometers)
-`pxsize_out`: Desired output pixel size. (micrometers)
-`prctileceiling`: Upper percentile used to threshold the pixel values of
                   particularly bright pixels.
-`nsigma`: Number of standard deviations from the localization coordinate at
           which we truncate the Gaussian. (Default = 5.0)
"""
function gaussim(smld::SMLMData.SMLD2D, pxsize::Float64;
    pxsize_out::Float64 = 0.005,
    prctileceiling::Float64 = 99.5,
    nsigma::Float64 = 5.0)
    # Determine the requested magnfication factor.
    mag = pxsize / pxsize_out

    # Generate a "raw" Gaussian image (i.e., no scaling or thresholding).
    image = SMLMData.makegaussim(smld; mag = mag, nsigma = nsigma)

    # Apply a percentile ceiling to improve contrast.
    upperbound = StatsBase.percentile(image[:], prctileceiling)
    image[image.>upperbound] .= upperbound

    # Perform a contrast stretch so that the pixels fills the range [0, 1].
    SMLMData.contraststretch!(image)

    return image
end

"""
    image = gaussim(smld::SMLMData.SMLD2D, pxsize::Float64, filename::String;
                    pxsize_out::Float64 = 0.005,
                    prctileceiling::Float64 = 99.5,
                    nsigma::Float64 = 5.0)

Generate and save a user-friendly (i.e., scaled and thresholded) Gaussian image.

# Description
This method is a wrapper for makegaussim() which generates a more user-friendly
Gaussian image.  That is, this method calls makegaussim(), applies a percentile
ceiling to the results, and then performs a contrast stretch so its pixels
occupy the range [0.0, 1.0].  This method of gaussim() saves the image in the
user-specified `filename`.

# Inputs
-`smld`: SMLMData.SMLD2D data structure containing localizations.
-`pxsize`: Pixel size of localizations in `smld`. (micrometers)
-`filename`: String specifying the location of the output image.
-`pxsize_out`: Desired output pixel size. (micrometers)
-`prctileceiling`: Upper percentile used to threshold the pixel values of
                   particularly bright pixels.
-`nsigma`: Number of standard deviations from the localization coordinate at
           which we truncate the Gaussian. (Default = 5.0)
"""
function gaussim(smld::SMLMData.SMLD2D, pxsize::Float64, filename::String;
    pxsize_out::Float64 = 0.005,
    prctileceiling::Float64 = 99.5,
    nsigma::Float64 = 5.0)
    # Prepare the image.
    image = SMLMData.gaussim(smld, pxsize;
        pxsize_out = pxsize_out,
        prctileceiling = prctileceiling,
        nsigma = nsigma)

    # Create a color image.
    redchannel(x) = min(1.0, 10.0 * x)
    greenchannel(x) = min(1.0, max(10.0 * (x - 0.4), 0.0))
    bluechannel(x) = min(1.0, max(5.0 * (x - 0.7), 0.0))
    imageRGB = Colors.RGB.(redchannel.(image),
        greenchannel.(image),
        bluechannel.(image))

    # Save the image.
    Images.save(filename, imageRGB)

    return image
end

"""
    image = makebinim(coords::Matrix{Float64},
                      datasize::Vector{Float64},
                      mag::Float64 = 20.0)

Make a binary image of the localizations in `coords`.

# Description
This function creates an image of the localizations in `coords` by placing a
hot pixel at the coordinates of each localization.

# Inputs
-`coords`: Localization coordinates. ([y x])
-`datasize`: Size of the data image. ([ysize xsize])
-`mag`: Approximate magnfication from data coordinates to SR coordinates.
        (Default = 20.0)

# Outputs
-`image`: Matrix{Float64} binary image of localizations.
"""
function makebinim(coords::Matrix{Float64},
    datasize::Vector{Int},
    mag::Float64 = 20.0)
    # Loop through localizations and add them to our output binary image.
    imagesize = Int.(round.(datasize * mag))
    image = zeros(Float64, imagesize[1], imagesize[2])
    inds = max.(1.0, (coords .- 0.5) * mag)
    inds[:, 1] = min.(imagesize[1], inds[:, 1])
    inds[:, 2] = min.(imagesize[2], inds[:, 2])
    inds = Int.(round.(inds))
    for nn = 1:size(coords, 1)
        image[inds[nn, 1], inds[nn, 2]] = 1.0
    end

    # Normalize the image to sum to 1.0.
    image = image ./ max(sum(image), 1.0)
    if !isapprox(sum(image), 1.0)
        @warn "Image is non-normalizable!  Returning flat image."
        image = ones(Float64, imagesize[1], imagesize[2]) ./ prod(imagesize)
    end

    return image
end

"""
    image = makebinim(smld::SMLMData.SMLD2D, mag::Float64 = 20.0)

Make a binary image of the localizations in `smld`.

# Description
This function creates an image of the localizations in `smld` by placing a
hot pixel at the coordinates of each localization.

# Inputs
-`smld`: SMLMData.SMLD2D data structure containing localizations.
-`mag`: Approximate magnfication from data coordinates to SR coordinates.
        (Default = 20.0)

# Outputs
-`image`: Matrix{Float64} binary image of localizations.
"""
function makebinim(smld::SMLMData.SMLD2D, mag::Float64 = 20.0)
    return makebinim([smld.y smld.x], smld.datasize, mag)
end

"""
    image = makehistim(coords::Matrix{Float64},
                       datasize::Vector{Int},
                       mag::Float64 = 20.0)

Make a histogram image of the localizations in `coords`.

# Description
This function creates an image of the localizations in `coords` by adding 1.0
to a pixel for each localization present within that pixel.  The final image
is then scaled so that it sums to 1.0.

# Inputs
-`coords`: Localization coordinates. ([y x])
-`datasize`: Size of the data image. ([ysize xsize])
-`mag`: Approximate magnfication from data coordinates to SR coordinates.
        (Default = 20.0)

# Outputs
-`image`: Matrix{Float64} histogram image of localizations.
"""
function makehistim(coords::Matrix{Float64},
    datasize::Vector{Int},
    mag::Float64 = 20.0)
    # Loop through localizations and add them to our output image.
    imagesize = Int.(round.(datasize * mag))
    image = zeros(Float64, imagesize[1], imagesize[2])
    inds = max.(1.0, (coords .- 0.5) * mag)
    inds[:, 1] = min.(imagesize[1], inds[:, 1])
    inds[:, 2] = min.(imagesize[2], inds[:, 2])
    inds = Int.(round.(inds))
    for nn = 1:size(coords, 1)
        image[inds[nn, 1], inds[nn, 2]] += 1.0
    end

    # Normalize the image to sum to 1.0.
    image = image ./ max(sum(image), 1.0)
    if !isapprox(sum(image), 1.0)
        @warn "Image is non-normalizable!  Returning flat image."
        image = ones(Float64, imagesize[1], imagesize[2]) ./ prod(imagesize)
    end

    return image
end

"""
    image = makehistim(smld::SMLMData.SMLD2D, mag::Float64 = 20.0)

Make a histogram image of the localizations in `smld`.

# Description
This function creates an image of the localizations in `smld` by adding 1.0
to a pixel for each localization present within that pixel.  The final image
is then scaled so that it sums to 1.0.

# Inputs
-`smld`: SMLMData.SMLD2D data structure containing localizations.
-`mag`: Approximate magnfication from data coordinates to SR coordinates.
        (Default = 20.0)

# Outputs
-`image`: Matrix{Float64} histogram image of localizations.
"""
function makehistim(smld::SMLMData.SMLD2D, mag::Float64 = 20.0)
    return makehistim([smld.y smld.x], smld.datasize, mag)
end


import LinearAlgebra,FastGaussQuadrature
import ProgressMeter

"""
    Ω, P = beyn(L::LinearOperatorFamily, Γ; <keyword arguments>)

Compute all eigenvalues of `L` inside the contour `Γ` together with the associated eigenvectors. The contour is given as a list of complex numbers which are interpreted as polygon vertices in the complex plane.  The eigenvalues are stored in the list `Ω` and the eigenvectors are stored in the columns of `P`. The eigenvector `P[:,i]` corresponds to the eigenvalue `Ω[i]`, i.e., they satisfy `L(Ω[i])P[:,i]=0`.

# Arguments
- `L::LinearOperatorFamily`: Definition of the non-linear eigenvalue problem
- `Γ::Array`: List of complex points defining the contour.
- `l::Integer = 5`: estimate of the number of eigenvalues inside of `Γ`.
- `K::Integer = 1`: Augmention dimension if `Γ` is assumed to contain more than `size(L)[1]` eigenvalues.
- `N::Integer = 16`: Number of evaluation points used to perform the Gauss-Legendre integration along each edge of `Γ`.
- `tol::Float=0.0`: Threshold value to discard spurious singular values. If set to `0` (the default) no singular values are discarded.
- `pos_test::bool=true`: If set to `true` perform positions test on the computed eigenvalues, i.e., check whether the eigenvalues are enclosed by `Γ` and disregard all eigenvalues which fail the test.
- `output::bool=true`: Show progressbar if `true`.
- `random::bool=false`: Randomly initialize the V matrix if `true`.

# Returns
- `Ω::Array`: List of computed eigenvalues
- `P::Matrix`: Matrix of eigenvectors.

# Notes
The original algorithm was first presented by Beyn in [1]. The implementation closely follows the pseudocode from Buschmann et al. in [2].

# References
[1] W.-J. Beyn, An integral method for solving nonlinear eigenvalue problems, Linear Algebra and its Applications, 2012, 436(10), p.3839-3863, https://doi.org/10.1016/j.laa.2011.03.030

[2] P.E. Buschmann, G.A. Mensah, J.P. Moeck, Solution of Thermoacoustic Eigenvalue Problems with a Non-Iterative Method, J. Eng. Gas Turbines Power, Mar 2020, 142(3): 031022 (11 pages) https://doi.org/10.1115/1.4045076

See also: [`inveriter`](@ref), [`lancaster`](@ref), [`mslp`](@ref), [`rf2s`](@ref), [`traceiter`](@ref)
"""
function beyn(L::LinearOperatorFamily,Γ;l=5,K=1,N=16,tol=0.0,pos_test=true,output=true,random=false)
    #This is Beyn's algorithm as implemented in Buschmann et al. 2019
    #Beyn's original paper from 2012 also suggests a residue test which might be implemented
    #in the future
    d=size(L(0))[1]
    K=max(K,div(l,d)+Int(mod(l,d)!=0)) #ensure minimum required augmentation
    #initialize V
    if l<d #TODO consider seeding for reproducibility
        if random
            V=rand(ComplexF64,d,l)
        else
            V=zeros(ComplexF64,d,l)
            for i=1:l
                V[i,i]=1.0+0.0im
            end
        end
    else
        #TODO: there might be an easier constructor
        #V=LinearAlgebra.Diagonal(ones(ComplexF64,d))
        V=zeros(ComplexF64,d,l)
        for i=1:d
            V[i,i]=1.0+0.0im
        end
    end

    function integrand(z)
        z,w=z
        A=Array{ComplexF64}(undef,d,l,2*K)
        A[:,:,1]=L(z)\V
        A[:,:,1]*=w
        for p =1:2*K-1
            A[:,:,p+1]=z^p*A[:,:,1]
        end
        return A
    end

    A=zeros(ComplexF64,d,l,2*K) # initialize A with zeros
    A=gauss(A,integrand,Γ,N,output) #integrate over contour

    #Assemble B matrices
    B=Array{ComplexF64}(undef,d*K,l*K,2)
    rows=1:d
    cols=1:l
    for i=0:K-1,j=0:K-1
        B[rows.+d*i,cols.+l*j,1]=A[:,:,i+j+1] #indexshift +1
        B[rows.+d*i,cols.+l*j,2]=A[:,:,i+j+2]
    end

    V,Σ,W=LinearAlgebra.svd(B[:,:,1])
    #sigma check
    if output
        println("############")
        println("singular values:")
        println(Σ)
    end
    if tol>0
        mask=map(σ->σ>tol,Σ)
        V,Σ,W=V[:,mask],Σ[mask],W[:,mask]
    end
    #TODO: rank test

    #invert Σ. It's a diagonal matrix so inversion is just one line
    Σ=LinearAlgebra.Diagonal(1 ./Σ) #TODO: check zero division
    #compute eigenvalues and eigenvectors
    Ω,P = LinearAlgebra.eigen(V'*B[:,:,2]*W*Σ)
    P=V[1:d,:]*P
    #position test
    if pos_test
        mask=map(z->inpoly(z,Γ),Ω)
        Ω,P=Ω[mask],P[:,mask]
    end

    return Ω,P
end

function gauss(int,f,Γ,N,output=false)
    X,W=FastGaussQuadrature.gausslegendre(N)
    lΓ=length(Γ)
    if output
        prog = ProgressMeter.Progress(lΓ*N,desc="Beyn... ", dt=.5,
             barglyphs=ProgressMeter.BarGlyphs('|','█', ['▁' ,'▂' ,'▃' ,'▄' ,'▅' ,'▆', '▇'],' ','|',),
             barlen=30)
    end
    for i = 1:lΓ
        if i==lΓ
            a,b=Γ[i],Γ[1]
        else
            a,b=Γ[i],Γ[i+1]
        end
        X̂=X.*(b-a)/2 .+(a+b)/2 #transform to segment
        #int+=mapreduce(f,+,zip(X̂,W)).*(b-a)/2
        iint=zero(int)
        for z in zip(X̂,W)
            iint+=f(z)
            if output
                ProgressMeter.next!(prog)
            end
        end
        int+=iint.*(b-a)/2
    end
    return int
end

#This is a Julia implementation of the point-polygon-problem as it is
#discussed on http://geomalgorithms.com/a03-_inclusion.html
#The algorithms are adapted for complex arithmetics to seamlessly
#work with beyn

function isleft(a,b,c)
    #c point to be tested
    #a, b points defining the line
    return (real(b)-real(a)) * (imag(c)-imag(a)) - (real(c)-real(a))*(imag(b)-imag(a))
end

# function inpoly(z,Γ)
#     #winding number test
#     wn=0
#     for i=1:length(Γ)
#         if i==length(Γ)
#             a,b=Γ[i],Γ[1]
#         else
#             a,b=Γ[i],Γ[i+1]
#         end
#         if imag(a) <= imag(z)
#             if imag(b)>imag(z)
#                 if isleft(a,b,z)>0
#                     wn+=1
#                 end
#             end
#         else
#             if imag(b) <= imag(z)
#                 if isleft(a,b,z)<0
#                     wn-=1
#                 end
#             end
#         end
#     end
#     return wn!=0
# end

inpoly(z,Γ)= wn(z,Γ)!=0

"""
    w=wn(z,Γ)

Compute winding number, i.e., how often `Γ` is wrapped around `z`. The sign of the winding number indocates the wrapping direction.
"""
function wn(z,Γ)
    #winding number test
    wn=0
    for i=1:length(Γ)
        if i==length(Γ)
            a,b=Γ[i],Γ[1]
        else
            a,b=Γ[i],Γ[i+1]
        end
        if imag(a) <= imag(z)
            if imag(b)>imag(z)
                if isleft(a,b,z)>0
                    wn+=1
                end
            end
        else
            if imag(b) <= imag(z)
                if isleft(a,b,z)<0
                    wn-=1
                end
            end
        end
    end
    return wn
end
## Thats Beyn in pieces...
"""
    A=compute_moment_matrices(L,Γ, <kwargs>)

Compute moment matrices of `L` integrating along `Γ`.

# Arguments
- `L::LinearOperatorFamily`
- `Γ`: List of complex points defining the contour.
- `l::Int=5`: (optional) estimate of the number of eigenvalues inside of `Γ`.
-  `K::Int=1`: (optional) Augmention dimension if `Γ` is assumed to contain more than `size(L)[1]` eigenvalues.
- `N::Int=16`: (optional)  Number of evaluation points used to perform the Gauss-Legendre integration along each edge of `Γ`.
- `output::Bool=false` (optional) toggle waitbar
- `random::Bool=false` (optional) toggle random initialization of V-matrix

# Returns
- `A::Array`: Moment matrices.

# Notes
A is a 3 dimensional array. The last index loops over the various Moments, i.e., `A[:,:,i]=∫_Γ z^i inv(L(z)) V dz`.
If `random`=false V is initialized with ones on its main diagonal. Otherwise it is random.

"""
function compute_moment_matrices(L::LinearOperatorFamily,Γ;l=5,K=1,N=16,output=false,random=false)
    d=size(L(0))[1]
    #initialize V
    if l<d #TODO consider seeding for reproducibility
        if random
            V=rand(ComplexF64,d,l)
        else
            V=zeros(ComplexF64,d,l)
            for i=1:l
                V[i,i]=1.0+0.0im
            end
        end
    else
        #TODO: there might be an easier constructor
        V=LinearAlgebra.Diagonal(ones(ComplexF64,d))
    end
    return compute_moment_matrices(L::LinearOperatorFamily,Γ,V;K=L,N=N,output=output)
end
function compute_moment_matrices(L::LinearOperatorFamily,Γ,V;K=1,N=16,output=false)
    d,l=size(V)

    function integrand(z)
        z,w=z
        A=Array{ComplexF64}(undef,d,l,2*K)
        A[:,:,1]=L(z)\V
        A[:,:,1]*=w
        for p =1:2*K-1
            A[:,:,p+1]=z^p*A[:,:,1]
        end
        return A
    end
    A=zeros(ComplexF64,d,l,2*K) # initialize A with zeros
    A=gauss(A,integrand,Γ,N,output) #integrate over contour

    return A
end
"""
    Ω,P = moments2eigs(A; tol_σ=0.0, return_σ=false)

Compute eigenvalues `Ω` and eigenvectors `P from moment matrices `A`.

# Arguments
- `A::Array`: moment matrices
- `tol::Float=0.0`: tolerance for truncating singular values.
- `return_σ::Bool=false`: if true also return the singular values as third return value.

# Returns
- `Ω::Array`: List of computed eigenvalues
- `P::Matrix`: Matrix of eigenvectors.
- `Σ::Array`: List of singular values (only if `return_σ==true`)

# Notes
The the i-th coloumn in P corresponds to the i-th eigenvalue in `Ω`, i.e., `Ω[i]` and `P[:,i]` are an eigenpair.

See also: [`compute_moment_matrices`](@ref)
"""
function moments2eigs(A;tol_σ=0.0,return_σ=false)
    #Assemble B matrices
    d=size(A[1],1)
    Δl=size(A[1],2)
    l=length(A)*Δl
    K=size(A[1],3)÷2
    B=Array{ComplexF64}(undef,d*K,l*K,2)
    rows=1:d
    cols=1:Δl
    for i=0:K-1,j=0:K-1
        for ll=1:length(A)
            B[rows.+d*i,cols.+(ll-1).*Δl.+l.*j,1].=A[ll][:,:,i+j+1] #indexshift +1
            B[rows.+d*i,cols.+(ll-1).*Δl.+l.*j,2].=A[ll][:,:,i+j+2]
        end
    end
    V,Σ,W=LinearAlgebra.svd(B[:,:,1])
    #sigma check
    if tol_σ>0
        mask=map(σ->σ>tol,Σ)
        V,Σ,W=V[:,mask],Σ[mask],W[:,mask]
    end
    #println("######")
    #println(Σ)
    #invert Σ. It's a diagonal matrix so inversion is just one line
    invΣ=LinearAlgebra.Diagonal(1 ./Σ) #TODO: check zero division
    #compute eigenvalues and eigenvectors
    Ω,P = LinearAlgebra.eigen(V'*B[:,:,2]*W*invΣ)
    P=V[1:d,:]*P

    if return_σ
        return Ω,P,Σ
    else
        return Ω,P
    end
end

"""
    Ω,P=pos_test(Ω,P,Γ)

Filter eigenpairs `Ω` and `P`for those whose eigenvalues lie inside `Γ`.

See also: [`moments2eigs`](@ref)
"""
function pos_test(Ω,P,Γ)
    mask=map(z->inpoly(z,Γ),Ω)
    Ω,P=Ω[mask],P[:,mask]
    return Ω,P
end

## count poles and zeros
"""
    n=count_poles_and_zeros(L,Γ;N=16,output=false)

Count number "n" of poles and zeros of the determinant of `L` inside the contour `Γ`.
The optional parameters `N` and `output`. respectively  determine the order of the
Gauss-Legendre integration and toggle whether output is desplayed.

# Notes
Pole orders count negative while zero order count positive.
The evaluation is based on applying the residue theorem to the determinant.
It utilizes Jacobi's formula to compute the necessary derivatives from a
trace operation. and is therefore only suitable for small problems.

"""
function count_poles_and_zeros(L,Γ;N=16,output=false)
  function integrand(z)
    z,w=z
    LU=SparseArrays.lu(L(z),check=false)
    L1=L(z,1)
    dz=0.0+0.0im
    for idx=1:size(LU)[1]
      dz+=(LU\Array(L1[:,idx]))[idx]
    end
    return dz*w
  end

  return gauss(0.0+0.0im,integrand,Γ,N,output)/2/pi/1.0im
end
## Projection method stuff
"""
    V=initialize_V(d::Int,l::Int,random::Bool=false)

initialioze matrix with `d`rows and `l` colums either as diagonal matrix
featuring ones on the main diagonal and anywhere else or with random entries
(`random`=true).

See also: [`generate_subspace`](@ref)
"""
function initialize_V(d::Int,l::Int;random::Bool=false)
    if random
        V=rand(ComplexF64,d,l)
        for i=1:l
            V[:,i].\=LinearAlgebra.norm(V[:,i])
        end
    else
        V=zeros(ComplexF64,d,l)
        for i=1:l
            V[i,i]=1.0+0.0im
        end
    end
    return V
end
raw"""
    Q,resnorm=generate_subspace(L,Y,tol,Z,N;output=true))

Compute orthonormal `Q` basis for a subspace that can be used to reduce the
dimension of `L`. The created subspace gurantees the residual of `L(z)\V` to be less than
`tol` for each `z∈Z`.

# Arguments
- `L::LinearOperatorFamily`: operator family for which the subspace is to be computed
- `Y::matrix`: matrix for residual test
- `tol::Float64`: tolerance for residual test
- `Z::List`: List of sample points
- `N::Int`: (optional) if set the list Z is interpreted as edges of a closed polygonal contour and N Gauss-Legendre sample points are generated for each edge.
- `output::Bool=false`: (optional) toggle progressbar
- `include_Y::Bool=true`: (optional) include Y in subspace

# Returns

- `Q::Matrix`: unitary matrix whose colums form the basis of the subspace
- `resnorm::List`: list of residuals at the sample points computed during subspace generation. (these values are an upper bound)

# Notes

The algorithm is based on the idea in [1]. However, it uses incremental QR
decompositions to built the subspace and is not computing Beyn's integral.
However, the obtained matrix can be used to project `L` on teh subspace and use
Beyn's algorithm or any other eigenvalue solver on the projected problem.

# References

[1] A Study on Matrix Derivatives for the Sensitivity Analysis of Electromagnetic
Eigenvalue Problems, P. Jorkowski and R. Schuhmann, 2020, IEEE Trans. Magn., 56,
[doi:10.1109/TMAG.2019.2950513](https://doi.org/10.1109/TMAG.2019.2950513)

See also: [`beyn`](@ref), [`initialize_V`](@ref), [`project`](@ref)
"""
function generate_subspace(L,Y,tol,Z;output::Bool=true,tol_err::Float64=Inf,include_Y=true)
    #TODO: sanity checks for sizes
    d,k=size(Y)
    dim=k
    N=length(Z) #IDEA: consider random permutation
    ## initialize subspace from first point
    A=[] #compressed qr storage for incremental qr
    τ=[] #compressed qr storage for incremental qr

    for kk = 1: k
        if include_Y
            qrfactUnblockedIncremental!(τ,A,Y[:,kk:kk])
        else
            qrfactUnblockedIncremental!(τ,A,L(Z[1])\Y[:,kk:kk])
        end
    end
    #QR=LinearAlgebra.qr(L(Z[1])\Y)
    #Q=QR.Q[:,1:dim]# TODO: optimize these QR calls
    resnorm=zeros(N*k)
    idx=0
    Q=get_Q(τ,A) #TODO: incrementally create only the last column, consider preallocation for speed
    QY=Q'*Y #project rhs

    ## Preallacations
    #Y=Array{ComplexF64}(undef,d,1)
    #Y_exact=Array{ComplexF64}(undef,d,1)

    if output
        prog = ProgressMeter.Progress(k*N,desc="Subspace.. ", dt=1,
         barglyphs=ProgressMeter.BarGlyphs('|','█', ['▁' ,'▂' ,'▃' ,'▄' ,'▅' ,'▆', '▇'],' ','|',),)
    end

    for z in Z
        if dim==d
            break
        end
        idx+=1
        Lz=L(z)
        #Lnorm=LinearAlgebra.norm(Lz)
        Lnorm=1
        QLQ=Q'*Lz*Q #projection to subspace
        for kk=1:k
            x=QLQ\QY[:,kk] #solve projected problem
            X=Q*x #backprojection
            #residual test
            #weights=ones(ComplexF64,d)
            #weights[20:end].=0
            #weights=get_weights(Lz)
            weights=1
            res=LinearAlgebra.norm((weights).*(Lz*X-Y[:,kk]))
            res/=Lnorm
            #println("$(kk+(idx-1)*k): $res vs $tol")
            #TODO: check that supdspace does not get linearly dependent
            # in order to enable 0 tolerance
            if res>tol
                # if true# idx<=3
                #     println("vorher:")
                #     println(idx," ",kk+(idx-1)*k," ",res," ",resnorm[5])
                #     println("nacher:")
                #     flush(stdout)
                # end
                dim+=1
                X_exact=Lz\Y[:,kk:kk]
                #err=LinearAlgebra.norm(weights.*(X_exact.-X))
                #println("error:$err")
                X=X_exact
                res=LinearAlgebra.norm((weights).*(Lz*X-Y[:,kk]))
                #if err>tol_err
                #    #tol=max(tol,res/10) #TODO: the minimum operation is not necessary
                #    tol=res*10
                #end

                #res=err
                res/=Lnorm
                #IMPORTANT qrfactUnblockedIncremental! overwrites X!
                #Therfore command must come after residual calculation
                qrfactUnblockedIncremental!(τ,A,X[:,1:1])
                #Q=create_Q(τ,A)
                Q_old,Q=Q,Array{ComplexF64}(undef,d,dim)
                Q[:,1:end-1]=Q_old[:,:]
                #initialize last column with e_dim_vector
                #this is unused but allacated space. Therefore its safe to be used
                # e_dim will be overwritten later to with the true
                # column, but it is used to initialize finding the
                #true column.
                Q[:,end]=zeros(ComplexF64,d)
                Q[dim,end]=1
                Q[:,end]=mult_QV(τ,A,Q[:,end]) #build last column
                ##reinitialize cache
                QLQ=Q'*Lz*Q #projection to subspace #TODO: do this incrementally using last column only
                QY=Q'*Y# project rhs

                #
                # recompute residual for Ale
                x=QLQ\QY[:,kk] #solve projected problem
                X=Q*x #backprojection
                res=LinearAlgebra.norm((weights).*(Lz*X-Y[:,kk]))
                #

                #ΔQLQ=Q*L*ΔQ #only build last colum
                #QV=[QV; ΔQ'*V]
                # if true#idx<=3
                #     println(idx," ",kk+(idx-1)*k," ",res," ",resnorm[5])
                #     flush(stdout)
                # end
            end
            resnorm[kk+(idx-1)*k]=res
            if output
                ProgressMeter.next!(prog)
            end
        end
    end
    #Q=create_Q(τ,A)
    if output
        println("Finished subspace generation!")
        if tol_err<Inf
            println("adapted residual tolerance is $tol .")
        end
    end
    return Q,resnorm
end

function generate_subspace(L,Y,tol,Γ,N::Int;output::Bool=true,tol_err::Float64=Inf,include_Y=true)
    ## generate list of Gauss-Legendre points
    X,W=FastGaussQuadrature.gausslegendre(N)
    lΓ=length(Γ)
    Z=zeros(ComplexF64, lΓ*N) #TODO: undef intialization
    #TODO: remove doubled edge points!
    for i = 1:lΓ
        if i==lΓ
            a,b=Γ[i],Γ[1]
        else
            a,b=Γ[i],Γ[i+1]
        end
        Z[1+(i-1)*N:i*N]=X.*(b-a)/2 .+(a+b)/2 #transform to segment
    end
    return generate_subspace(L,Y,tol,Z,output=output,tol_err=tol_err,include_Y=include_Y)
end

"""
    P::LinearOperatorFamily=project(L::LinearOperatorFamily,Q)

Project `L` on the subspace spanned by the unitary matrix `Q`, i.e.
`P(z)=Q'*L(z)*Q`.

See also: [`generate_subspace`](@ref)
"""
function project(L::LinearOperatorFamily,Q)
    P=LinearOperatorFamily()
    P.params=deepcopy(L.params)
    P.eigval=L.eigval
    P.auxval=L.auxval
    P.mode=deepcopy(L.mode)
    P.active=deepcopy(L.active)

    #term=Term(A2,(pow2,),((:λ,),),"A2")
    for term in L.terms
        M=Q'*term.coeff*Q
        push!(P,Term(M,deepcopy(term.func),deepcopy(term.params),
            deepcopy(term.symbol),deepcopy(term.operator)))
    end
    return P
end

## incremental QR
# TODO: merge request for julia base
#===
hack of line 185 in
https://github.com/JuliaLang/julia/blob/69fcb5745bda8a5588c089f7b65831787cffc366/stdlib/LinearAlgebra/src/qr.jl#L301-L378
for obtaining incremental qr
===#
using LinearAlgebra: reflector!, reflectorApply!
##
function qrfactUnblockedIncremental!(τ,A,V::AbstractMatrix{T}) where {T} #AbstarctArray?
    #require_one_based_indexing(A)
    #require_one_based_indexing(T)
    V=deepcopy(V)
    m=length(A)
    n=length(V)
    #apply all previous reflectors
    for k=1:m
        x = view(A[k], k:n,1)#TODO: consider end
        reflectorApply!(x,τ[k],view(V,k:n,1:1))
    end
    #create new reflector
    x = view(V, m+1:n,1)
    τk= reflector!(x)

    #append latest updates

    #if  !isapprox(V[m+1],0.0+0.0im,atol=1E-8)
        push!(τ,τk)
        push!(A,V)
    #end
    return
end
##
function mult_VQ(τ,A,V)
    m=length(A)
    n=length(A[1])
    #@inbounds begin
        for k=1:m
            τk=τ[k]
            vk=zeros(ComplexF64,n)
            vk[k]=1
            vk[k+1:end]=A[k][k+1:end]
            V=(V-(V*vk)*(vk'*τk))
        end
    #end
    return V
end
function mult_QV(τ,A,V;trans=true)
    m=length(A)
    n=length(A[1])
    M=1:m
    if trans
        M=reverse(M)
    end
    #@inbounds begin
        for k=M #its transposed so householder transforms are executed in reverse order
            τk=τ[k]
            vk=zeros(ComplexF64,n)
            vk[k]=1
            vk[k+1:end]=A[k][k+1:end]
            V=(V-(τk*vk)*(vk'*V))
        end
    #end
    return V
end

function get_Q(τ,A)
    n=size(A[1],1)
    dim=length(τ)
    mult_QV(τ,A,initialize_V(n,dim))
end

function get_R(A)
    n=length(A)
    m=length(A[1])
    R=zeros(ComplexF64,m,n)
    for i=1:n
        R[1:i,i]=A[i][1:i]
    end
    return R
end

function get_weights(M)
    m,n=size(M)
    weights=Array{ComplexF64}(undef,m)
    for i=1:m
        weights[i]=M[i,i]#sum(M[i,:])
    end
    return 1. /weights
end

## Givens rotation
function givens_rot!(M,i,j)
    a=M[i]
    b=M[j]
    if b!=0
        #r=hypot(a,b)
        #hack of chypot
        r = a/b
        r = b*sqrt(1.0+0.0im+r*r)
        c=a/r
        s=-b/r
    else
        c = 1.0+0.0im
        s = 0.0+0.0im
        r = a
    end
    ϕ=log(c+1.0im*s)/1.0im
    #apply givens rotation
    M[i]=r
    M[j]=ϕ
    return nothing
end
function incremental_QR!(A,v)
    push!(A,v)
    n=length(A)
    m=length(A[1])
    #apply previous givens rotations
    for j=1:n-1
        for i=j+1:m
            ϕ=A[j][i]
            c=cos(ϕ)
            s=sin(ϕ)
            A[n][j],A[n][i]=c*A[n][j]-s*A[n][i], s*A[n][j]+c*A[n][i]
        end
    end

    #do new givens rotations
    for j=n+1:m
        givens_rot!(A[n],n,j)
    end
    return nothing
end
function get_Q(A)
    m=length(A[1])
    n=length(A)
    Q=zeros(ComplexF64,m,n)
    for i=1:n
        Q[i,i]=1
    end
    for i=1:m
        for j=i+1:n
            ϕ=A[i][j]
            c=cos(ϕ)
            s=sin(ϕ)
            Q[:,i],Q[:,j]=c*Q[:,i]-s*Q[:,j], s*Q[:,i]+c*Q[:,j]
        end
    end
    return Q
end

#TODO: hack the imporved julia version
function chypot(a,b)
    if a == 0
        h=0
    else
        r = b/a
        h = a*sqrt(1+r*r)
    end
    return h
end


@testset "Mincut" begin

    g = lg.CompleteDiGraph(5)
    cap1 = [
       0.0 2.0 2.0 0.0 0.0
       0.0 0.0 0.0 0.0 3.0
       0.0 1.0 0.0 3.0 0.0
       0.0 0.0 0.0 0.0 1.0
       0.0 0.0 0.0 0.0 0.0
    ];
    (part1, part2, value) = LightGraphsFlows.mincut(g,1,5,cap1,LightGraphsFlows.PushRelabelAlgorithm())
    @test value ≈ 4.0
    @test part1 == [1]
    @test sort(part2) == collect(2:5)
    cap2 = [
       0.0 3.0 2.0 0.0 0.0
       0.0 0.0 0.0 0.0 3.0
       0.0 1.0 0.0 3.0 0.0
       0.0 0.0 0.0 0.0 1.5
       0.0 0.0 0.0 0.0 0.0
    ];
    (part1, part2, value) = LightGraphsFlows.mincut(g,1,5,cap2,LightGraphsFlows.PushRelabelAlgorithm())
    @test value ≈ 4.5
    @test sort(part1) == collect(1:4)
    @test part2 == [5]

    g2 = lg.DiGraph(5)
    lg.add_edge!(g2,1,2)
    lg.add_edge!(g2,1,3)
    lg.add_edge!(g2,3,4)
    lg.add_edge!(g2,3,2)
    lg.add_edge!(g2,2,5)

    (part1, part2, value) = LightGraphsFlows.mincut(g2,1,5,cap1,LightGraphsFlows.PushRelabelAlgorithm())
    @test value ≈ 3.0
    @test sort(part1) == [1,3,4]
    @test sort(part2) == [2,5]

end


export discretizepolygon

"""
Discretize the polygon given by the (complex) entries of Z.

# Arguments
- `z`: Polygon vertices given as complex values. Pass an empty array for the unit disk.
  Pass a single value for a disk centered at that value. Pass two values for Chebyshev points.
- `include_interior_points`: Whether to return interior points.
- `npts`: Number of boundary points to return.
- `nptsint`: Minimum number of interior points (more than this may be returned).

# Return values
- `boundary_points`: Boundary points (`npts` of these), followed by the input
  polygon vertices, with an additional final vertex equal to the first vertex.
- `interior_points`: Interior points, if specified to be returned, otherwise an
  empty vector. If specified, there will be at least `nptsint` of these.
"""
function discretizepolygon(
    z::Vector{CT} = [],
    include_interior_points::Bool = false,
    npts::Int = 10000,
    nptsint::Int = 5) where CT<:Complex{<:Real}

    if isempty(z) # return unit disk
        z = [CT(0)]
    end

    if length(z) == 1 # return disk centered at z
        zz = z[1] .+ cis.(2 * pi * (1:npts)/npts)
    elseif length(z) == 2 # return Chebyshev points
        zz = (z[2]-z[1])/2 * (cos.(pi*(npts-1:-1:0)/(npts-1)).+1) .+ z[1]
    else
        z = [z[:]; z[1]] # close polygon
        L = sum(abs.(diff(z))) # length of polygon

        # 0<=alph<=1 is position of current point between z[ind] and z[ind+1]
        ind = 1
        alph = 0.0
        zz = [z[ind]]
        remL = L/npts
        while length(zz) < npts
            d = abs(z[ind+1] - z[ind])
            if (1-alph)*d < remL # can go to end of edge
                ind += 1
                remL -= (1-alph) * d
                alph = 0.0
            else # next point zz is between z[ind] and z[ind+1]
                alph += remL / d
                remL = L / npts
                push!(zz, z[ind] + alph * (z[ind+1] - z[ind]))
            end
        end
    end

    append!(zz, z)

    Z = Vector{CT}()
    if include_interior_points
        if length(z) == 2 # interval case
            xnr = 2*nptsint
            if (xnr&1) == 0 xnr += 1 end
            xpts = range(z[1], stop = z[2], length = xnr)
            Z = collect(xpts[2:2:end])
            return zz, Z
        end

        points = length(z) == 1 ? zz : z
        realz = real(points)
        imagz = imag(points)
        real_min, real_max = extrema(realz)
        imag_min, imag_max = extrema(imagz)

        iter = 0
        spacing = (real_max - real_min) / 2.0001 / sqrt(nptsint)
        while length(Z) < nptsint
            iter += 1
            if iter > 10
                error("Failed to find interior polygon points. Polygon too narrow? (Note that intervals should be given by their two endpoints only.)")
            end

            xnr = trunc(Int, (real_max - real_min) / (2*spacing))
            ynr = trunc(Int, (imag_max - imag_min) / (2*spacing))
            spacing /= sqrt(sqrt(2))
            if xnr <= 1 || ynr <= 1
                continue
            end

            xpts = range(real_min, stop = real_max, length = xnr)
            xpts = xpts[2:2:end]

            ypts = range(imag_min-eps(), stop = imag_max+eps(), length = ynr)
            ypts = ypts[2:2:end]

            Z = [x + y*im for x in xpts for y in ypts]::Vector{CT}
            Z = filter(p -> inpolygon(real(p), imag(p), realz, imagz), Z)
        end
    end

    return zz, Z
end


export common_prefix_distance, generic_levenshtein, hamming_distance, levenshtein_distance, lcs_distance

"""
sim_common_prefix computes the length of the common prefix among
two strings represented as arrays
"""
function sim_common_prefix(a::T, b::T)::Int where {T <: Any}
    len_a::Int = length(a)
    len_b::Int = length(b)
    i::Int = 1
    min_len::Int = min(len_a, len_b)
    @inbounds while i <= min_len && a[i] == b[i]
    	i += 1
    end

    return i - 1
end

mutable struct CommonPrefixDistance
	calls::Int
	CommonPrefixDistance() = new(0)
end

function common_prefix_distance(a::T, b::T) where T
	return sim_common_prefix(a, b) / max(length(a), length(b))
end

"""
generic_levenshtein computes the edit distance between two strings,
this is a low level function. Please use dist_lev
"""
function generic_levenshtein(a::T, b::T, icost::Int, dcost::Int, rcost::Int)::Int where T
    if length(a) < length(b)
        a, b = b, a
    end

    alen::Int = length(a)
    blen::Int = length(b)

    alen == 0 && return blen
    blen == 0 && return alen

    C::Vector{Int} = Array(0:blen)

    prevA::Int = 0
    @inbounds for i in 1:alen
        prevA = i
        prevC::Int = C[1]
        j::Int = 1

        while j <= blen
            cost::Int = rcost
            if a[i] == b[j]
               cost = 0
            end
            C[j] = prevA
            j += 1
            prevA = min(C[j]+dcost, prevA+icost, prevC+cost)
            prevC = C[j]
        end

        C[j] = prevA
    end

    return prevA
end

"""
levenstein_distance
computes edit distance over two strings or arrays of comparable objects
"""
function levenshtein_distance(a::T, b::T)::Float64 where T
    generic_levenshtein(a, b, 1, 1, 1)
end

"""
lcs_distance
computes lcs distance over associated to the longest common subsequence; it accepts two strings or arrays of comparable objects
"""
function lcs_distance(a::T, b::T)::Float64 where T
    generic_levenshtein(a, b, 1, 1, 2)
end

# function kerrormatch(a::T1, b::T2, errors::Int)::Bool where {T1 <: Any,T2 <: Any}
#     # if length(a) < length(b)
#     #     a, b = b, a
#     # end

#     alen::Int = length(a)
#     blen::Int = length(b)

#     alen == 0 && return alen == blen
#     blen == 0 && return true

#     C::Vector{Int} = Vector{Int}(0:blen)

#     @inbounds for i in 1:alen
#         prevA::Int = 0
#         prevC::Int = C[1]
#         j::Int = 1

#         while j <= blen
#             cost::Int = 1
#             if a[i] == b[j]
#                 cost = 0
#             end
#             C[j] = prevA
#             j += 1
#             prevA = min(C[j]+1, prevA+1, prevC+cost)
#             prevC = C[j]
# 	    end

#         C[j] = prevA
#         if prevA <= errors
#             return true
#         end
#     end

#     return false
# end

# function best_match_levenshtein(a::T1, b::T2)::Int where {T1 <: Any,T2 <: Any}
#     # if length(a) < length(b)
#     #     a, b = b, a
#     # end

#     alen::Int = length(a)
#     blen::Int = length(b)

#     alen == 0 && return blen
#     blen == 0 && return alen

#     C::Vector{Int} = 1:blen |> collect

#     mindist = alen
#     @inbounds for i in 1:alen
#         prevA::Int = 0
#         prevC::Int = C[1]
#         j::Int = 1

#         while j <= blen
#             cost::Int = 1
#             if a[i] == b[j]
#                 cost = 0
#             end
#             C[j] = prevA
#             j += 1
#             prevA = min(C[j]+1, prevA+1, prevC+cost)
#             prevC = C[j]
#         end

#         C[j] = prevA
#         if prevA < mindist
#             mindist = prevA
#         end
#     end

#     return mindist
# end

"""
hamming_distance computes the hamming distance between two slices of integers
"""

function hamming_distance(a::T, b::T)::Float64 where T
    d::Int = 0

    @inbounds for i = 1:length(a)
        #if a[i] != b[i]
        #    d += 1
        #end
        d += Int(a[i] != b[i])
    end

    return d
end


```Implement binary search.```

function binary_search(arr,l,r,x)
    if r ≥ l
        mid = l + (r - l) ÷ 2

            if arr[mid] == x
                return mid
            elseif arr[mid] > x
                return binary_search(arr, l, mid-1, x)
            else
                return binary_search(arr,  mid+1, r,  x)
            end
    else
        return -1
    end
end

arr = [1,2,3,6,8,10,12,15]
x=10

# Function call
result = binary_search(arr, 0, length(arr)-1, x)

arr[result]


using MacroTools: @q

macro nograd(ex)
  isexpr(ex, :tuple) || (ex = Expr(:tuple, ex))
  blk = @q begin end
  for f in ex.args
    back = MacroTools.@q _ -> ($__source__; nothing)
    push!(blk.args, :(@inline Zygote._pullback(::Context, ::Core.Typeof($(esc(f))), args...) = $(esc(f))(args...), $back))
  end
  return blk
end

macro which(ex)
  @capture(ex, f_(args__)) || error("Zygote.@which f(args...)")
  :(InteractiveUtils.@which adjoint(Context(), $(esc(f)), $(esc.(args)...)))
end

"""

    checkpointed(f, xs...)

Use gradient checkpointing on the call `f(xs...)`. This means that
`checkpointed(f, xs...) === f(xs...)`, but when computing the derivative
intermediate results from the forward pass of `f` will not be stored. Instead the forward
pass will be repeated, when computing the derivative.
This saves memory at the cost of increasing exectution time.

!!! warning
    If `f` is not a pure function, `checkpointed` will likely give wrong results.
"""
checkpointed(f, xs...) = f(xs...)

function Zygote._pullback(ctx::Zygote.AContext, ::typeof(checkpointed), f, xs...)
    y = f(xs...)
    function pullback_checkpointed(Δy)
        y, pb = Zygote._pullback(ctx, f, xs...)
        return (nothing, pb(Δy)...)
    end
    return y, pullback_checkpointed
end

"""
    hessian(f, x)

Construct the Hessian `∂²f/∂x²`, where `x` is a real number or an array,
and `f(x)` is a real number. When `x` is an array, the result is a matrix
`H[i,j] = ∂²f/∂x[i]∂x[j]`, using linear indexing `x[i]` even if the argument
is higher-dimensional.

This uses forward over reverse, ForwardDiff over Zygote, calling `hessian_dual(f, x)`.
See [`hessian_reverse`](@ref) for an all-Zygote alternative.

# Examples

```jldoctest; setup=:(using Zygote)
julia> hessian(x -> x[1]*x[2], randn(2))
2×2 Array{Float64,2}:
 0.0  1.0
 1.0  0.0

julia> hessian(x -> sum(x.^3), [1 2; 3 4])  # uses linear indexing of x
4×4 Array{$Int,2}:
 6   0   0   0
 0  18   0   0
 0   0  12   0
 0   0   0  24

julia> hessian(sin, pi/2)
-1.0
```
"""
hessian(f, x) = hessian_dual(f, x)

hessian_dual(f, x::AbstractArray) = forward_jacobian(x -> gradient(f, x)[1], x)[2]

hessian_dual(f, x::Number) = ForwardDiff.derivative(x -> gradient(f, x)[1], x)

"""
    hessian_reverse(f, x)

This should be equivalent to [`hessian(f, x)`](@ref hessian),
but implemented using reverse over reverse mode, all Zygote.
(This is usually much slower, and more likely to find errors.)
"""
hessian_reverse(f, x::AbstractArray) = jacobian(x -> gradient(f, x)[1], x)[1]

hessian_reverse(f, x::Number) = gradient(x -> gradient(f, x)[1], x)[1]

"""
    jacobian(f, args...) -> Tuple

For each array `a ∈ args` this returns a matrix with `Ja[k,i] = ∂y[k]/∂a[i]`
where `y = f(args...)` is usually a vector.
Arrays of higher dimension are treated like `vec(a)`, or `vec(y)` for output.

For scalar `x::Number ∈ args`, the result is a vector `Jx[k] = ∂y[k]/∂x`,
while for scalar `y` all results have just one row.

With any other argument type, no result is produced, even if [`gradient`](@ref) would work.

This reverse-mode Jacobian needs to evaluate the pullback once for each element of `y`.
Doing so is usually only efficient when `length(y)` is small compared to `length(a)`,
otherwise forward mode is likely to be better.

See also [`hessian`](@ref), [`hessian_reverse`](@ref).

# Examples

```jldoctest; setup=:(using Zygote)
julia> jacobian(a -> 100*a[1:3].^2, 1:7)[1]  # first index (rows) is output
3×7 Array{$Int,2}:
 200    0    0  0  0  0  0
   0  400    0  0  0  0  0
   0    0  600  0  0  0  0

julia> jacobian((a,x) -> a.^2 .* x, [1,2,3], 1)  # scalar argument has vector jacobian
([2 0 0; 0 4 0; 0 0 6], [1, 4, 9])

julia> jacobian((a,d) -> prod(a, dims=d), [1 2; 3 4; 5 6], 2)
([2 0 … 0 0; 0 4 … 3 0; 0 0 … 0 5], [0, 0, 0])
```

!!! warning
    For arguments of any type except `Number` & `AbstractArray`, the result is `nothing`.

```jldoctest; setup=:(using Zygote)
julia> jacobian((a,s) -> a.^length(s), [1,2,3], "str")
([3 0 0; 0 12 0; 0 0 27], nothing)

julia> jacobian((a,t) -> sum(a .* t[1]) + t[2], [1,2,3], (4,5))
([4 4 4], nothing)

julia> gradient((a,t) -> sum(a .* t[1]) + t[2], [1,2,3], (4,5))  # gradient undersands the tuple
([4, 4, 4], (6, 1))
```
"""
function jacobian(f, args...)
  y, back = pullback(_jvec∘f, args...)
  out = map(args) do x
    T = promote_type(eltype(x), eltype(y))
    dx = x isa AbstractArray ? similar(x, T, length(y), length(x)) :
      x isa Number ? similar(y, T, length(y)) :
      nothing
  end
  delta = _eyelike(y)
  for k in LinearIndices(y)
    grads = back(delta[:,k])
    for (dx, grad) in zip(out, grads)
      dx isa AbstractArray || continue
      _gradcopy!(view(dx,k,:), grad)
    end
  end
  out
end

_jvec(x::AbstractArray) = vec(x)
_jvec(x::Number) = _jvec(vcat(x))
_jvec(x) = throw(ArgumentError("jacobian expected a function which returns an array, or a scalar, got $(typeof(x))"))
_jvec(x::AbstractArray{<:Complex}) = throw(ArgumentError("jacobian does not accept complex output"))

_eyelike(y::Vector) = Matrix{eltype(y)}(I, length(y), length(y))
function _eyelike(y::AbstractVector) # version which works on GPU
  out = fill!(similar(y, length(y), length(y)), 0)
  out[LinearAlgebra.diagind(out)] .= 1
  out
end

_gradcopy!(dst::AbstractArray, src::AbstractArray{<:Number}) = copyto!(dst, src)
_gradcopy!(dst::AbstractArray, src::Number) = copyto!(dst, src)
_gradcopy!(dst::AbstractArray, src::Nothing) = dst .= 0
_gradcopy!(dst::AbstractArray, src::AbstractArray) = copyto!(dst, g isa Number ? g : 0 for g in src) # e.g. Union{Nothing,Float64}

"""
    jacobian(loss, ::Params)

Like [`gradient`](@ref) with implicit parameters, this method takes a zero-argument function
and returns an `IdDict`-like object, now containing the Jacobian for each parameter.

# Examples
```jldoctest; setup=:(using Zygote)
julia> xs = [1 2; 3 4]; ys = [5,7,9];

julia> Jxy = jacobian(() -> ys[1:2] .+ sum(xs.^2), Params([xs, ys]))
Grads(...)

julia> Jxy[ys]
2×3 Array{$Int,2}:
 1  0  0
 0  1  0

julia> Jxy[xs]
2×4 Array{$Int,2}:
 2  6  4  8
 2  6  4  8
```
"""
function jacobian(f, pars::Params)
  y, back = pullback(_jvec∘f, pars)
  out = IdDict()
  for p in pars
    T = Base.promote_type(eltype(p), eltype(y))
    J = similar(y, T, length(y), length(p))
    out[p] = J
  end
  delta = _eyelike(y)
  for k in LinearIndices(y)
    grads = back(delta[:,k])
    for p in pars
      out[p] isa AbstractArray || continue
      _gradcopy!(view(out[p],k,:), grads[p])
    end
  end
  Grads(out, pars)
end


# # [Basics of wiring diagrams](@id wiring_diagram_basics)
#
#md # [![](https://img.shields.io/badge/show-nbviewer-579ACA.svg)](@__NBVIEWER_ROOT_URL__/generated/wiring_diagrams/wiring_diagram_basics.ipynb)
#
# Using Catlab, you can create, manipulate, serialize, and visualize *wiring
# diagrams*, also known as [string
# diagrams](https://ncatlab.org/nlab/show/string+diagram). The flexible data
# structure for wiring diagrams allows arbitrary data to be attached to boxes,
# ports, and wires, and supports recursively nested diagrams.
#
# You can interact with wiring diagrams using two different progamming
# interfaces:
#
# 1. **Categorical**: A high-level, functional interface expressed in terms of
#    categorical concepts, such as composition (`compose`), monoidal products
#    (`otimes`), duplication (`mcopy`), and deletion (`delete`).
#
# 2. **Imperative**: A lower-level, mutating interface to directly manipulate
#    boxes, ports, and wires, via operations like adding boxes (`add_box`) and
#    wires (`add_wire`).
#
# In this notebook, we introduce both interfaces. We do not explicitly cover the
# visualization API, although for illustrative purposes we will draw wiring
# diagrams using Graphviz. Thus, you should install
# [Graphviz](https://www.graphviz.org/) if you wish to run this notebook.

using Catlab.WiringDiagrams

using Catlab.Graphics
import Catlab.Graphics: Graphviz

show_diagram(d::WiringDiagram) = to_graphviz(d,
  orientation=LeftToRight,
  labels=true, label_attr=:xlabel,
  node_attrs=Graphviz.Attributes(
    :fontname => "Courier",
  ),
  edge_attrs=Graphviz.Attributes(
    :fontname => "Courier",
  )
)

# ## Data structures
#
# The basic building blocks of a wiring diagram are boxes, ports, and wires. The
# top-level data structure is `WiringDiagram`, defined in the module
# `Catlab.WiringDiagrams`. A wiring diagram consists of boxes (usually of type
# `Box`) connected by wires (of type `Wire`). Each box has a sequence of input
# ports and a sequence of output ports, as does the wiring diagram itself. The
# wires have sources and targets, both of which consist of a box and a port on
# that box.
#
# The boxes in a wiring diagram are indexed by integer IDs. Boxes can be
# retrieved by ID, and wires refer to boxes using their IDs. Two special IDs,
# obtained by `input_id` and `output_id` methods, refer to the inputs and
# outputs of the diagram itself. In this way, wires can connect the (inner)
# boxes of a diagram to the diagram's "outer box".
#
# The `WiringDiagram` data structure is an elaborate wrapper around a directed
# graph from [LightGraphs.jl](https://github.com/JuliaGraphs/LightGraphs.jl).
# The underlying `DiGraph` object can be accessed using the `graph` method. The
# vertices of this graph are exactly the box IDs. The graph should never be
# mutated directly, on pain of creating inconsistent state, but it does allow
# convenient access to the large array of [graph
# algorithms](https://juliagraphs.github.io/LightGraphs.jl/latest/) supported by
# LightGraphs.
#
# All this is somewhat abstract but should become clearer as we see concrete
# examples.

# ## Categorical interface
#
# In this example, the wiring diagrams will carry symbolic expressions (of type
# `Catlab.ObExpr` and `Catlab.HomExpr`).

using Catlab.Theories

A, B, C, D = Ob(FreeBiproductCategory, :A, :B, :C, :D)
f = Hom(:f, A, B)
g = Hom(:g, B, C)
h = Hom(:h, C, D)

f

# ### Generators
#
# Convert each of the morphism generators into a diagram with a single box.

f, g, h = to_wiring_diagram(f), to_wiring_diagram(g), to_wiring_diagram(h)
f
#-
show_diagram(f)

# ### Composition

compose(f,g)
#-
show_diagram(compose(f,g))

# ### Monoidal products

otimes(f,h)
#-
show_diagram(otimes(f,h))

# ### Copy and merge, delete and create

mcopy(codom(f),2)
#-
show_diagram(mcopy(codom(f),2))
#-
show_diagram(compose(f, mcopy(codom(f),2)))
#-
show_diagram(compose(mcopy(dom(f),2), otimes(f,f)))

# ## Imperative interface
#
# We now show how to manipulate wiring diagrams using the low-level, imperative
# interface. The diagrams will carry Julia symbols.

f = Box(:f, [:A], [:B])
g = Box(:g, [:B], [:C])
h = Box(:h, [:C], [:D])

f

# ### Composition
#
# For example, here is how to manually construct a composition of two boxes.
#
# The `add_box!` method adds a box to a wiring diagrams and returns the ID
# assigned to the box. How the boxes are indexed is an implementation detail
# that you should not rely on; use the IDs that the system gives you.

d = WiringDiagram([:A], [:C])

fv = add_box!(d, f)
gv = add_box!(d, g)

add_wires!(d, [
  (input_id(d),1) => (fv,1),
  (fv,1) => (gv,1),
  (gv,1) => (output_id(d),1),
])

nboxes(d)
#-
nwires(d)
#-
d
#-
show_diagram(d)

# ### Products
#
# Here is how to manually construct a product of two boxes.

d = WiringDiagram([:A,:C], [:B,:D])

fv = add_box!(d, f)
hv = add_box!(d, h)

add_wires!(d, [
  (input_id(d),1) => (fv,1),
  (input_id(d),2) => (hv,1),
  (fv,1) => (output_id(d),1),
  (hv,1) => (output_id(d),2),
])

d
#-
show_diagram(d)


using MemorylessNonlinearities
using Test

x = [-3,-2,-1,0,1,2,3]

@testset "MemorylessNonlinearities.jl" begin

    @info "Test MemorylessNonlinearities"

    @test filt(Arctangent(100000), x) ≈ [-1,-1,-1,0,1,1,1] atol=0.0001
    @test filt(Arctangent(10), x) ≈ [-0.9788,-0.9682,-0.9365,0,0.9365,0.9682,0.9788] atol=0.0001

    @test filt(Blanking(1.0), x) == [0,0,-1,0,1,0,0]

    @test filt(CauchyNL(1.0), x) == [-0.6,-0.8,-1.0,0.0,1.0,0.8,0.6]

    @test filt(Clipping(1.0), x) == [-1,-1,-1,0,1,1,1]

    @test filt(HampelThreePart(1.0,2.0,3.0), x) == [0.0,-1.0,-1.0,0.0,1.0,1.0,0.0]

    @test filt(SαSNL(2.0, 1.0, 0.0, false), x) ≈ x ./ 2 atol=0.0001
    @test filt(SαSNL(1.0, 1.0, 0.0, false), x) ≈ [-0.6,-0.8,-1.0,0.0,1.0,0.8,0.6] atol=0.0001
    @test filt(SαSNL(2.0, 1.0, 0.0, true), x) ≈ x ./ 2 atol=0.0001
    @test filt(SαSNL(1.0, 1.0, 0.0, true), x) ≈ [-0.6,-0.8,-1.0,0.0,1.0,0.8,0.6] atol=0.0001
    @test filt(SαSNL(2.0, 1.0, 0.0), x) ≈ x ./ 2 atol=0.0001
    @test filt(SαSNL(1.0, 1.0, 0.0), x) ≈ [-0.6,-0.8,-1.0,0.0,1.0,0.8,0.6] atol=0.0001
    αs = [1.000001, 1.001000, 1.002000, 1.123457, 1.234567, 1.345678, 1.456789,
          1.567899, 1.678999, 1.789999, 1.899999, 1.998000, 1.999000, 1.999999]
    xtmp = [-999.987654, -500.505050, -100.101010, -30.303030, -10.101010, -5.505050, -3.303030, -1.101010, -0.010101,
            0.0, 0.010101, 1.101010, 3.303030, 5.505050, 10.101010, 30.303030, 100.101010, 500.505050, 999.987654]
    for α in αs
        @test filt(SαSNL(α, 1.0, 0.0, true), xtmp) ≈ filt(SαSNL(α, 1.0, 0.0, false), xtmp) rtol=0.01
        @test filt(SαSNL(α, 2.0, 0.0), xtmp) ≈ filt(SαSNL(α, 2.0, 0.0, false), xtmp) rtol=0.01
    end

    @test filt(SoftClipping(2.0), x) ≈ [-2.0,-2.0,-1.375,0.0,1.375,2.0,2.0]

    @test filt(TurkeyBiweight(3.0), x) ≈ [-0.0,-0.6173,-0.7901,0.0,0.7901,0.6173,0.0] atol=0.0001

    @test filt(InverseHyperbolicSine(1.0), x) ≈ [-1.8184,-1.4436,-0.8814,0.0,0.8814,1.4436,1.8184] atol=0.0001
end

@testset "utils.jl" begin

    @info "Test utils"

    x = randn(1000)
    xrescale = minmaxrescale(x, -0.1, 0.3)
    @test minimum(xrescale) ≈ -0.1 atol=1e-6
    @test maximum(xrescale) ≈ 0.3 atol=1e-6

end


##
using Plots
plotlyjs()

theme(:juno)

##
using IJulia
display(1)

print("hello doug")

##
plot([1,2,1])

function f(x)
    return x
end

f(3)

3+3

using IJulia
notebook()

## Simple testing suite

## Preloads
using Statistics
using FFTW
using Plots
using BenchmarkTools
using Profile
using LinearAlgebra

## BLAS.dot

test_img = rand(256,256)

function variance_BLAS(A)
    (Nx, Ny)  = size(A)
    norm_im_vec = reshape(A, Nx*Ny)
    BLAS.dot(norm_im_vec,norm_im_vec)/(Nx*Ny)
end

function variance(A)
    (Nx, Ny)  = size(A)
    sum(A .* A)/(Nx*Ny)
end

function variance_BLAS_sq(A)
    (Nx, Ny)  = size(A)
    BLAS.dot(A,A)/(Nx*Ny)
end

@time variance(test_img)

@benchmark variance(test_img)

@time variance_BLAS(test_img)

@benchmark variance_BLAS(test_img)

#BLAS dot 30x faster; Was the unflattened version really wrong

@time variance_BLAS_sq(test_img)

@benchmark variance_BLAS_sq(test_img)

function variance_lc(A)
    (Nx, Ny)  = size(A)
    sum([x^2 for x in A])/(Nx*Ny)
end

@time variance_lc(test_img)

@benchmark variance_lc(test_img)

function variance_lc_abs2(A)
    (Nx, Ny)  = size(A)
    sum([abs2(x) for x in A])/(Nx*Ny)
end

@time variance_lc_abs2(test_img)

@benchmark variance_lc_abs2(test_img)

## Test the * versus mul!

function Doug_mul(A)
    B_test = zeros(256,256)
    B_test     .= A' * A
end

@time Doug_mul(test_img)

@benchmark Doug_mul(test_img)

function noraml_mul(A)
    B_test = zeros(256,256)
    mul!(B_test,A',A)
end

@time noraml_mul(test_img)

@benchmark noraml_mul(test_img)

BLAS.syrk!(test_img,B_test)


using Manifolds, Manopt, Test, ManifoldsBase

@testset "gradients" begin
    M = Circle()
    N = PowerManifold(M, 4)
    x = [0.1, 0.2, 0.3, 0.5]
    tvTestξ = [-1.0, 0.0, 0.0, 1.0]
    @test ∇TV(N, x) == tvTestξ
    @test ∇TV(M, (x[1], x[1])) ==
          (zero_tangent_vector(M, x[1]), zero_tangent_vector(M, x[1]))
    @test norm(N, x, ∇TV(N, x, 2) - tvTestξ) ≈ 0
    tv2Testξ = [0.0, 0.5, -1.0, 0.5]
    @test ∇TV2(N, x) == tv2Testξ
    @test norm(N, x, forward_logs(N, x) - [0.1, 0.1, 0.2, 0.0]) ≈ 0 atol = 10^(-16)
    @test norm(
        N,
        x,
        ∇intrinsic_infimal_convolution_TV12(N, x, x, x, 1.0, 1.0)[1] -
        [-1.0, 0.0, 0.0, 1.0],
    ) ≈ 0
    @test norm(N, x, ∇intrinsic_infimal_convolution_TV12(N, x, x, x, 1.0, 1.0)[2]) ≈ 0
    x2 = [0.1, 0.2, 0.3]
    N2 = PowerManifold(M, size(x2)...)
    @test ∇TV2(N2, x2) == zeros(3)
    @test ∇TV2(N2, x2, 2) == zeros(3)
    @test ∇TV(M, (0.0, 0.0), 2) == (0.0, 0.0)
    # 2d forward logs
    N3 = PowerManifold(M, 2, 2)
    N3C = PowerManifold(M, 2, 2, 2)
    x3 = [0.1 0.2; 0.3 0.5]
    x3C = cat(x3, x3; dims=3)
    tC = cat([0.2 0.3; 0.0 0.0], [0.1 0.0; 0.2 0.0]; dims=3)
    @test norm(N3C, x3C, forward_logs(N3, x3) - tC) ≈ 0 atol = 10^(-16)

    M = Circle()
    p = 0
    q = π / 4
    @test ∇distance(M, p, q) == q - p
end


# Test get/set objective coefficients in LP

using Gurobi
using Base.Test

env = Gurobi.Env()

# original model
#
#   maximize  2x + 2y
#
#	s.t. 0.2 <= x, y <= 1
#

model = gurobi_model(env;
	name="lp_03",
	sense=:maximize,
	f=[2.0, 2.0],
	lb=[0.2, 0.2],
	ub=[1.0, 1.0])

println(model)

lb_ = lowerbounds(model)
ub_ = upperbounds(model)
c_ = objcoeffs(model)

@test lb_ == [0.2, 0.2]
@test ub_ == [1.0, 1.0]
@test c_ == [2.0, 2.0]

optimize(model)

println()
println("soln = $(get_solution(model))")
println("objv = $(get_objval(model))")

# change objective (warm start)
#
#	maximize x - y
#
#	s.t. 0.2 <= x, y <= 1
#

set_objcoeffs!(model, [1, -1])
update_model!(model)

c_ = objcoeffs(model)
@test c_ == [1.0, -1.0]

optimize(model)

println()
println("soln = $(get_solution(model))")
println("objv = $(get_objval(model))")

gc()


using PyPlot
using Random

ntrials = 10000            # Number of experiments
x = Int64[]                # Array of outcomes
sumx = 0
for i = 1:ntrials
    nrolls = 0
    found_a_six = false
    while !found_a_six
        nrolls += 1
        roll = rand(1:6)
        if roll == 6
            found_a_six = true
        end
    end
    push!(x, nrolls)
    sumx += nrolls
end

plt.hist(x, 50);
xlabel("Number of rolls")
ylabel("Count")
average = sumx / ntrials

function card_rank(card)
    (card - 1) % 13 + 1
end

function card_suit(card)
    (card - 1) ÷ 13 + 1
end

cards = 1:52      # All cards
[card_rank.(cards) card_suit.(cards)]

cards = randperm(52)
hand = cards[1:5]
[card_rank.(hand) card_suit.(hand)]

ntrials = 100000
nflush = 0
for itrial = 1:ntrials
    cards = randperm(52)
    hand = cards[1:5]

    suits = card_suit.(hand)
    same_suit = true
    for i = 2:5
        if suits[i] ≠ suits[1]
            same_suit = false
            break
        end
    end
    if same_suit
        nflush += 1
    end
end

approx_probability = nflush / ntrials

exact_probability = 4 * binomial(13,5) / binomial(52,5)

n = 10000               # Number of darts
x = 2rand(n) .- 1       # Coordinates
y = 2rand(n) .- 1

plot([-1,1,1,-1,-1], [-1,-1,1,1,-1])  # Draw square
theta = 2π*(0:100)./100               # Draw circle
plot(cos.(theta), sin.(theta))
plot(x, y, linestyle="None", marker=".", markersize=1)   # Plot dart points
axis("equal")

# Determine if points are inside the circle (a "hit")
hits = 0
for i = 1:n
    if x[i]^2 + y[i]^2 ≤ 1
        hits += 1
    end
end

approx_pi = 4hits / n

function random_walk(n)
    x = [0]
    y = [0]
    while abs(x[end]) < n && abs(y[end]) < n
        if rand() < 0.5
            if rand() < 0.5  # Up
                push!(x, x[end])
                push!(y, y[end] + 1)
            else             # Down
                push!(x, x[end])
                push!(y, y[end] - 1)
            end
        else
            if rand() < 0.5  # Right
                push!(x, x[end] + 1)
                push!(y, y[end])
            else             # Left
                push!(x, x[end] - 1)
                push!(y, y[end])
            end
        end
    end
    x,y
end

n = 10
x,y = random_walk(n)

plot(x, y, marker=".", markersize=8)     # Draw dots at each point in x,y
grid(true)
axis([-n,n,-n,n]);


using DifferentialEquations
using GLMakie

#=
 # 1. simplest case

  ̇x = - λ ̇x

=#

testeq(u, p, t) = @. - p.k * ( p.a - u ) * u

u0  = [0.1]
tspan = (0.0, 60.0)
p = (; k = 1.0, a = 1.6)

prob = ODEProblem(testeq, u0, tspan, p)
sol = solve(prob) #, reltol = 1e-6, abstol = 1e-8)

begin
    f = Figure()

    ax = Axis(f[1,1], title = "Numerical solution and interpolation", xlabel=L"x(t)", ylabel=L"t")
    scatter!(sol.t, sol[1,:])
    lines!(sol.t, sol[1,:])

    sol_int = sol(LinRange(tspan..., 100))
    lines!(sol_int.t, sol_int[1,:])

    mkpath("plots")
    save("plots/00_first_order.png", f)
    f
end


function _hist_add!{T}(counts::Dict{T,Int}, y::Vector{T}, region::UnitRange{Int})
    for i in region
        lbl = y[i]
        counts[lbl] = get(counts, lbl, 0) + 1
    end
    return counts
end

"""
Given a vector of labels, this returns a Dict where the keys are
the unique y[i] and the values are the number of times that y[i]
occurs in the vector y.
"""
_hist{T}(y::Vector{T}, region::UnitRange{Int} = 1:endof(y)) = _hist_add!(Dict{T,Int}(), y, region)

"""
Given the vector y, this simply returns the value that
appears most frequently in y.
"""
function majority_vote(y::Vector)
    if length(y) == 0
        return nothing
    end
    counts = _hist(y)
    top_vote = y[1]
    top_count = -1
    for (k,v) in counts
        if v > top_count
            top_vote = k
            top_count = v
        end
    end
    return top_vote
end

# n = 1000
# y = rand([true, false], n);
# @time majority_vote(y)

"""
This was formerly called neg_z1_loss() in Ben's original DecisionTree code,
and the variable `y` was formerlly called `labels`
"""
function _classifcation_error_loss(y::Vector, weights::Vector)
    missmatches = y .!= majority_vote(y)
    loss = sum(weights[missmatches])
    return -loss
end

# This method is dispatched when weights are omitted. This
# allows us to compute the loss function 5x faster
function _classifcation_error_loss(y::Vector)
    loss = sum(y)
    return -loss        # tons of mem-alloc here
end

#
# n = 1000
# y = rand([true, false], n);
# wgt = repeat([1], inner = n);
#
# @time _classifcation_error_loss(y)

function mean_squared_error(actual, predicted)
    return mean((actual - predicted).^2)
end

function rsq(actual, predicted)
    ss_residual = sum((actual - predicted).^2)
    ss_total = sum((actual .- mean(actual)).^2)
    return 1.0 - ss_residual/ss_total
end


#!/usr/bin/env julia

#=
-----------------
Utility Functions
-----------------
=#
"""
    trapz(y, x)

Array based trapezoidal integration of y with respect to x.

Limits of integration defined by the start and end points of the arrays.
"""
function trapz(y::Vector{RheoFloat}, x::Vector{RheoFloat})

    n = length(x)

    @assert n==length(y) "X and Y array length must match."
    n==1 && return zero(RheoFloat)

    r = zero(RheoFloat)
    @inbounds for i in 2:n
        r += (y[i-1] + y[i])*(x[i] - x[i-1])
    end

    r/2

end

"""
    derivCD(y, x)

Given two arrays of data, x and y, calculate dy/dx using central difference
method and forward and backward difference for array boundaries.
"""
function derivCD(y::Vector{RheoFloat}, x::Vector{RheoFloat})

    # get length
    N = length(x)

    # assert y and x arrays are same length
    @assert length(y)==N "X and Y Array lengths must match."

    # initialise zero array of length y
    ydot = similar(y)

    # assume 'imaginary' previous point is 0.0, and Δx is the same as the next one ahead
    # this is a physical assumption that material is at rest before first data point.
    # Could be problematic in some cases if sudden jump as we are actually missing
    # important information about how quickly that jump happened.
    @inbounds ydot[1] = y[1]/(x[2] - x[1])

    # central difference with uneven spacing for general case of constant or variable sample rate
    @inbounds for i in 2:(N-1)
        Δx₁ = x[i] - x[i-1]
        Δx₂ = x[i+1] - x[i]
        ydot[i] = (y[i+1]*Δx₁^2 + (Δx₂^2 - Δx₁^2)*y[i] - y[i-1]*Δx₂^2)/(Δx₁*Δx₂*(Δx₁ + Δx₂))
    end

    # 1st order backwards difference for last element
    ydot[N] = (y[N] - y[N-1])/(x[N] - x[N-1])

    return ydot

end

"""
    derivBD(y, x)

Given two arrays of data, x and y, calculate dy/dx using 1st order
backward difference. Assumes y==0 at a previous point, i.e.
y is 'at rest'. Captures instantaneous loading where derivCD will smooth.
"""
function derivBD(y::Vector{RheoFloat}, x::Vector{RheoFloat})

    # get length
    N = length(x)

    # assert y and x arrays are same length
    @assert length(y)==N "X and Y Array lengths must match."

    # initialise zero array of length y
    ydot = similar(y)

    # assume 'imaginary' previous point is 0.0, and Δx is the same as the next one ahead
    # this is a physical assumption that material is at rest before first data point.
    # Could be problematic in some cases if sudden jump as we are actually missing
    # important information about how quickly that jump happened.
    @inbounds ydot[1] = y[1]/(x[2] - x[1])

    # backwards difference method for rest of points
    @inbounds for i in 2:N
        ydot[i] = (y[i] - y[i-1])/(x[i] - x[i-1])
    end

    return ydot

end

"""
    doublederivCD(y, x)

Given two arrays of data, x and y, calculate d^2(y)/dx^2 using 2nd order difference. Data
should conform to the following constraints:
- No step loading (signal should start at 0)
"""
function doublederivCD(y::Vector{RheoFloat}, x::Vector{RheoFloat})
    # get length
    N = length(x)

    # assert y and x arrays are same length
    @assert length(y)==N "X and Y Array lengths must match."

    # initialise zero array of length y
    yddot = similar(y)

    # no physical assumptions made, unlike for first order BD derivative.
    # forward difference for first element, backwards difference for last element
    @inbounds yddot[1] = (y[3] - 2*y[2] + y[1])/(x[2] - x[1])^2
    @inbounds yddot[N] = (y[N] - 2*y[N-1] + y[N-2])/(x[N] - x[N-1])^2

    # central difference method for rest of points
    @inbounds for i in 2:(N-1)
        yddot[i] = (y[i+1] - 2*y[i] + y[i-1])/(x[i] - x[i-1])^2
    end

    return yddot
end

"""
    derivFbasic(α, ydot, yddot, t, dt)

Compute the Caputo fractional derivative using a simple convolution on the integrated-by-parts
form which does not have a singularity in the kernel. Sample rate must be constant, and this
attribute is not checked in the function body for computational efficiency.
"""
function derivFbasic(α, ydot, yddot, t, dt)
    I1 = ydot[1]*t.^(1-α)
    I2 = conv(t.^(1-α), yddot)[1:length(t)]*dt
    return (I1 + I2)/gamma(2 - α)
end

function constantcheck(t::Vector{RheoFloat})
    # get array of backward differences
    diff = t[2:end] - t[1:end-1]
    # check if any element is not approximately equal to 1st element
    check = all(x -> x ≈ diff[1], diff)
end

function getsampleperiod(t::Vector{RheoFloat})
    # check sample rate is constant, otherwise sample period varies
    @assert constantcheck(t) "Sample-rate must be constant"
    # return sample period
    sampleperiod = t[2] - t[1]
end

"""
    closestindex(x, val)

Find the index of the array element closest to val.
"""
function closestindex(x::Vector{T}, val::Real) where {T<:Real}

    # intialise closest match variable, assuming best match is index 1
    ibest = 1

    # diff between value and current element
    dxbest = abs(x[ibest]-val)

    # loop through all elements, looking for smallest difference
    @inbounds for I in eachindex(x)
        dx = abs(x[I]-val)
        if dx < dxbest
            dxbest = dx
            ibest = I
        end
    end

    ibest
end

"""
    closestindices(x, vals)

Uses `closestindex` iteratively to find closest index for each value in `vals` array,
returns array of indices.
"""
closestindices(x::Vector{T1}, vals::Vector{T2}) where {T1<:Real, T2<:Real} = broadcast(closestindex, (x,), vals)

#=
--------------------------------
Preprocessing base functionality
--------------------------------
=#
"""
    getsigma(τ, samplerate)

Generate sigma/std deviation for gaussian smoothing kernel.

Acts as a low pass filter. Information of time scale τ will be half power,
faster will be increasingly cut. Called by smoothgauss function.
"""
function getsigma(τ::Real, samplerate::Real)

    # get freq, reduce to half power and generate gaussian std. deviation
    smoothfreq = 1.0/τ

    sF_halfpower = smoothfreq/sqrt(2.0*log(2.0))

    σ = samplerate/(2.0*π*sF_halfpower)

    RheoFloat(σ)
end

# """
#     fixed_resample(x::Vector{T}, y::Vector{T}, boundaries::Vector{U}, elperiods::Union{Vector{U}, U}) where T<:RheoFloat where U<:Integer
#
# Resample two arrays with new sample rate(s).
# It can either reduce the sample rate by not taking every array element or it can increase the sample
# rate by inserting new elements. If the number of elements per period (elperiods) is a positive number, then it upsamples; if
# it is negative it downsamples.
#
# The function returns the resampled x and y arrays.
# """
# function fixed_resample(x::Vector{T}, y::Vector{T}, boundaries::Vector{U}, elperiods::Union{Vector{U}, U}; includelastel::Bool=true) where T<:RheoFloat where U<:Integer
#
#     # assert correct function signature
#     @assert length(x)==length(y) "X and Y arrays must have same length."
#     @assert length(elperiods)==length(boundaries)-1 "Number of different sample periods must be 1 less than boundaries provided"
#
#     # initialise resampled arrays as empty
#     xᵦ = Vector{RheoFloat}(undef, 0)
#     yᵦ = Vector{RheoFloat}(undef, 0)
#
#     # loop through boundaries
#     for i in 1:length(boundaries)-1
#
#         # upsampling, starts at each element then intepolates up N times
#         if !signbit(elperiods[i])
#             for k in boundaries[i]:(boundaries[i+1]-1)
#                     x_add  = range(x[k], x[k+1]; length=elperiods[i]+1)[1:end-1]
#                     y_add =  y[k] .+ (x_add[1:end] .- x[k]).*(y[k+1] .- y[k])./(x[k+1] .- x[k])
#                     xᵦ = append!(xᵦ, x_add)
#                     yᵦ = append!(yᵦ, y_add)
#              end
#
#         # downsampling, simply takes every N element as in downsample function
#         elseif signbit(elperiods[i])
#             append!(xᵦ,x[boundaries[i]:abs(elperiods[i]):(boundaries[i+1]-1)])
#             append!(yᵦ,y[boundaries[i]:abs(elperiods[i]):(boundaries[i+1]-1)])
#         end
#     end
#
#     # last element may be missed, so check and add if needed
#     lastel = boundaries[end]
#     if includelastel && xᵦ[end]<x[lastel]
#         append!(xᵦ,x[lastel])
#         append!(yᵦ,y[lastel])
#     end
#
#     return xᵦ, yᵦ
# end

#=
-----------------------------------------
Fitting and predicting base functionality
-----------------------------------------
=#
function singularitytest(modulus; t1::RheoFloat=zero(RheoFloat))

    startval = modulus(t1)

    if isnan(startval) || startval == Inf
        return true
    else
        return false
    end

end

"""
    boltzintegral_nonsing(modulus, time_series, prescribed_dot)

Calculate Boltzmann Superposition integral using direct integration method.

This is much slower and slightly less accurate (depending on sample resolution)
than the convolution method. However, it works for variable sample rate.

# Arguments

- `modulus`: Viscoelastic modulus function in which the parameters have been substituted
- `time_series`: The array of times
- `prescribed_dot`: Derivative of (usually prescribed) variable inside the integration kernel
"""
function boltzintegral_nonsing(modulus, time_series::Vector{Float64}, prescribed_dot::Vector{Float64})

    # need to add an additional 'previous' time point to capture any instantaneous loading
    time_previous = time_series[1] - (time_series[2] - time_series[1])
    time_mod = vcat([time_previous], time_series)
    # material is assumed at rest at this 'previous' time
    prescribed_dot_mod = vcat([0.0], prescribed_dot)

    I = zeros(length(time_mod))
    @inbounds for (i,v) in enumerate(time_mod)
        # generate integral for each time step
        τ = time_mod[1:i]
        Modulus_arg = v .- τ
        Modulusᵢ = modulus(Modulus_arg)
        df_dtᵢ = prescribed_dot_mod[1:i]
        intergrand = Modulusᵢ.*df_dtᵢ

        I[i] = trapz(intergrand, τ)
    end
    # fix initial point
    I[2] = (prescribed_dot[1]*modulus(time_series)*(time_series[2] - time_series[1]))[1]
    I[2:end]

end

"""
    obj_var_nonsing(params, grad, modulus, time_series, prescribed_dot, measured; _insight::Bool = false)

Generate the sum-of-squares of the difference between `measured` and the Boltzmann
convolution integral of the selected `modulus` and `prescribed_dot`. Used when
sample rate is variable and no singularity in model.

# Arguments

- `params`: Array of parameters sent to `modulus`
- `grad`: Gradient argument used by NLOpt
- `modulus`: Viscoelastic modulus function
- `time_series`: Array of time data
- `prescribed_dot`: Convolved with `modulus` over `time_series`, usually dϵ/dt for stress relaxation and dσ/dt for creep
- `measured`: Data for comparison against, usually σ for stress relaxation and ϵ for creep
- `_insight`: Declare whether insight info should be shown when this function is called, true or false
"""
function obj_var_nonsing(params, grad, modulus, time_series, prescribed_dot, measured; _insight::Bool = false)

    _insight && println("Current Parameters: ", params)

    mod = (t->modulus(t,params))
    convolved = boltzintegral_nonsing(mod, time_series, prescribed_dot)

    cost = sum((measured - convolved).^2)

end

"""
    boltzintegral_sing(modulus, time_series, prescribed_dot)

Calculate Boltzmann Superposition integral using direct integration method.

This is much slower and slightly less accurate (depending on sample resolution)
than the convolution method. However, it works for variable sample rate.

# Arguments

- `modulus`: Viscoelastic modulus function in which the parameters have been substituted
- `time_series`: The array of times
- `prescribed_dot`: Derivative of (usually prescribed) variable inside the integration kernel
"""
function boltzintegral_sing(modulus, time_series::Vector{Float64}, prescribed_dot::Vector{Float64})

    offset_t0 = (time_series[2] - time_series[1])/singularity_offset
    # need to add an additional 'previous' time point to capture any instantaneous loading
    time_previous = time_series[1] - (time_series[2] - time_series[1])
    time_mod = vcat([time_previous], time_series)
    # material is assumed at rest at this 'previous' time
    prescribed_dot_mod = vcat([0.0], prescribed_dot)

    offset = offset_t0
    I = zeros(length(time_mod))
    for (i,v) in enumerate(time_mod)
        if i>1
            offset = (time_mod[i] - time_mod[i-1])/singularity_offset
        end
        τ = time_mod[1:i]
        Modulus_arg = v .- τ
        Modulus_arg[end] = offset
        Modulusᵢ = modulus(Modulus_arg)
        df_dtᵢ = prescribed_dot_mod[1:i]

        intergrand = Modulusᵢ.*df_dtᵢ

        I[i] = trapz(intergrand, τ)
    end
    # fix initial point
    I[2] = (prescribed_dot[1]*modulus([offset_t0])*(time_series[2] - time_series[1]))[1]
    I[2:end]

end

"""
    obj_var_sing(params, grad, modulus, time_series, prescribed_dot, measured; _insight::Bool = false)

Generate the sum-of-squares of the difference between `measured` and the Boltzmann
convolution integral of the selected `modulus` and `prescribed_dot`. Used when
sample rate is variable and there IS singularity in model.

# Arguments

- `params`: Array of parameters sent to `modulus`
- `grad`: Gradient argument used by NLOpt
- `modulus`: Viscoelastic modulus function
- `time_series`: Array of time data
- `prescribed_dot`: Convolved with `modulus` over `time_series`, usually dϵ/dt for stress relaxation and dσ/dt for creep
- `measured`: Data for comparison against, usually σ for stress relaxation and ϵ for creep
- `_insight`: Declare whether insight info should be shown when this function is called, true or false
"""
function obj_var_sing(params, grad, modulus, time_series, prescribed_dot, measured; _insight::Bool = false)

    _insight && println("Current Parameters: ", params)

    mod = (t->modulus(t,params))
    convolved = boltzintegral_sing(mod, time_series, prescribed_dot)

    # skip first point for error computation, workaround for singularity approximation error
    cost = sum((measured[2:end] - convolved[2:end]).^2)

end

"""
    boltzconvolve(modulus, time_series, dt::RheoFloat,prescribed_dot)

Calculate Boltzmann Superposition integral using convolution method.

This is much faster and slightly more accurate (depending on sample resolution)
than the integral method. However, it only works for constant sample rate.

# Arguments

- `modulus`: Viscoelastic modulus function in which the parameters have been substituted
- `time_series`: The array of times
- `dt`: Constant time step (sample period)
- `prescribed_dot`: Derivative of (usually prescribed) variable inside the integration kernel
"""
function boltzconvolve(modulus, time_series, dt, prescribed_dot)

    Modulus = modulus(time_series)
    # fast convolution
    β = conv(Modulus, prescribed_dot)
    # pick out relevant elements (1st half) and multiply by dt
    β = β[1:length(time_series)]*dt

end

"""
    obj_const_nonsing(params, grad, modulus, time_series, dt, prescribed_dot, measured; _insight::Bool = false)

Generate the sum-of-squares of the difference between `measured` and the Boltzmann
convolution integral of the selected `modulus` and `prescribed_dot`. Used when
sample rate is constant and model does not feature singularity.

# Arguments

- `params`: Array of parameters sent to `modulus`
- `grad`: Gradient argument used by NLOpt
- `modulus`: Viscoelastic modulus function
- `time_series`: Array of time data
- `dt`: Constant time step (sample period)
- `prescribed_dot`: Convolved with `modulus` over `time_series`, usually dϵ/dt for stress relaxation and dσ/dt for creep
- `measured`: Data for comparison against, usually σ for stress relaxation and ϵ for creep
- `_insight`: Declare whether insight info should be shown when this function is called, true or false
"""
function obj_const_nonsing(params, grad, modulus, time_series, dt, prescribed_dot, measured; _insight::Bool = false)

    _insight && println("Current Parameters: ", params)

    # Replace parameters inside the viscoelastic modulus
    mod = (t->modulus(t,params))
    convolved = boltzconvolve(mod, time_series, dt, prescribed_dot)

    cost = sum((measured - convolved).^2)
    return cost

end

"""
    obj_const_sing(params, grad, modulus, time_series, dt, prescribed_dot, measured; _insight::Bool = false)

Generate the sum-of-squares of the difference between `measured` and the Boltzmann
convolution integral of the selected `modulus` and `prescribed_dot`. Used when
sample rate is constant and model does feature singularity.

# Arguments

- `params`: Array of parameters sent to `modulus`
- `grad`: Gradient argument used by NLOpt
- `modulus`: Viscoelastic modulus function
- `time_series`: Array of time data
- `dt`: Constant time step (sample period)
- `prescribed_dot`: Convolved with `modulus` over `time_series`, usually dϵ/dt for stress relaxation and dσ/dt for creep
- `measured`: Data for comparison against, usually σ for stress relaxation and ϵ for creep
- `_insight`: Declare whether insight info should be shown when this function is called, true or false
"""
function obj_const_sing(params, grad,modulus, time_series,dt, prescribed_dot, measured; _insight::Bool = false)

    _insight && println("Current Parameters: ", params)

    mod = (t->modulus(t,params))
    # convolved = boltzconvolve_sing(modulus, time_series, dt, params, prescribed_dot)
    convolved = boltzconvolve(mod, time_series, dt, prescribed_dot)

    # skip first point for error computation, workaround for singularity approximation error
    cost = sum((measured[2:end] - convolved[2:end]).^2)

end

"""
    obj_const_weighted(params, grad, modulus, time_series, dt, prescribed_dot, measured_weighted, weights; _insight::Bool = false)

Post-convolution weighted constant sample-rate cost function for emphasising regions without introducing variable sample-rates.
"""
function obj_const_weighted(params, grad, modulus, time_series, dt, prescribed_dot, measured_weighted, weights; _insight::Bool = false)

    _insight && println("Current Parameters: ", params)

    # Replace parameters inside the viscoelastic modulus
    mod = (t->modulus(t,params))
    convolved = boltzconvolve(mod, time_series, dt, prescribed_dot)

    cost = sum((measured_weighted - convolved[weights]).^2)
    return cost
end

"""
    leastsquares_init(params_init, low_bounds, hi_bounds, modulus, time_series, dt, prescribed_dot, measured; insight = false, sampling = "constant", singularity = false)

Initialise then begin a least squares fitting of the supplied data.

# Arguments

- `params_init`: Initial parameters to be used (starting guess)
- `low_bounds`: Lower bounds for parameters
- `hi_bounds`: Higher bounds for parameters
- `modulus`: Viscoelastic modulus function
- `time_series`: Array of time data
- `dt`: Constant time step (sample period)
- `prescribed_dot`: Convolved with `modulus` over `time_series`, usually dϵ/dt for stress relaxation and dσ/dt for creep
- `measured`: Data for comparison against, usually σ for stress relaxation and ϵ for creep
- `constant_sampling`: true if sample rate is constant, false otherwise
- `insight`: Declare whether insight info should be shown when this function is called, true or false
- `singularity`: Presence of singularity in model
- `indweight`: indices weighting, see `modelfit` for more discussion
"""
function leastsquares_init(params_init::Vector{RheoFloat}, low_bounds::RheovecOrNone,
                           hi_bounds::RheovecOrNone, modulus,
                           time_series::Vector{RheoFloat}, dt::RheoFloat,
                           prescribed_dot::Vector{RheoFloat}, measured::Vector{RheoFloat};
                           insight::Bool = false, constant_sampling::Bool=true,
                           singularity::Bool = false, _rel_tol = 1e-4, indweights=nothing)

    # initialise NLOpt.Opt object with :LN_SBPLX Subplex algorithm
    opt = Opt(:LN_SBPLX, length(params_init))

    # set lower bounds and upper bounds unless they take null value
    if !isnothing(low_bounds)
        low_bounds = convert(Vector{Float64},low_bounds)
        lower_bounds!(opt, low_bounds)
    end

    if !isnothing(hi_bounds)
        hi_bounds = convert(Vector{Float64}, hi_bounds)
        upper_bounds!(opt, hi_bounds)
    end

    # set relative tolerance
    xtol_rel!(opt, _rel_tol)

    # Convert to float64 to avoid conversion by NLOpt
    params_init = convert(Vector{Float64},params_init)
    time_series = convert(Vector{Float64},time_series)
    prescribed_dot = convert(Vector{Float64},prescribed_dot)
    measured = convert(Vector{Float64},measured)
    dt = convert(Float64, dt)

    # set Opt object as a minimisation objective. Use a closure for additional
    # arguments sent to object objectivefunc
    if constant_sampling && !singularity && isnothing(indweights)
        min_objective!(opt, (params, grad) -> obj_const_nonsing(params, grad, modulus,
                                                            time_series, dt,
                                                            prescribed_dot, measured;
                                                            _insight = insight))

    elseif constant_sampling && singularity && isnothing(indweights)
        # remove singularity, just go close to it, 1/10th over first sample period
        time_series[1] = 0.0 + (time_series[2] - time_series[1])/singularity_offset
        min_objective!(opt, (params, grad) -> obj_const_sing(params, grad, modulus,
                                                        time_series, dt,
                                                        prescribed_dot, measured;
                                                        _insight = insight))

    elseif constant_sampling && !singularity && !isnothing(indweights)
        measured_weighted = measured[indweights]
        min_objective!(opt, (params, grad) -> obj_const_weighted(params, grad, modulus,
                                                            time_series, dt,
                                                            prescribed_dot, measured_weighted, indweights;
                                                            _insight = insight))

    elseif constant_sampling && singularity && !isnothing(indweights)
        indsingular = indweights[indweights.>1]
        measured_weighted = measured[indsingular]
        # remove singularity, just go close to it, 1/10th over first sample period
        time_series[1] = 0.0 + (time_series[2] - time_series[1])/singularity_offset
        min_objective!(opt, (params, grad) -> obj_const_weighted(params, grad, modulus,
                                                        time_series, dt,
                                                        prescribed_dot, measured_weighted, indsingular;
                                                        _insight = insight))

    elseif !constant_sampling && !singularity
        min_objective!(opt, (params, grad) -> obj_var_nonsing(params, grad, modulus,
                                                        time_series, prescribed_dot,
                                                        measured; _insight = insight))

    elseif !constant_sampling && singularity
        @warn "Note that large changes in sample rate, particularly from low sample rate to
        much higher sample rate can introduce significant innacuracies due to the singularity
        approximation method being used. To avoid this, please resample your data to a constant
        sample rate before fitting."
        min_objective!(opt, (params, grad) -> obj_var_sing(params, grad, modulus,
                                                        time_series, prescribed_dot,
                                                        measured; _insight = insight))

    end

    # minimise objective func, minx are the parameters resulting in minimum
    (minf, minx, ret) = NLopt.optimize(opt, params_init)

    # return all
    return (convert(RheoFloat,minf), convert(Vector{RheoFloat},minx), ret)

end

#=
----------------------------------------------
Step fitting and predicting base functionality
----------------------------------------------
=#
function obj_step_nonsing(params, grad, modulus, t, prescribed::Float64, measured::Vector{Float64}; _insight=false)
    if _insight
        println("Current Parameters: ", params)
    end

    mod = (t->modulus(t,params))
    estimated = prescribed*mod(t)

    cost = sum((measured - estimated).^2)
end

function obj_step_weighted(params, grad, modulus, t, prescribed::Float64, measured::Vector{Float64}, weights; _insight=false)
    if _insight
        println("Current Parameters: ", params)
    end

    mod = (t->modulus(t,params))
    estimated = prescribed*mod(t)

    cost = sum((measured - estimated[weights]).^2)
end

function leastsquares_stepinit(params_init::Vector{RheoFloat}, low_bounds::RheovecOrNone,
                           hi_bounds::RheovecOrNone, modulus,
                           time_series::Vector{RheoFloat}, prescribed::RheoFloat,
                           measured::Vector{RheoFloat}; insight::Bool = false,
                           singularity::Bool = false, _rel_tol = 1e-4, indweights=nothing)

    # initialise NLOpt.Opt object with :LN_SBPLX Subplex algorithm
    opt = Opt(:LN_SBPLX, length(params_init))

    # set lower bounds and upper bounds unless they take null value
    if !isnothing(low_bounds)
        low_bounds = convert(Vector{Float64}, low_bounds)
        lower_bounds!(opt, low_bounds)
    end

    if !isnothing(hi_bounds)
        hi_bounds = convert(Vector{Float64}, hi_bounds)
        upper_bounds!(opt, hi_bounds)
    end

    # set relative tolerance
    xtol_rel!(opt, _rel_tol)

    # set Opt object as a minimisation objective. Use a closure for additional
    # arguments sent to object objectivefunc

    params_init = convert(Vector{Float64},params_init)
    time_series = convert(Vector{Float64},time_series)
    prescribed = convert(Float64,prescribed)
    measured = convert(Vector{Float64},measured)

    if isnothing(indweights) && !singularity
        min_objective!(opt, (params, grad) -> obj_step_nonsing(params, grad, modulus,
                                                            time_series, prescribed, measured;
                                                            _insight = insight))

    elseif isnothing(indweights) && singularity
        indices = collect(Integer, 2:length(measured))
        measured_weighted = measured[indices]
        min_objective!(opt, (params, grad) -> obj_step_weighted(params, grad, modulus,
                                                        time_series, prescribed, measured_weighted, indices;
                                                        _insight = insight))

    elseif !isnothing(indweights) && !singularity
        measured_weighted = measured[indweights]
        min_objective!(opt, (params, grad) -> obj_step_weighted(params, grad, modulus,
                                                            time_series, prescribed, measured_weighted, indweights;
                                                            _insight = insight))

    elseif !isnothing(indweights) && singularity
        indsingular = indweights[indweights.>1]
        measured_weighted = measured[indsingular]
        min_objective!(opt, (params, grad) -> obj_step_weighted(params, grad, modulus,
                                                        time_series, prescribed, measured_weighted, indsingular;
                                                        _insight = insight))

    end

    # minimise objective func, minx are the parameters resulting in minimum
    (minf, minx, ret) = NLopt.optimize(opt, params_init)

    # return all
    return (convert(RheoFloat,minf), convert(Vector{RheoFloat},minx), ret)

end