using Distributions: TDist, Uniform, Normal, cf
import Distributions: cdf
"""
      Binomial Distribution is a distribution well known for its use in
           statistical tests and decision making models. In order to calculate
           binomial distribution, you will need the positives and size of your array.\n
      --------------------\n
      positives = 5\n
      n = 10\n
      r = binomial_dist(positives,n)
       """
function binomial_dist(positives, size; mode = :REC)
    # p = n! / x!(n-x!)*π^x*(1-π)^N-x
    n = size
    x = positives
    if mode != :REC
        factn = factorial(big(n))
        factx = factorial(big(x))
    else
        factn = fact(n)
        factx = fact(x)
    end
    return(factn / (factx * (n-x)))
end
# ---- Normal Distribution ----
"""
    ## Normal Distribution
    ### Description
      Calculates the normal distribution of an array.\n
      --------------------\n
    ### Input
      NormalDist(x)\n
      --------------------\n
      #### Positional Arguments
      Array{Any} - x:: Array for which the normal distribution should use the
      data from.\n
      --------------------\n
     ### Output
     norm:: A Lathe distribution\n
     ---------------------\n
     ### Functions
     Distribution.apply(xt) :: Applies the distribution to xt\n
     Distribution.cdf(statistic, alpha, dof) :: Applies the distribution's
     corresponding cummulitive distribution function.\n
     ---------------------\n
     ### Data
     σ :: Standard Deviation of the input data.\n
     μ :: Mean of the input data.
       """
struct NormalDist{c, p} <: Distribution
           σ::Float64
           μ::Float64
           N::Int64
           cdf::c
           apply::p
           function NormalDist(array)
              N = length(array)
               σ = std(array)
               μ = mean(array)
               apply(xt) = Array{Real}([i = (i-μ) / σ for i in xt])
               cdf(xt) = bvnuppercdf(σ, μ, xt)
               new{typeof(cdf), typeof(apply)}(σ, μ, N, cdf, apply)
            end
end
# ---- T distribution ----
"""
    ## T Distribution
    ### Description
      Calculates the T distribution of an array.\n
      --------------------\n
    ### Input
      TDist(x)\n
      --------------------\n
      #### Positional Arguments
      Array{Any} - x:: Array for which the T distribution should use the
      data from.\n
      --------------------\n
     ### Output
     t:: A Lathe distribution\n
     ---------------------\n
     ### Functions
     Distribution.apply(xt) :: Applies the distribution to xt\n
     Distribution.cdf(statistic, alpha, dof) :: Applies the distribution's
     corresponding cummulitive distribution function.\n
     ---------------------\n
     ### Data
     μ :: Mean of the input data.\n
     N :: The length of the input data.
       """
struct T_Dist{c, p} <: Distribution
μ::Float64
N::Int64
apply::p
cdf::c

function T_Dist(general)
  norm = NormalDist(general)
  general = norm.apply(general)
  μ = mean(general)
  N = length(general)
  apply(xt) = (mean(norm.apply(xt)) - μ) / (std(norm.apply(xt)) / sqrt(N))
  cdf(t, dog) = cf(TDist(dog), Real(t))
  new{typeof(cdf), typeof(apply)}(μ, N, apply, cdf)
  end
end
# ---- Uniform Dist ----
"""
    ## Uniform Distribution
    ### Description
      Calculates the Uniform distribution of an array.\n
      --------------------\n
    ### Input
      UniformDist(x)\n
      --------------------\n
      #### Positional Arguments
      Array{Any} - x:: Array for which the T distribution should use the
      data from.\n
      --------------------\n
     ### Output
     t:: A Lathe distribution\n
     ---------------------\n
     ### Functions
     Distribution.apply(xt) :: Applies the distribution to xt\n
     Distribution.cdf(xt) :: Applies the distribution's
     corresponding cummulitive distribution function.\n
     ---------------------
       """
function UniformDist(array)
    dist = Uniform(minimum(array), maximum(array))
    apply(xt) = pdf(dist, xt)
    cdf(x) = cdf(dist, x)
    (var) ->(dist;cdf;apply)
end

function bvnuppercdf(dh::Float64, dk::Float64, r::Float64)
	if abs(r) < 0.3
	   ng = 1
	   lg = 3
	elseif abs(r) < 0.75
	   ng = 2
	   lg = 6
	else
	   ng = 3
	   lg = 10
	end
	h = dh
	k = dk
	hk = h*k
	bvn = 0.0
	if abs(r) < 0.925
	   	if abs(r) > 0
	      	hs = (h * h + k * k) * 0.5
	      	asr = asin(r)
	      	for i = 1:lg
	         	for j = -1:2:1
	            	sn = sin(asr * (j * bvncdf_x_array[i, ng] + 1.0) * 0.5)
	            	bvn += bvncdf_w_array[i, ng] * exp((sn * hk - hs) / (1.0 - sn*sn))
	        	end
	      	end
	      	bvn *= asr / (4.0pi)
	   	end
	   	bvn += cdf(Normal(), -h) * cdf(Normal(), -k)
	else
	   	if r < 0
	      	k = -k
	      	hk = -hk
	   	end
	   	if abs(r) < 1
	      	as = (1.0 - r) * (1.0 + r)
	      	a = sqrt(as)
	      	bs = (h - k)^2
	      	c = (4.0 - hk) * 0.125
	      	d = (12.0 - hk) * 0.0625
	      	asr = -(bs / as + hk) * 0.5
	      	if ( asr > -100 )
	      		bvn = a * exp(asr) * (1.0 - c * (bs - as) * (1.0 - d * bs / 5.0) / 3.0 + c * d * as * as / 5.0)
	      	end
	      	if -hk < 100
	         	b = sqrt(bs)
	         	bvn -= exp(-hk * 0.5) * sqrt(2.0pi) * cdf(Normal(), -b / a) * b * (1.0 - c * bs * (1.0 - d * bs / 5.0) / 3.0)
	      	end
	     	a /= 2.0
		    for i = 1:lg
	         	for j = -1:2:1
	            	xs = (a * (j*bvncdf_x_array[i, ng] + 1.0))^2
	            	rs = sqrt(1.0 - xs)
	            	asr = -(bs / xs + hk) * 0.5
	            	if asr > -100
	               		bvn += a * bvncdf_w_array[i, ng] * exp(asr) * (exp(-hk * (1.0 - rs) / (2.0 * (1.0 + rs))) / rs - (1.0 + c * xs * (1.0 + d * xs)))
	            	end
	         	end
	        end
	      	bvn /= -2.0pi
	   	end
	   	if r > 0
	      	bvn += cdf(Normal(), -max(h, k))
	   	else
	      	bvn = -bvn
	      	if k > h
	      		bvn += cdf(Normal(), k) - cdf(Normal(), h)
	      	end
		end
	end
	return bvn
end


#==
Linear
    Regression
==#
@doc """
      Linear Regression is a well-known linear function used for predicting
      continuous features with a mostly linear or semi-linear slope.\n
      --------------------\n
      ==PARAMETERS==\n
     [y] <- Fill with your trainY values. Should be an array of shape (0,1) or (1,0)\n
     [x] <- Fill in with your trainX values. Should be an array of shape (0,1) or (1,0)\n
     --------------------\n
     ==Functions==\n
     predict(xt) <- Returns a prediction from the model based on the xtrain value passed (xt)
       """
mutable struct LinearRegression{P} <: LinearModel
    A::Float64
    B::Float64
    predict::P
    regressors::Array{LinearModel}
    function LinearRegression(x::Array,y::Array)
        # a = ((∑y)(∑x^2)-(∑x)(∑xy)) / (n(∑x^2) - (∑x)^2)
        # b = (x(∑xy) - (∑x)(∑y)) / n(∑x^2) - (∑x)^2
        regressors = []
        if length(x) != length(y)
            throw(ArgumentError("The array shape does not match!"))
        end
        # Get our Summations:
        Σx = sum(x)
        Σy = sum(y)
        # dot x and y
        xy = x .* y
        # ∑dot x and y
        Σxy = sum(xy)
        # dotsquare x
        x2 = x .^ 2
        # ∑ dotsquare x
        Σx2 = sum(x2)
        # n = sample size
        n = length(x)
        # Calculate a
        a = (((Σy) * (Σx2)) - ((Σx * (Σxy)))) / ((n * (Σx2))-(Σx^2))
        # Calculate b
        b = ((n*(Σxy)) - (Σx * Σy)) / ((n * (Σx2)) - (Σx ^ 2))
        predict(xt::Array) = (xt = [i = a + (b * i) for i in xt])
        P = typeof(predict)
        return new{P}(a, b, predict, [])
    end
        function LinearRegression(x::DataFrame,y::Array)
            # a = ((∑y)(∑x^2)-(∑x)(∑xy)) / (n(∑x^2) - (∑x)^2)
            # b = (x(∑xy) - (∑x)(∑y)) / n(∑x^2) - (∑x)^2
            regressors = []
            count = 1
            [push!(regressors, LinearRegression(feature, y) for feature in x)]
            a = nothing
            b = nothing
            for m in regressors
                if a != nothing
                    a = mean(a, m.a)
                    b = mean(b, m.b)
                else
                    a = m.a
                    b = m.b
                end
            end
            predict(xt::DataFrame) = _compare_predCon(models, xt)
            P = typeof(predict)
            return new{P}(a, b, predict, regressors)
    end
end
#==
Linear
    Least
     Square
==#
"""

      Least Square regressors are ideal for predicting continous features.\n
      --------------------\n
      x = [7,6,5,6,5]\n
      y  = [3.4.5.6.3]\n
      xtrain = [7,5,4,5,3,5,7,8]\n
      Type = :LIN\n
      model = models.LeastSquare(x,y,Type)\n
      y_pred = models.predict(model,xtrain)\n
      -------------------\n
      HYPER PARAMETERS\n
      --------------------\n
      ==Functions==\n
      predict(xt) <- Returns a prediction from the model based on the xtrain value passed (xt)
       """
function LinearLeastSquare(x,y)
    if length(x) != length(y)
        throw(ArgumentError("The array shape does not match!"))
    end
        xy = x .* y
        sxy = sum(xy)
        n = length(x)
        x2 = x .^ 2
        sx2 = sum(x2)
        sx = sum(x)
        sy = sum(y)
        # Calculate the slope:
        a = ((n*sxy) - (sx * sy)) / ((n * sx2) - (sx)^2)
     # Calculate the y intercept
        b = (sy - (a*sx)) / n
    predict(xt) = (xt = [z = (a * x) + b for x in xt])
    (var)->(a;b;predict)
end
#==
Lasso
    Regression
==#
function lasso_cost(X, Y, B, alp)
     m = length(Y)
     cost = (sum(((X .* B) - Y).^2)/2) +(sum(broadcast(abs, B)))*alp/2
     return cost
end
function gradientDescent(X, Y, B, learningRate, numIterations, alp)
    m = length(Y)
    for iteration in 1:numIterations
        loss = (X .* B) - Y
        for i in 1:length(X)
            if(B[i]==0)
                B[i]=1
            end
        end
        gradient = ((X' * loss) .+ (alp .* (broadcast(abs, B) ./ B))) / m
        B = B - learningRate * gradient
    end
    return B
end
mutable struct LassoRegression{P}
    x::Array
    y::Array
    predict::P
    lambda::Float64
    i::Float64
    LearningRate::Float64
    n_iterations::Int64
    function LassoRegression(x, y, lambda = 0; lr = .01, n_iterations = 1200)
        i = 10 ^ 6
        predict(xt) = pred_lasso(x, y, xt, i, learningRate = lr, n_iterations = n_iterations)
        new{typeof(predict)}(x, y, predict, lambda, i, lr, n_iterations)
    end
    function pred_lasso(x, y, B, i; learningRate = .01, n_iterations = 1200)
        costm = 10 ^ 30
        newB = zeros(size(B))
          while i > 0.001
             B1 = gradientDescent(x, y, B, learningRate, 1200,i)
             cost = lasso_cost(x, y, B1, i)
            if (cost < costm)
                costm = cost
                newB = B1
                lambda = i
            end
         i=i/2
        end
        return(B .* newB)
    end
end


function F!{T}(byp::BansalYaronProblem, μs::Vector{Float64}, σs::Vector{Float64}, bs::Matrix{Float64}, bs∂μ::Matrix{Float64}, bs∂σ::Matrix{Float64}, bs∂2μ::Matrix{Float64}, bs∂2σ::Matrix{Float64}, y::Vector{T}, ydot::Vector{T})
    μn = length(μs)
    σn = length(σs)
    IJ = μn * σn
    ij = 0
    @inbounds for σi in 1:σn, μi in 1:μn
        ij += 1
        V = zero(T)
        @simd for kl in 1:IJ
            V += bs[kl, ij] * y[kl]
        end
        V∂μ = zero(T)
        @simd for kl in 1:IJ
            V∂μ += bs∂μ[kl, ij] * y[kl]
        end
        V∂σ = zero(T)
        @simd for kl in 1:IJ
            V∂σ += bs∂σ[kl, ij] * y[kl]
        end
        V∂2μ = zero(T)
        @simd for kl in 1:IJ
            V∂2μ += bs∂2μ[kl, ij] * y[kl]
        end
        V∂2σ = zero(T)
        @simd for kl in 1:IJ
            V∂2σ += bs∂2σ[kl, ij] * y[kl]
        end
        if (μi == 1 | μi == μn)
            ydot[ij] = V∂μ
        elseif (σi == 1 | σi == σn)
            ydot[ij] = V∂σ
        else
            ydot[ij] = (byp.ρ * byp.θ * max(zero(T), V^(1-1/byp.θ))
                           + (- byp.ρ * byp.θ + (1-byp.γ) * μs[μi] - 0.5 * (1-byp.γ) * byp.γ * byp.νD^2 * σs[σi]) * V
                           + (byp.κμ * (byp.μ - μs[μi])) * V∂μ
                           + (byp.κσ * (1.0 - σs[σi])) * V∂σ
                           + (0.5 * byp.νμ^2 * σs[σi]) * V∂2μ
                           + (0.5 * byp.νσ^2 * σs[σi]) * V∂2σ
                           )
        end
    end
    return ydot
end

function solve(::Type{Val{:spectral}}, byp::BansalYaronProblem, μs, σs,  bs::Matrix{Float64}, bs∂μ::Matrix{Float64}, bs∂σ::Matrix{Float64}, bs∂2μ::Matrix{Float64}, bs∂2σ::Matrix{Float64}, oldV; kwargs...)
    out = nlsolve((y, ydot) -> F!(byp, μs, σs,  bs, bs∂μ, bs∂σ, bs∂2μ, bs∂2σ, y, ydot), oldV; method = :trust_region, show_trace = true, kwargs...)
    V = bs' * out.zero
    return μs, σs, V
end

function solve(::Type{Val{:spectral}}, byp::BansalYaronProblem; iterations = 50, kwargs...)
    μmin = byp.μ - 6 * byp.νμ / sqrt(2*byp.κμ)
    μmax = byp.μ + 6 * byp.νμ / sqrt(2*byp.κμ)
    σmin = 0.0
    σmax = 3.0
    basisμ = Basis(Cheb, 25, μmin, μmax)
    basisσ = Basis(Cheb, 25, σmin, σmax)
    basis = Basis(basisμ, basisσ)
    S, (μs, σs) = nodes(basis)
    bs = BasisStructure(basis, Expanded(), S, [0 0]').vals[1]'
    bs∂μ = BasisStructure(basis, Expanded(), S, [1 0]').vals[1]'
    bs∂σ = BasisStructure(basis, Expanded(), S, [0 1]').vals[1]'
    bs∂2μ = BasisStructure(basis, Expanded(), S, [2 0]').vals[1]'
    bs∂2σ = BasisStructure(basis, Expanded(), S, [0 2]').vals[1]'
    oldV = bs' \ fill(byp.V, size(bs, 1))
    solve(Val{:spectral}, byp, μs, σs, bs, bs∂μ, bs∂σ, bs∂2μ, bs∂2σ, oldV; iterations = iterations, kwargs...)
end


struct LKPyramid
    layers::Vector{Matrix{Gray{Float64}}}

    Iy::Union{Nothing, Vector{Matrix{Gray{Float64}}}}
    Ix::Union{Nothing, Vector{Matrix{Gray{Float64}}}}

    Iyy::Union{Nothing, Vector{Matrix{Gray{Float64}}}}
    Ixx::Union{Nothing, Vector{Matrix{Gray{Float64}}}}
    Iyx::Union{Nothing, Vector{Matrix{Gray{Float64}}}}
end

function LKPyramid(image, levels; downsample = 2, σ = 1.0, compute_gradients::Bool = true)
    pyramid = gaussian_pyramid(image, levels, downsample, σ)
    !compute_gradients &&
        return LKPyramid(pyramid, nothing, nothing, nothing, nothing, nothing)

    total_levels = levels + 1
    Iy = Vector{Matrix{Gray{Float64}}}(undef, total_levels)
    Ix = Vector{Matrix{Gray{Float64}}}(undef, total_levels)

    Iyy = Vector{Matrix{Gray{Float64}}}(undef, total_levels)
    Ixx = Vector{Matrix{Gray{Float64}}}(undef, total_levels)
    Iyx = Vector{Matrix{Gray{Float64}}}(undef, total_levels)

    filling = Fill(zero(eltype(pyramid[1])))
    for (i, layer) in enumerate(pyramid)
        Iy[i], Ix[i] = imgradients(layer, KernelFactors.scharr, filling)
        Iyy[i], Ixx[i], Iyx[i] = compute_partial_derivatives(Iy[i], Ix[i])
    end
    LKPyramid(pyramid, Iy, Ix, Iyy, Ixx, Iyx)
end

function optflow(
    first_img::AbstractMatrix{T}, second_img::AbstractMatrix{T},
    points::Vector{SVector{2, Float64}}, algorithm::LucasKanade,
) where T <: Gray
    displacement = fill(SVector{2, Float64}(0.0, 0.0), length(points))
    first_pyramid = LKPyramid(first_img, algorithm.pyramid_levels)
    second_pyramid = LKPyramid(
        second_img, algorithm.pyramid_levels; compute_gradients=false,
    )
    optflow!(first_pyramid, second_pyramid, points, displacement, algorithm)
end

function optflow!(
    first_img::AbstractMatrix{T}, second_img::AbstractMatrix{T},
    points::Vector{SVector{2, Float64}}, displacement::Vector{SVector{2, Float64}},
    algorithm::LucasKanade,
) where T <: Gray
    first_pyramid = LKPyramid(first_img, algorithm.pyramid_levels)
    second_pyramid = LKPyramid(
        second_img, algorithm.pyramid_levels; compute_gradients=false,
    )
    optflow!(first_pyramid, second_pyramid, points, displacement, algorithm)
end

function optflow!(
    first_pyramid::LKPyramid, second_pyramid::LKPyramid,
    points::Vector{SVector{2, Float64}}, displacement::Vector{SVector{2, Float64}},
    algorithm::LucasKanade,
)
    has_enough_layers =
        length(first_pyramid.layers) > algorithm.pyramid_levels &&
        length(second_pyramid.layers) > algorithm.pyramid_levels
    has_enough_layers || throw("Not enough layers in pyramids.")

    n_points = length(points)
    status = trues(n_points)

    window = algorithm.window_size
    window2x = 2 * window

    for level in (algorithm.pyramid_levels + 1):-1:1
        level_resolution = axes(first_pyramid.layers[level])
        # Interpolate layer to get sub-pixel precision.
        # We never go out-of-bound, so there is no need to extrapolate.
        interploated_layer = interpolate(second_pyramid.layers[level], BSpline(Linear()))

        for n in 1:n_points
            @inbounds !status[n] && continue

            point = get_pyramid_coordinate(@inbounds(points[n]), level)
            offsets = get_offsets(point, point, window, level_resolution)
            grid = get_grid(point, offsets)

            G_inv, min_eigenvalue = compute_spatial_gradient(first_pyramid, grid, level)
            if min_eigenvalue < algorithm.eigenvalue_threshold
                @inbounds status[n] = false
                continue
            end

            pyramid_contribution = SVector{2}(0.0, 0.0)
            for _ in 1:algorithm.iterations
                putative_flow = @inbounds displacement[n] + pyramid_contribution
                putative_correspondence = point + putative_flow
                if !lies_in(level_resolution, putative_correspondence)
                    @inbounds status[n] = false
                    break
                end

                new_offsets = get_offsets(
                    point, putative_correspondence, window, level_resolution,
                )
                # Recalculate gradient only if the offset changes.
                if new_offsets != offsets
                    offsets = new_offsets
                    grid = get_grid(point, offsets)
                    G_inv, min_eigenvalue = compute_spatial_gradient(
                        first_pyramid, grid, level,
                    )
                    if min_eigenvalue < algorithm.eigenvalue_threshold
                        @inbounds status[n] = false
                        break
                    end
                else
                    offsets = new_offsets
                    grid = get_grid(point, offsets)
                end

                estimated_flow = compute_flow_vector(
                    putative_correspondence,
                    first_pyramid, interploated_layer, level,
                    grid, offsets, G_inv,
                )
                # Epsilon termination criteria.
                abs(estimated_flow[1]) < algorithm.ϵ &&
                    abs(estimated_flow[2]) < algorithm.ϵ && break

                pyramid_contribution += estimated_flow
                # Check if tracked point is out of image bounds.
                if !lies_in(level_resolution, point + pyramid_contribution)
                    @inbounds status[n] = false
                    break
                end
            end
            @inbounds begin
            # Check if flow is too big.
            if status[n] && is_lost(displacement[n], window2x)
                status[n] = false
            end
            if status[n]
                displacement[n] += pyramid_contribution
                level != 1 && (displacement[n] *= 2.0)
            end
            end
        end
    end

    displacement, status
end

function compute_partial_derivatives(Iy, Ix; σ = 4)
    kernel = KernelFactors.gaussian(σ)
    kernel_factors = kernelfactors((kernel, kernel))

    squared = typeof(Iy)(undef, size(Iy))
    filtered = typeof(Iy)(undef, size(Iy))

    squared .= Iy .* Iy
    imfilter!(filtered, squared, kernel_factors)
    Iyy_integral_table = integral_image(filtered)

    squared .= Ix .* Ix
    imfilter!(filtered, squared, kernel_factors)
    Ixx_integral_table = integral_image(filtered)

    squared .= Iy .* Ix
    imfilter!(filtered, squared, kernel_factors)
    Iyx_integral_table = integral_image(filtered)

    Iyy_integral_table, Ixx_integral_table, Iyx_integral_table
end

function _compute_spatial_gradient(
    grid, Iyy_integral, Iyx_integral, Ixx_integral,
)
    sum_Iyy = boxdiff(Iyy_integral, grid[1], grid[2])
    sum_Ixx = boxdiff(Ixx_integral, grid[1], grid[2])
    sum_Iyx = boxdiff(Iyx_integral, grid[1], grid[2])
    SMatrix{2, 2, Float64}(sum_Iyy, sum_Iyx, sum_Iyx, sum_Ixx)
end

function compute_spatial_gradient(pyramid::LKPyramid, grid, level)
    G = _compute_spatial_gradient(
        grid, pyramid.Iyy[level], pyramid.Iyx[level], pyramid.Ixx[level],
    )
    U, S, V = svd2x2(G)
    G_inv = pinv2x2(U, S, V)
    min_eigenvalue = min(S[1, 1], S[2, 2]) / prod(length.(grid))

    G_inv, min_eigenvalue
end

function prepare_linear_system(corresponding_point, A, Iy, Ix, offsets, B)
    P, Q = size(A)
    by, bx = 0.0, 0.0

    @inbounds for q in 1:Q, p in 1:P
        r = corresponding_point[1] + offsets[1][p]
        c = corresponding_point[2] + offsets[2][q]

        δI = A[p, q] - B(r, c)
        by += δI * Iy[p, q]
        bx += δI * Ix[p, q]
    end

    SVector{2, Float64}(by, bx)
end

function compute_flow_vector(
    corresponding_point,
    first_pyramid::LKPyramid, etp, level,
    grid, offsets, G_inv,
)
    b = prepare_linear_system(
        corresponding_point,
        view(first_pyramid.layers[level], grid[1], grid[2]),
        view(first_pyramid.Iy[level], grid[1], grid[2]),
        view(first_pyramid.Ix[level], grid[1], grid[2]),
        offsets, etp,
    )
    G_inv * b
end

@inline is_lost(displacement, window) =
    @inbounds(displacement[1] > window || displacement[2] > window)

@inline lies_in(area, point) = @inbounds(
    first(area[1]) ≤ point[1] ≤ last(area[1]) &&
    first(area[2]) ≤ point[2] ≤ last(area[2])
)

"""
# Arguments
- `level`: Level of the pyramid in `[1, levels]` range.
"""
@inline get_pyramid_coordinate(point, level) = floor.(Int, point ./ 2 ^ (level - 1))

function get_offsets(point, new_point, window, image_axes)
    rows, cols = image_axes

    @inbounds begin
    up = floor(Int, min(window, min(point[1], new_point[1]) - first(rows)))
    down = floor(Int, min(window, last(rows) - max(point[1], new_point[1])))
    left = floor(Int, min(window, min(point[2], new_point[2]) - first(cols)))
    right = floor(Int, min(window, last(cols) - max(point[2], new_point[2])))
    end

    (-up:down, -left:right)
end

function get_grid(point, offsets)
    map(point, offsets) do p, o
        p+first(o):p+last(o)
    end
end


##### Generic methods #####

"""
    length(d::MultivariateDistribution) -> Int

Return the sample dimension of distribution `d`.
"""
length(d::MultivariateDistribution)

"""
    size(d::MultivariateDistribution)

Return the sample size of distribution `d`, *i.e* `(length(d),)`.
"""
size(d::MultivariateDistribution)

## sampling

"""
    rand!(d::MultivariateDistribution, x::AbstractArray)

Draw samples and output them to a pre-allocated array x. Here, x can be either a vector of
length `dim(d)` or a matrix with `dim(d)` rows.
"""
rand!(d::MultivariateDistribution, x::AbstractArray)

function rand!(d::MultivariateDistribution, x::AbstractVector)
    length(x) == length(d) ||
        throw(DimensionMismatch("Output size inconsistent with sample length."))
    _rand!(d, x)
end

function rand!(d::MultivariateDistribution, A::AbstractMatrix)
    size(A,1) == length(d) ||
        throw(DimensionMismatch("Output size inconsistent with sample length."))
    _rand!(sampler(d), A)
end

"""
    rand(d::MultivariateDistribution)

Sample a vector from the distribution `d`.

    rand(d::MultivariateDistribution, n::Int) -> Vector

Sample n vectors from the distribution `d`. This returns a matrix of size `(dim(d), n)`,
where each column is a sample.
"""
rand(d::MultivariateDistribution) = _rand!(d, Vector{eltype(d)}(undef, length(d)))
rand(d::MultivariateDistribution, n::Int) = _rand!(sampler(d), Matrix{eltype(d)}(undef, length(d), n))

"""
    _rand!(d::MultivariateDistribution, x::AbstractArray)

Generate a vector sample to `x`. This function does not need to perform dimension checking.
"""
_rand!(d::MultivariateDistribution, x::AbstractArray)

## domain

"""
    insupport(d::MultivariateDistribution, x::AbstractArray)

If ``x`` is a vector, it returns whether x is within the support of ``d``.
If ``x`` is a matrix, it returns whether every column in ``x`` is within the support of ``d``.
"""
insupport{D<:MultivariateDistribution}(d::Union{D, Type{D}}, x::AbstractArray)

function insupport!(r::AbstractArray, d::Union{D,Type{D}}, X::AbstractMatrix) where D<:MultivariateDistribution
    n = length(r)
    size(X) == (length(d),n) ||
        throw(DimensionMismatch("Inconsistent array dimensions."))
    for i in 1:n
        @inbounds r[i] = insupport(d, view(X, :, i))
    end
    return r
end

insupport(d::Union{D,Type{D}}, X::AbstractMatrix) where {D<:MultivariateDistribution} =
    insupport!(BitArray(undef, size(X,2)), d, X)

## statistics

"""
    mean(d::MultivariateDistribution)

Compute the mean vector of distribution `d`.
"""
mean(d::MultivariateDistribution)

"""
    var(d::MultivariateDistribution)

Compute the vector of element-wise variances for distribution `d`.
"""
var(d::MultivariateDistribution)

"""
    entropy(d::MultivariateDistribution)

Compute the entropy value of distribution `d`.
"""
entropy(d::MultivariateDistribution)

"""
    entropy(d::MultivariateDistribution, b::Real)

Compute the entropy value of distribution ``d``, w.r.t. a given base.
"""
entropy(d::MultivariateDistribution, b::Real) = entropy(d) / log(b)

"""
    cov(d::MultivariateDistribution)

Compute the covariance matrix for distribution `d`. (`cor` is provided based on `cov`).
"""
cov(d::MultivariateDistribution)

"""
    cor(d::MultivariateDistribution)

Computes the correlation matrix for distribution `d`.
"""
function cor(d::MultivariateDistribution)
    C = cov(d)
    n = size(C, 1)
    @assert size(C, 2) == n
    R = Matrix{eltype(C)}(undef, n, n)

    for j = 1:n
        for i = 1:j-1
            @inbounds R[i, j] = R[j, i]
        end
        R[j, j] = 1.0
        for i = j+1:n
            @inbounds R[i, j] = C[i, j] / sqrt(C[i, i] * C[j, j])
        end
    end

    return R
end

# pdf and logpdf

"""
    pdf(d::MultivariateDistribution, x::AbstractArray)

Return the probability density of distribution `d` evaluated at `x`.

- If `x` is a vector, it returns the result as a scalar.
- If `x` is a matrix with n columns, it returns a vector `r` of length n, where `r[i]` corresponds
to `x[:,i]` (i.e. treating each column as a sample).

`pdf!(r, d, x)` will write the results to a pre-allocated array `r`.
"""
pdf(d::MultivariateDistribution, x::AbstractArray)

"""
    logpdf(d::MultivariateDistribution, x::AbstractArray)

Return the logarithm of probability density evaluated at `x`.

- If `x` is a vector, it returns the result as a scalar.
- If `x` is a matrix with n columns, it returns a vector `r` of length n, where `r[i]` corresponds to `x[:,i]`.

`logpdf!(r, d, x)` will write the results to a pre-allocated array `r`.
"""
logpdf(d::MultivariateDistribution, x::AbstractArray)

_pdf(d::MultivariateDistribution, X::AbstractVector) = exp(_logpdf(d, X))

function logpdf(d::MultivariateDistribution, X::AbstractVector)
    length(X) == length(d) ||
        throw(DimensionMismatch("Inconsistent array dimensions."))
    _logpdf(d, X)
end

function pdf(d::MultivariateDistribution, X::AbstractVector)
    length(X) == length(d) ||
        throw(DimensionMismatch("Inconsistent array dimensions."))
    _pdf(d, X)
end

function _logpdf!(r::AbstractArray, d::MultivariateDistribution, X::AbstractMatrix)
    for i in 1 : size(X,2)
        @inbounds r[i] = logpdf(d, view(X,:,i))
    end
    return r
end

function _pdf!(r::AbstractArray, d::MultivariateDistribution, X::AbstractMatrix)
    for i in 1 : size(X,2)
        @inbounds r[i] = pdf(d, view(X,:,i))
    end
    return r
end

function logpdf!(r::AbstractArray, d::MultivariateDistribution, X::AbstractMatrix)
    size(X) == (length(d), length(r)) ||
        throw(DimensionMismatch("Inconsistent array dimensions."))
    _logpdf!(r, d, X)
end

function pdf!(r::AbstractArray, d::MultivariateDistribution, X::AbstractMatrix)
    size(X) == (length(d), length(r)) ||
        throw(DimensionMismatch("Inconsistent array dimensions."))
    _pdf!(r, d, X)
end

function logpdf(d::MultivariateDistribution, X::AbstractMatrix)
    size(X, 1) == length(d) ||
        throw(DimensionMismatch("Inconsistent array dimensions."))
    T = promote_type(partype(d), eltype(X))
    _logpdf!(Vector{T}(undef, size(X,2)), d, X)
end

function pdf(d::MultivariateDistribution, X::AbstractMatrix)
    size(X, 1) == length(d) ||
        throw(DimensionMismatch("Inconsistent array dimensions."))
    T = promote_type(partype(d), eltype(X))
    _pdf!(Vector{T}(undef, size(X,2)), d, X)
end

"""
    _logpdf{T<:Real}(d::MultivariateDistribution, x::AbstractArray)

Evaluate logarithm of pdf value for a given vector `x`. This function need not perform dimension checking.
Generally, one does not need to implement `pdf` (or `_pdf`) as fallback methods are provided in `src/multivariates.jl`.
"""
_logpdf(d::MultivariateDistribution, x::AbstractArray)

"""
    loglikelihood(d::MultivariateDistribution, x::AbstractMatrix)

The log-likelihood of distribution `d` w.r.t. all columns contained in matrix `x`.
"""
function loglikelihood(d::MultivariateDistribution, X::AbstractMatrix)
    size(X, 1) == length(d) || throw(DimensionMismatch("Inconsistent array dimensions."))
    return sum(i -> _logpdf(d, view(X, :, i)), 1:size(X, 2))
end

##### Specific distributions #####

for fname in ["dirichlet.jl",
              "multinomial.jl",
              "dirichletmultinomial.jl",
              "mvnormal.jl",
              "mvnormalcanon.jl",
              "mvlognormal.jl",
              "mvtdist.jl",
              "vonmisesfisher.jl"]
    include(joinpath("multivariate", fname))
end


using DataDrivenDiffEq
using ModelingToolkit
using LinearAlgebra
using OrdinaryDiffEq
using Test

@testset "Basis" begin
    @variables u[1:3]
    @parameters w[1:2]
    h = [1u[1]; 1u[2]; cos(w[1]*u[2]+w[2]*u[3]); 1u[3]+1u[2]]
    basis = Basis(h, u, parameters = w)
    basis_2 = unique(basis)
    @test size(basis) == size(h)
    # TODO This works fine for "manual" execution of the testset but fails
    # with Pkg.test
    @test basis([1.0; 2.0; π], p = [0. 1.]) ≈ [1.0; 2.0; -1.0; π+2.0]
    @test size(basis) == size(basis_2)
    push!(basis_2, sin(u[2]))
    @test size(basis_2)[1] == length(h)+1
    push!(basis, 1u[3]+1u[2])
    unique!(basis)
    @test size(basis) == size(h)
    g = [1.0*u[1]; 1.0*u[3]; 1.0*u[2]]
    basis = Basis(g, u, parameters = w)
    f = jacobian(basis)
    @test f([1;1;1], [0.0 0.0]) ≈ [1.0 0.0 0.0; 0.0 0.0 1.0; 0.0 1.0 0.0]
    @test_nowarn sys = ODESystem(basis)
end

@testset "DMD" begin
    # Create some linear data
    A = [0.9 -0.2; 0.0 0.2]
    y = [[10.; -10.]]
    for i in 1:10
        push!(y, A*y[end])
    end
    X = hcat(y...)
    @test_throws AssertionError ExactDMD(X[:, 1:end-2], dt = -1.0)
    estimator = ExactDMD(X[:,1:end-2])
    @test isstable(estimator)
    @test estimator.Ã ≈ A
    @test eigvals(estimator) ≈ eigvals(A)
    @test eigvecs(estimator) ≈ eigvecs(A)
    @test_nowarn dynamics(estimator)
    @test_throws AssertionError dynamics(estimator, discrete = false)
    @test_nowarn update!(estimator, X[:, end-1], X[:,end])
end

@testset "EDMD" begin
    # Test for linear system
    function linear_sys(u, p, t)
        x = -0.9*u[1]
        y = -0.3*u[2]
        return [x;y]
    end

    u0 = [π; 1.0]
    tspan = (0.0, 20.0)
    prob = DiscreteProblem(linear_sys, u0, tspan)
    sol = solve(prob,FunctionMap())

    @variables u[1:2]
    h = [1u[1]; 1u[2]; sin(u[1]); cos(u[1]); u[1]*u[2]]
    basis = Basis(h, u)

    @test_throws AssertionError ExtendedDMD(sol[:,:], basis, dt = -1.0)
    estimator = ExtendedDMD(sol[:,:], basis)
    @test basis == estimator.basis
    basis_2 = reduce_basis(estimator, threshold = 1e-5)
    @test size(basis_2)[1] < size(basis)[1]
    estimator_2 = ExtendedDMD(sol[:,:], basis_2)
    p1 = DiscreteProblem(dynamics(estimator), u0, tspan, [])
    s1 = solve(p1,FunctionMap())
    p2 = DiscreteProblem(dynamics(estimator_2), u0, tspan, [])
    s2 = solve(p2,FunctionMap())
    p3 = DiscreteProblem(linear_dynamics(estimator_2), estimator_2(u0), tspan, [])
    s3 = solve(p3,FunctionMap())
    @test sol[:,:] ≈ s1[:,:]
    @test sol[:,:] ≈ s2[:,:]
    @test hcat(estimator_2.basis.(eachcol(sol[:,:]))...)≈ s3[:,:]
    @test eigvals(estimator_2) ≈ [-0.9; -0.3]

    # Test for nonlinear system
    function nonlinear_sys(du, u, p, t)
        du[1] = sin(u[1])
        du[2] = -0.3*u[2] -0.9*u[1]
    end

    prob = DiscreteProblem(nonlinear_sys, u0, tspan)
    sol = solve(prob,FunctionMap())
    estimator = ExtendedDMD(sol[:,:], basis)
    p4 = DiscreteProblem(dynamics(estimator), u0, tspan, [])
    s4 = solve(p4,FunctionMap())
    @test sol[:,:] ≈ s4[:,:]
end

@testset "DMDc" begin
    # Define measurements from unstable system with known control input
    X = [4 2 1 0.5 0.25; 7 0.7 0.07 0.007 0.0007]
    U = [-4 -2 -1 -0.5]
    B = Float32[1; 0]

    # But with a little more knowledge
    sys = DMDc(X, U, B = B)
    @test isa(get_dynamics(sys), ExactDMD)
    @test sys.koopman.Ã ≈[1.5 0; 0 0.1]
    @test get_input_map(sys) ≈ [1.0; 0.0]
    @test !isstable(sys)
    @test_nowarn eigen(sys)

    # Check the solution of an unforced and forced system against each other
    dudt_ = dynamics(sys)
    prob = DiscreteProblem(dudt_, X[:, 1], (0., 10.))
    sol_unforced = solve(prob,  FunctionMap())

    dudt_ = dynamics(sys, control = (u, p, t) -> -0.5u[1])
    prob = DiscreteProblem(dudt_, X[:, 1], (0., 10.))
    sol = solve(prob, FunctionMap())

    @test all(abs.(diff(sol[1,:])) .< 1e-5)
    @test sol[2,:] ≈ sol_unforced[2,:]
end

@testset "SInDy" begin
    # Test the pendulum
    function pendulum(u, p, t)
        du1 = u[2]
        du2 = -sin(u[1]) - 0.1*u[2]
        return [du1; du2]
    end
    u0 = [0.99π; 0.3]
    tspan = (0.0, 10.0)
    prob = ODEProblem(pendulum, u0, tspan)
    sol = solve(prob,Tsit5())
    # Create the differential data
    DX = similar(sol[:,:])
    for (i, xi) in enumerate(eachcol(sol[:,:]))
        DX[:,i] = pendulum(xi, [], 0.0)
    end
    # Create a basis
    @variables u[1:2]

    polys = [u[1]^0]
    for i ∈ 1:3
        for j ∈ 1:3
            push!(polys, u[1]^i*u[2]^j)
        end
    end

    h = [1u[1];1u[2]; cos(u[1]); sin(u[1]); u[1]*u[2]; u[1]*sin(u[2]); u[2]*cos(u[2]); polys...]

    basis = Basis(h, u, parameters = [])
    Ψ = SInDy(sol[:,:], DX, basis, ϵ = 1e-2)
    @test size(Ψ)[1] == 2

    # Simulate
    estimator = ODEProblem(dynamics(Ψ), u0, tspan, [])
    sol_ = solve(estimator,Tsit5())
    @test sol[:,:] ≈ sol_[:,:]
end


struct HailstoneSeq{T<:Integer}
    count::T
end

Base.eltype(::HailstoneSeq{T}) where T = T

function Base.iterate(h::HailstoneSeq, state=h.count)
    if state == 1
        (1, 0)
    elseif state < 1
        nothing
    elseif iseven(state)
        (state, state ÷ 2)
    elseif isodd(state)
        (state, 3state + 1)
    end
end

function Base.length(h::HailstoneSeq)
    len = 0
    for _ in h
        len += 1
    end
    return len
end

function Base.show(io::IO, h::HailstoneSeq)
    f5 = collect(Iterators.take(h, 5))
    print(io, "HailstoneSeq{", join(f5, ", "), "...}")
end

hs = HailstoneSeq(27)
println("Collection of the Hailstone sequence from 27: $hs")
cl = collect(hs)
println("First 5 elements: ", join(cl[1:5], ", "))
println("Last 5 elements: ", join(cl[end-4:end], ", "))

Base.isless(h::HailstoneSeq, s::HailstoneSeq) = length(h) < length(s)
println("The number with the longest sequence under 100,000 is: ", maximum(HailstoneSeq.(1:100_000)))


using PowerLaws
using Base.Test

# tests are based moby dick word distribution
# and other data sets from http://tuvalu.santafe.edu/~aaronc/powerlaws/data.htm
# results are compared to result from R poweRlaw package
# test are done with tolerance to 1e-8
tolerance = 1e-8

moby_data = vec(readdlm(string(Pkg.dir("PowerLaws"),"/data/moby_dick.txt"),' ',Int))
cities = vec(readdlm(string(Pkg.dir("PowerLaws"),"/data/cities.txt"),' ',Int))
electrical_blackout = vec(readdlm(string(Pkg.dir("PowerLaws"),"/data/electrical_blackouts_US.txt"),' ',Int))
population = vec(readdlm(string(Pkg.dir("PowerLaws"),"/data/population.txt"),' ','\n'))

println("test: discrete powerlaw")
est = estimate_xmin(moby_data, dis_powerlaw)
@test_approx_eq_eps est[1].α 1.9527275186009831 tolerance
@test est[1].θ == 7.0
@test_approx_eq_eps est[2] 0.008252950457938835 tolerance

est = estimate_xmin(moby_data, dis_powerlaw, xmins = [2,3,4,10,20])
@test_approx_eq_eps est[1].α 1.9550379794745847 tolerance
@test est[1].θ == 10.0
@test_approx_eq_eps est[2] 0.011867106465087929 tolerance

est1 = estimate_xmin(cities, dis_powerlaw)
@test_approx_eq_eps est1[1].α 1.614392642791718 tolerance
@test est1[1].θ == 1021.0
@test_approx_eq_eps est1[2] 0.06088582981503132 tolerance

println("test: discrete powerlaw - bootstrap")
est1 = estimate_xmin(electrical_blackout, dis_powerlaw)
@test_approx_eq_eps est1[1].α 1.2201523540878356 tolerance
@test est1[1].θ == 1000.0
@test_approx_eq_eps est1[2] 0.36278306128485405 tolerance

bootstr = bootstrap(moby_data,dis_powerlaw,no_of_sims = 15)
@test length(bootstr) == 15

bootstr = bootstrap(moby_data,est[1],no_of_sims = 15)
@test length(bootstr) == 15

bootstr = bootstrap(moby_data,dis_powerlaw,3,no_of_sims = 15)
@test length(bootstr) == 15

bootstr = bootstrap(moby_data,est[1],2,no_of_sims = 15)
@test length(bootstr) == 15

println("test: continuous powerlaw")
est = estimate_xmin(moby_data, con_powerlaw)
@test_approx_eq_eps est[1].α 1.9335240463440206 tolerance
@test est[1].θ == 26.0
@test_approx_eq_eps est[2] 0.03204776085711486 tolerance

est = estimate_xmin(moby_data, con_powerlaw, xmins = [2,3,4,10,20])
@test_approx_eq_eps est[1].α 1.9514282451732778 tolerance
@test est[1].θ == 20.0
@test_approx_eq_eps est[2] 0.04416094210009813 tolerance

est = estimate_xmin(collect(1:100),con_powerlaw)
@test_approx_eq_eps est[1].α 9.178824791215408 tolerance
@test est[1].θ == 79.0
@test_approx_eq_eps est[2] 0.1824330509645657 tolerance

est1 = estimate_xmin(population,con_powerlaw)
@test_approx_eq_eps est1[1].α 2.0337071519535046 tolerance
@test est1[1].θ == 96479.0
@test_approx_eq_eps est1[2] 0.0 tolerance

println("test: continuous powerlaw - bootstrap")
bootstr = bootstrap(moby_data,con_powerlaw,no_of_sims = 15)
@test length(bootstr) == 15

bootstr = bootstrap(moby_data,est[1],no_of_sims = 15)
@test length(bootstr) == 15

bootstr = bootstrap(moby_data,con_powerlaw,3,no_of_sims = 15)
@test length(bootstr) == 15

bootstr = bootstrap(moby_data,est[1],2,no_of_sims = 15)
@test length(bootstr) == 15

println("test: compare distribution")
data = collect(1:100)
d1 = fit(dis_powerlaw,data)
d2 = fit(Poisson,data)
ll_hoods_r = logpdf(d1,data) - logpdf(d2,data)
cmpd = compare_distributions(d1,d2,data)
@test typeof(cmpd) == compare_distributions
@test cmpd.data == data
@test cmpd.log_likehoods_ratio == ll_hoods_r
@test cmpd.xmin == 1
@test cmpd.sig_level == 0.05
@test_approx_eq_eps cmpd.V_test_stat 5.7463540130003254 tolerance
@test_approx_eq_eps cmpd.V_p_val 0.9999999954405856 tolerance
@test cmpd.V_preff_distr == 1
@test cmpd.C_b == 62
@test_approx_eq_eps cmpd.C_p_val 0.020978735677851468 tolerance
@test cmpd.C_preff_distr == 1

cmpd = compare_distributions(d1,Poisson,data)
@test typeof(cmpd) == compare_distributions
@test cmpd.data == data
@test cmpd.log_likehoods_ratio == ll_hoods_r
@test cmpd.xmin == 1
@test cmpd.sig_level == 0.05
@test_approx_eq_eps cmpd.V_test_stat 5.7463540130003254 tolerance
@test_approx_eq_eps cmpd.V_p_val 0.9999999954405856 tolerance
@test cmpd.V_preff_distr == 1
@test cmpd.C_b == 62
@test_approx_eq_eps cmpd.C_p_val 0.020978735677851468 tolerance
@test cmpd.C_preff_distr == 1

cmpd = compare_distributions(dis_powerlaw,Poisson,data)
@test typeof(cmpd) == compare_distributions
@test cmpd.data == data
@test cmpd.log_likehoods_ratio == ll_hoods_r
@test cmpd.xmin == 1
@test cmpd.sig_level == 0.05
@test_approx_eq_eps cmpd.V_test_stat 5.7463540130003254 tolerance
@test_approx_eq_eps cmpd.V_p_val 0.9999999954405856 tolerance
@test cmpd.V_preff_distr == 1
@test cmpd.C_b == 62
@test_approx_eq_eps cmpd.C_p_val 0.020978735677851468 tolerance
@test cmpd.C_preff_distr == 1

cmpd = compare_distributions(dis_powerlaw,con_powerlaw,data)
@test cmpd == Union{}

cmpd = compare_distributions(d1,con_powerlaw,data)
@test cmpd == Union{}

try
    cmpd = compare_distributions(d1,con_powerlaw(),data)
catch e
    @test true == isa(e,MethodError)
end

moby_data = sort(moby_data)
d1 = estimate_xmin(moby_data,con_powerlaw)[1]
d2 = fit(Exponential,moby_data[18072:end])
cmpd = compare_distributions(d1,d2,moby_data,26)
@test cmpd.xmin == 26
@test cmpd.sig_level == 0.05
@test_approx_eq_eps cmpd.V_test_stat 8.710089666437788 tolerance
@test_approx_eq_eps cmpd.V_p_val 1.0 tolerance
@test cmpd.V_preff_distr == 1
@test cmpd.C_b == 566
@test_approx_eq_eps cmpd.C_p_val 0.0 tolerance
@test cmpd.C_preff_distr == 1

d1 = estimate_xmin(moby_data,dis_powerlaw)[1]
d2 = fit(Poisson,moby_data[15898:end])
cmpd = compare_distributions(d1,d2,moby_data,7.0)
@test cmpd.xmin == 7.0
@test cmpd.sig_level == 0.05
@test_approx_eq_eps cmpd.V_test_stat 4.448486373104684 tolerance
@test_approx_eq_eps cmpd.V_p_val 0.9999956761232807 tolerance
@test cmpd.V_preff_distr == 1
@test cmpd.C_b == 2757
@test_approx_eq_eps cmpd.C_p_val 0.0 tolerance
@test cmpd.C_preff_distr == 1


ei(::Type{T}, i::Integer, n::Integer) where T = (v = zeros(T, n); v[i] = one(T); v)
ei(i::Integer, n::Integer) = ei(Float64, i, n)

function pivot!(M::Array{T,2}, i::Int, j::Int) where T
    m,n = size(M)
    @assert M[i, j] != 0 "pivot  illegal"
    M[i, :] = M[i, :] / M[i, j]
    for k in setdiff(1:m, i)
        M[k, :] -= M[k, j] * M[i, :]
    end
    return M
end


include("FoggyMountainObj.jl")

function main()
# One BFGS run with poor starting values
theta = [8.0, -8.0]
thetahat, obj_value, converged = fminunc(FoggyMountainObj, theta)
println()
println("The result with poor start values")
println("objective function value: ", round(obj_value,digits=5))
println("local minimizer: ", thetahat)
println()

# Now try simulated annealing
lb = [-20.0,-20.0]
ub = -lb
xopt = samin(FoggyMountainObj, theta, lb, ub, verbosity=1)

# Now try BFGS with multiple start values
bestobj = 1000
thetahat = zeros(2)
for i = 1:50
    theta = (ub-lb).*rand(2) + lb # use same bounds as SA
    theta, obj_value, converged = fminunc(FoggyMountainObj, theta)
    if obj_value < bestobj
        thetahat = theta
        bestobj = obj_value
    end
end
println("the estimate using BFGS with multiple start values: ", thetahat)
println("the best objective function value: ", bestobj)
end
main()


using Game2048
using BenchmarkTools
using StatsBase

board = initboard()
move!(board, left)

@time res = simulate_games(10_000);
res
@time mean(res)
@time extrema(res)

@benchmark simulate_game()


@testset " Generate data for Lorenz-63 model " begin

    import DifferentialEquations: ODEProblem, solve

    σ = 10.0
    ρ = 28.0
    β = 8.0 / 3

    dt_integration = 0.01
    dt_states = 1
    dt_obs = 8
    params = [σ, ρ, β]
    var_obs = [1]
    nb_loop_train = 10^2
    nb_loop_test = 10
    sigma2_catalog = 0.0
    sigma2_obs = 2.0

    ssm = StateSpaceModel(
        lorenz63,
        dt_integration,
        dt_states,
        dt_obs,
        params,
        var_obs,
        nb_loop_train,
        nb_loop_test,
        sigma2_catalog,
        sigma2_obs,
    )

    # compute u0 to be in the attractor space
    u0 = [8.0; 0.0; 30.0]
    tspan = (0.0, 5.0)
    prob = ODEProblem(ssm.model, u0, tspan, params)
    u0 = last(solve(prob, reltol = 1e-6, save_everystep = false))

    xt, yo, catalog = generate_data(ssm, u0)

    @show last(xt.u)

    @test true

end


# ML parameter estimation with filter result log-likelihood (via Nelder-Mead)

function fit{T}(y::Matrix{T}, build::Function, theta0::Vector{T};
          u::Array{T}=zeros(size(y,1), build(theta0).nu))
    objective(theta) = kalman_filter(y, build(theta), u=u).loglik
    kfit = Optim.optimize(objective, theta0)
    return (kfit.minimum, build(kfit.minimum), filter(y, build(kfit.minimum)))
end #fit

# Expectation-Maximization (EM) parameter estimation

function fit{T}(y::Array{T}, pmodel::ParametrizedSSM, params::SSMParameters;
          u::Array{T}=zeros(size(y,1), pmodel.nu), eps::Float64=1e-6, niter::Int=typemax(Int))

    n = size(y, 1)
    @assert n == size(u, 1)

    y_orig = copy(y)
    y = y'
    y_notnan = !isnan(y)
    y = y .* y_notnan

    u_orig = copy(u)
    u = u'

    I_nx = speye(pmodel.nx)
    I0_nx = zeros(I_nx)

    I_ny = speye(pmodel.ny)
    I0_ny = zeros(I_ny)

    I_nu = speye(pmodel.nu)

    # Requirement: zeros-rows in G and H remain consistent
    x_deterministic       = all(pmodel.G(1) .== 0, 2)
    all_x_deterministic   = all(x_deterministic)

    y_deterministic       = all(pmodel.H(1) .== 0, 2)
    all_y_deterministic   = all(y_deterministic)

    x1_deterministic      = all(pmodel.J .== 0, 2) |> full |> vec
    all_x1_deterministic  = all(x1_deterministic)
    Id_x1                 = spdiagm(x1_deterministic)

    if any(x_deterministic)
        OQ0, OQp = I_nx[find(x_deterministic), :], I_nx[find(!x_deterministic), :]
        Id(M_t::Array{Int,2}) = spdiagm(OQ0' * all((OQ0 * M_t * OQp') .== 0, 2)[:])
    else
        Id(_::Array{Int,2}) = I0_nx
    end

    phi(t)  = (pmodel.G(t)' * pmodel.G(t)) \ pmodel.G(t)'
    xi(t)   = (pmodel.H(t)' * pmodel.H(t)) \ pmodel.H(t)'
    Pi      = (pmodel.J' * pmodel.J) \ pmodel.J'

    na, nb, nq, nc, nd, nr, nx1, ns =
        map(length, Vector[params.A, params.B, params.Q,
                        params.C, params.D, params.R, params.x1, params.S])

    estimate_A, estimate_B, estimate_Q, estimate_C, estimate_D, estimate_R,
        estimate_x1, estimate_S = map(x->x>0, [na, nb, nq, nc, nd, nr, nx1, ns])

    function em_kernel!{T}(params::SSMParameters{T})

        m = pmodel(params)

        #=
        print("Expectations (smoothing)... ")
        tic()
        =#

        if estimate_A | estimate_Q
            exs, P, Plag1, loglik = lag1_smooth(y_orig, u_orig, m)
        else
            smoothed = kalman_smooth(y_orig, m, u=u_orig)
            exs, P, loglik = smoothed.smoothed', smoothed.error_cov, smoothed.loglik
        end

        #=
        toc()
        println("Negative log-likelihood: ", loglik)

        print("Maximizations... ")
        tic()
        =#

        if estimate_A || estimate_B || estimate_Q || estimate_x1
            Qinv    = inv(pmodel.Q(params.Q))
            Vinv_   = all_x_deterministic ? I0_nx : phi(1)' * Qinv * phi(1)
        end #if ABQ

        if estimate_B || estimate_C || estimate_D || estimate_R || estimate_x1
            Rinv  = inv(pmodel.R(params.R))
            Winv_ = all_y_deterministic ? I0_ny : xi(1)' * Rinv * xi(1)
        end #if BCDRx1

        if estimate_x1
            Sinv  = inv(pmodel.S(params.S))
            P1inv_   = all_x1_deterministic ? I0_nx : Pi' * Sinv * Pi
        end #if x1

        HRH(t) = pmodel.H(t) * pmodel.R(params.R) * pmodel.H(t)'
        HRH_nonzero_rows(t) = diag(HRH(t)) .!= 0

        function N(t::Int)
            HRHt = HRH(t)
            O = I_ny[find(y_notnan[:, t] & HRH_nonzero_rows(t)), :]
            return I - HRHt * O' * inv(O * HRHt * O') * O
        end #N

        ex  = exs[:, 1]
        exx = I0_nx

        At  = m.A(1)
        ut  = u[:, 1]

        if estimate_B || estimate_x1

            ex1_f = (I_nx - Id_x1) * ex + Id_x1 * pmodel.x1.f
            ex1_D = Id_x1 * pmodel.x1.D
            ex1 = ex1_f + ex1_D * params.x1

            M   = 1 * (m.A(1) .!= 0) #TODO: Sparse? Assumption: constant position of nonzero values in A
            M_t = copy(M)
            Idt     = Idt1      = I_nx
            At1_    = At2_      = I_nx
            f_Bu_t1 = f_Bu_t2   = zeros(m.nx) # Sparse initialization?
            D_Bu_t1 = D_Bu_t2   = zeros(m.nx, nb)
            ku_f_Bt = ku_f_Bt1  = zeros(m.nx)
            ku_D_Bt = ku_D_Bt1  = zeros(m.nx, nb)

        end # if Bx1

        estimate_A && ((A_S1, A_S2) = (zeros(na, na), zeros(na)))

        if estimate_B # B setup

            Δt1, Δt2 = zeros(m.nx, nb), zeros(m.nx)
            Δt3, Δt4 = zeros(m.ny, nb), zeros(m.ny)

            B_S1, B_S2 = zeros(nb, nb), zeros(nb)

        end #if B

        estimate_Q && ((Q_S1, Q_S2) = (zeros(nq, nq), zeros(nq)))
        estimate_C && ((C_S1, C_S2) = (zeros(nc, nc), zeros(nc)))
        estimate_D && ((D_S1, D_S2) = (zeros(nd, nd), zeros(nd)))
        estimate_R && ((R_S1, R_S2) = (zeros(nr, nr), zeros(nr)))

        if estimate_x1 #x1 setup

            Δ5t, Δ6t = zeros(m.nx, nx1), zeros(m.nx)
            Δ7t, Δ8t = zeros(m.ny, nx1), zeros(m.ny)

            x1_S1 = pmodel.x1.D' * P1inv_ * pmodel.x1.D
            x1_S2 = pmodel.x1.D' * P1inv_ * (ex - pmodel.x1.f)

        end # if x1

        for t in 1:n

            (t > 1) && (estimate_A || estimate_Q || estimate_x1) && (But_prev = m.B(t-1) * ut)

            At_prev = At #TODO: Use this in the appropriate places?
            At      = m.A(t)

            ut      = u[:, t]
            Pt      = P[t]

            ex_prev = ex
            ex      = exs[:, t]

            exx_prev  = exx
            exx       = Pt + ex * ex'

            !all_x_deterministic && (estimate_A || estimate_B || estimate_Q || estimate_x1) && (Vinv_ = phi(t)' * Qinv * phi(t))

            if (estimate_A || estimate_Q) && t > 1

                exx1 = Plag1[t] + ex * ex_prev'

                if estimate_A
                    pkp = kron(pmodel.A3(t)', pmodel.A1(t))
                    D_At = pkp * pmodel.A2.D
                    f_At = pkp * pmodel.A2.f
                    A_kp = kron(exx_prev, Vinv_)
                    A_S1 += D_At' * A_kp * D_At
                    A_S2 += D_At' * (vec(Vinv_ * exx1) -
                                  A_kp * f_At - vec(Vinv_ * But_prev * ex_prev'))
                end #if A

                if estimate_Q
                    Q_S1 += pmodel.Q.D' * pmodel.Q.D
                    Q_S2 += pmodel.Q.D' * vec(phi(t) * (exx - exx1 * At' -
                                At * exx1' - ex * But_prev' - But_prev * ex' + At * exx_prev * At' +
                                At * ex_prev * But_prev' + But_prev * ex_prev' * At' + But_prev * But_prev') * phi(t)')
                end #if Q

            end #if AQ

            if estimate_B || estimate_C || estimate_D || estimate_R || estimate_x1

                yt = y[:, t]
                Ct = m.C(t)
                Nt = N(t)
                Dut = m.D(t) * ut
                ey = yt - Nt * (yt - Ct * ex - Dut)
                !all_y_deterministic && (Winv_ = xi(t)' * Rinv * xi(t))

                if estimate_C || estimate_R

                    eyx = Nt * Ct * Pt + ey * ex'

                    if estimate_C
                        pkp = kron(pmodel.C3(t)', pmodel.C1(t))
                        f_Ct = pkp * pmodel.C2.f
                        D_Ct = pkp * pmodel.C2.D
                        C_kp = kron(exx, Winv_)
                        C_S1 += D_Ct' * C_kp * D_Ct
                        C_S2 += D_Ct' * (vec(Winv_ * eyx) - C_kp * f_Ct - vec(Winv_ * Dut * ex'))
                    end #if C

                    if estimate_R
                        I2 = diagm(!y_notnan[:, t])
                        eyy = I2 * (Nt * HRH(t) + Nt * Ct * Pt * Ct' * Nt') * I2 + ey * ey'
                        R_S1 += pmodel.R.D' * pmodel.R.D
                        R_S2 += pmodel.R.D' * vec(xi(t) * (eyy - eyx * Ct' -
                                    Ct * eyx' - ey * Dut' - Dut * ey' + Ct * exx * Ct' +
                                    Ct * ex * Dut' + Dut * ex' * Ct' + Dut * Dut') * xi(t)')
                    end #if R

                end #if CR

                if estimate_B || estimate_x1

                    if estimate_B

                        if t > 1
                            Δt1 = At_prev * Idt1 * D_Bu_t2 + ku_D_Bt1
                            Δt2 = ex - At_prev * (I - Idt1) * ex_prev - At_prev * Idt1 * (At2_ * ex1 + f_Bu_t2) - ku_f_Bt1
                        end

                        Δt3 = Ct * Idt * D_Bu_t1
                        Δt4 = ey - Ct * (I - Idt) * ex - Ct * Idt * (At1_ * ex1 + f_Bu_t1) - Dut

                        B_S1 += Δt1' * Vinv_ * Δt1 + Δt3' * Winv_ * Δt3
                        B_S2 += Δt1' * Vinv_ * Δt2 + Δt3' * Winv_ * Δt4

                    end #if B

                    if estimate_x1

                        if t > 1
                            Δ5t = At_prev * Idt1 * At2_ * ex1_D
                            Δ6t = ex - At_prev * (I - Idt1) * ex_prev - At_prev * Idt1 * (At2_ * ex1_f + f_Bu_t2 + D_Bu_t2 * params.B) - But_prev
                        end

                        Δ7t = Ct * Idt * At1_ * ex1_D
                        Δ8t = ey - Ct * (I - Idt) * ex - Ct * Idt * (At1_ * ex1_f + f_Bu_t1 + D_Bu_t1 * params.B) - Dut

                        x1_S1 += Δ5t' * Vinv_ * Δ5t + Δ7t' * Winv_ * Δ7t
                        x1_S2 += Δ5t' * Vinv_ * Δ6t + Δ7t' * Winv_ * Δ8t

                    end #x1

                    Idt = Id(M_t)
                    Idt1 = Idt
                    t <= pmodel.nx && (M_t *= M)

                    At2_ = At1_
                    At1_ = At * At1_

                    kuB = kron(ut', pmodel.B1(t))

                    ku_f_Bt   = kuB * pmodel.B2.f
                    ku_f_Bt1  = ku_f_Bt

                    f_Bu_t2   = f_Bu_t1
                    f_Bu_t1   = ku_f_Bt + At * f_Bu_t1

                    ku_D_Bt   = kuB * pmodel.B2.D
                    ku_D_Bt1  = ku_D_Bt

                    D_Bu_t2   = D_Bu_t1
                    D_Bu_t1   = ku_D_Bt + At * D_Bu_t1

                end #Bx1

                if estimate_D
                    pkp = kron(I_nu, pmodel.D1(t))
                    f_Dt = pkp * pmodel.D2.f
                    D_Dt = pkp * pmodel.D2.D
                    D_kp = kron(ut', I_ny)
                    D_S1 += D_Dt' * D_kp' * Winv_ * D_kp * D_Dt
                    D_S2 += D_Dt' * D_kp' * Winv_ * (ey - Ct * ex - D_kp * f_Dt)
                end #if D

            end #if BCDRx1

        end #for

        params.A[:]   = estimate_A ? A_S1 \ A_S2 : T[]
        params.B[:]   = estimate_B ? B_S1 \ B_S2 : T[]
        params.Q[:]   = estimate_Q ? Q_S1 \ Q_S2 : T[]

        params.C[:]   = estimate_C ? C_S1 \ C_S2 : T[]
        params.D[:]   = estimate_D ? D_S1 \ D_S2 : T[]
        params.R[:]   = estimate_R ? R_S1 \ R_S2 : T[]

        params.x1[:]  = estimate_x1 ? x1_S1 \ x1_S2 : T[]
        params.S[:]   = estimate_S ? (pmodel.S.D' * pmodel.S.D) \ pmodel.S.D' *
                        vec(Pi * (P[1] + exs[:, 1] * exs[:, 1]' -
                        exs[:, 1]*m.x1' - m.x1*exs[:, 1]' + m.x1*m.x1') * Pi') : T[]

        #toc()

        return loglik

    end #em_kernel

    fraction_change(ll_prev, ll_curr) = isinf(ll_prev) ?
        1 : 2 * (ll_prev - ll_curr) / (ll_prev + ll_curr + 1e-6)

    ll, ll_prev = Inf, Inf

    while (niter > 0) && (fraction_change(ll_prev, ll) > eps)
        ll_prev = ll
        ll      = em_kernel!(params)
        niter -= 1
    end #while

    (niter == 0) && warn("Parameter estimation timed out - results may have failed to converge")

    return params, kalman_smooth(y_orig, pmodel(params), u=u_orig)

end #fit

function fit{T}(y::Array{T}, model::StateSpaceModel{T}; u::Array{T}=zeros(size(y,1), model.nu),
              eps::Float64=1e-6, niter::Int=typemax(Int))

    # B, Z, x1 default to parametrized as fully unconstrained
    A, A_params = parametrize_full(model.A(1))
    C, C_params = parametrize_full(model.C(1))
    x1, x1_params = parametrize_full(model.x1)

    # U, A default to fixed
    B, B_params = parametrize_none(model.B(1))
    D, D_params = parametrize_none(model.D(1))

    # Q, R, P1 default to parametrized as diagonal with independent elements - any other values
    #   are ignored / set to zero
    Q, Q_params = parametrize_diag(diag(model.V(1)))
    R, R_params = parametrize_diag(diag(model.W(1)))
    P1, P1_params = parametrize_diag(diag(model.P1))

    pmodel = ParametrizedSSM(A, Q, C, R, x1, P1, B=B, D=D)
    params = SSMParameters(A=A_params, B=B_params, Q=Q_params,
                            C=C_params, D=D_params, R=R_params,
                            x1=x1_params, P1_params)
    fit(y, pmodel, params, u=u, eps=eps, niter=niter)

end #fit


using SpectralDistances, DynamicAxisWarping, DSP, Distances

nfft = 32
T = 200
q = sin.(1:T)
y = [zeros(10nfft);q;zeros(10nfft)] .+ 0.0001 .* randn.()

Q = spectrogram(q,nfft)
Y = spectrogram(y,nfft)

n, m = size(Q.power)
rad = 5

## Discrete grid transport
res = dtwnn(
    log1p.(Q.power),
    log1p.(Y.power),
    DiscreteGridTransportDistance(Cityblock(), eltype(Q.power), n, n),
    rad,
    saveall = true,
)

@test res.cost < 0.01
@test res.loc == 21
@test all(isfinite, res.dists)
isinteractive() && plot(res.dists)

fm  = TimeWindow(inner = LS(na = 2, λ=1e-3), n = nfft, noverlap = nfft÷2)

Qm  = fm(q) |> change_precision(Float64)
Ym  = fm(y) |> change_precision(Float64);

res = dtwnn(
    Qm,
    Ym,
    EuclideanRootDistance(p = 1, weight = simplex_residueweight),
    rad,
    saveall = true,
)
@test res.cost < 0.01
@test  20 <= res.loc <= 22
@test all(isfinite, res.dists)
isinteractive() && plot(res.dists)
##
res_dtwotrd = dtwnn(
    Qm,
    Ym,
    OptimalTransportRootDistance(p = 1, weight = simplex_residueweight),
    rad,
    saveall = true,
    tol = 1e-3,
)
@test res.cost < 0.01
@test  20 <= res.loc <= 22
@test all(isfinite, res.dists)
isinteractive() && plot(res.dists)

dist = TimeDistance(
    inner = OptimalTransportRootDistance(p = 1, β = 0.5, weight = simplex_residueweight),
    tp    = 1,
    c     = 0.1,
)
res = distance_profile(dist, Qm, Ym, tol=1e-3, check_interval=10)
@test minimum(res) < 1
@test  20 <= argmin(res) <= 22
@test all(isfinite, res)
isinteractive() && plot(res)


function correctorfluxes!(Fbarforward::Vector{Float64},Fbarbackward::Vector{Float64},
    Qforward::Vector{Float64},Aforward::Vector{Float64},Qbackward::Vector{Float64},
    Abackward::Vector{Float64},beta::Float64,rho::Float64,
    acolscor::Vector{Int8},qcolscor::Vector{Int8})
    Fbarforward[acolscor] .= Qforward;
    Fbarforward[qcolscor] .= Qforward.^2./Aforward .+ (1./3).*beta./rho.*
        Aforward.^1.5;
        # system.branches.Fbarforward[i][system.solverparams.acolscor] .= (
        #     system.branches.Qforward[i]);
        # system.branches.Fbarforward[i][system.solverparams.qcolscor] .= (
        #     system.branches.Qforward[i].^2./system.branches.Aforward[i] .+
        #     (1./3).*system.branches.beta[i][end]./system.solverparams.rho.*
        #     system.branches.Aforward[i].^1.5);
    Fbarbackward[acolscor] .= Qbackward;
    Fbarbackward[qcolscor] .= Qbackward.^2./Abackward .+ (1./3).*beta./rho.*
        Abackward.^1.5;
        # system.branches.Fbarbackward[i][system.solverparams.acolscor] .= (
        #     system.branches.Qbackward[i]);
        # system.branches.Fbarbackward[i][system.solverparams.qcolscor] .= (
        #     system.branches.Qbackward[i].^2./system.branches.Abackward[i] .+
        #     (1./3).*system.branches.beta[i][end]./system.solverparams.rho.*
        #     system.branches.Abackward[i].^1.5);
end


using DataFrames
using DecisionTree
using JLD

# Read the iris data set.
df = readtable(ARGS[1], header = false)

# Get the features and labels.
features = convert(Array, df[:, 1:4])
labels = convert(Array, df[:, 5])

# Train decision tree classifier.
model = DecisionTreeClassifier(pruning_purity_threshold=0.9, maxdepth=6)
DecisionTree.fit!(model, features, labels)

# Save the model.
save(ARGS[2], "model", model)


#!/usr/bin/env julia

using RobotOS
@rosimport barc.msg: ECU,  Z_KinBkMdl
@rosimport data_service.msg: TimeData
@rosimport geometry_msgs.msg: Vector3,  PointStamped
@rosimport geometry_msgs.msg: Vector3
@rosimport ackermann_msgs.msg: AckermannDrive, AckermannDriveStamped
@rosimport nav_msgs.msg: Path, Odometry
@rosimport duckietown_msgs.msg: Twist2DStamped, LanePose

rostypegen()
using barc.msg
using data_service.msg
using geometry_msgs.msg
using ackermann_msgs.msg
using nav_msgs.msg
using JuMP
using Ipopt
using Distances
using duckietown_msgs.msg

# define model parameters
L_a     = 0.1         # distance from CoG to front axel
L_b     = 0.1         # distance from CoG to rear axel
dt      = 0.1           # time step of system
la      =0.08           # center to front wheel of trycicle model
T       = 150  # MPC horizon
dt=0.1 # Smapling time
global Getpos  = false
zmax=[10;10;100*pi]
umax=[0.6;3*dt]
n=3
m=2
# predictive horizon

l       = 0.1   #length of the car
X=[0.0;0.0;0.0]
path = Path()

function solveMPC(l,n,m,T,z0,zT,zmax,umax,dt,step)
    #T : horizon
    #z0 : intial position for every iteration
    #zT : reference   +++++++++++++++
    #n :dim of input
    #m : dim of state
    #
    mpc = Model(solver=IpoptSolver(print_level=0))
    @defVar(mpc, -zmax[i] <= z[t=1:T+1,i=1:n] <= zmax[i])
    @defVar(mpc, -umax[i] <= u[t=1:T+1,i=1:m] <= umax[i])

    # Cost
    #@setObjective(mpc, Min,
    #    sum{100*(z[1,t]+z[2,t]+z[3,t]+z[4,t])^2 + sum{u[j,t]^2,j=1:m},t=0:T})
    #@setObjective(mpc, Min,
    #sum{(z[t,1]-zT[t,1])^2+(z[t,2]-zT[t,2])^2+10e-5*(z[t,3]-zT[t,3])^2  ,t=2:T+1})
@defNLExpr(mpc, bta[i = 1:T], atan( L_a / (L_a + L_b) * tan(u[i,1]) ) )
      @setObjective(mpc, Min,
    sum{(z[T,1]-zT[1])^2+(z[T,2]-zT[2])^2+(z[T,3]-zT[3])^2,t=1:T})
    # Link state and control across the horizon
    for t = 1:T
        @addNLConstraint(mpc, z[t+1,1] == z[t,1] + dt*u[t,2]*cos(z[t,3]))
        @addNLConstraint(mpc, z[t+1,2] == z[t,2] + dt*u[t,2]*sin(z[t,3]))
        @addNLConstraint(mpc, z[t+1,3] == z[t,3] + dt*u[t,2]/l*sin(bta[t]))
        #@addNLConstraint(mpc, z[t+1,4] == z[t,4] + dt*u[t,2])

    end

    # Initial conditions
    @addConstraint(mpc, z[1,:] .== z0)
    # Final state
    #@addConstraint(mpc, z[:,T] .== zT)
    # Solve the NLP
    solve(mpc)
    # Return the control plan
    step = step +1
    return getValue(u), getValue(z), step
end

#------------------- sub state estimation-------------------
function SEcb(msg::Odometry)
    # update mpc initial condition
    global Getpos = true
    X[1] =    msg.pose.pose.position.x
    X[2] =    msg.pose.pose.position.y
    X[3] =    msg.pose.pose.orientation.w
    #X[4] =    msg.twist.twist.linear.x

end

#--------------  Find the nearest point on the reference of the car ----------------
function find_nearest(x,f,start)
    #print(x[1:2])
    #print(f[1:2,1])
    dis = Array(Float64,size(f,1))
    ref = Array(Float64,T)
    #print(size(dis))
    for i in 1:size(f,1)
        dis[i] = euclidean(x[1:2],f[i,1:2])
    end

    refstep=4
    ref_start = indmin(dis)
    if ref_start <= start
       ref_start = start+1
     end
    print(ref_start)

    if (ref_start > size(f,1) - T*refstep) && (ref_start < size(f,1))
        #print(size(f,1)-ref_start)
	r=(size(f,1)-ref_start)%refstep
	if r != 0
                   ref = [ f[ref_start:refstep:size(f,1)-r,:] ; f[refstep-r:refstep:(T*refstep-(size(f,1)-ref_start)+r),:]   ]
               else
	    ref = [ f[ref_start:refstep:size(f,1),:] ; f[1:refstep:1+(T*refstep-(size(f,1)-ref_start)),:]   ]
                end
    elseif ref_start > size(f,1)
        ref_start = ref_start - size(f,1)
        ref = f[ref_start:refstep:ref_start+T*refstep,:]
    elseif ref_start == size(f,1)
	ref = f[1:refstep:T*refstep+1,:]
    else
        ref = f[ref_start:refstep:ref_start+T*refstep,:]
        #print(ref)
    end

    return ref,ref_start
end

#--------------------Main function-----------------------------
function main()
    zT = [0;1.7;1/2*pi]  # Initial state - COO
    Xb =[0;2;pi]
    step=0
    # initiate node, set up publisher / subscriber topic
    init_node("mpc_parking")
    pub_car_cmd = Publisher("mpc/car_cmd", Twist2DStamped, queue_size=1)
    pub_first_refp = Publisher("goal", PointStamped, queue_size=1)
    sub  = Subscriber{Odometry}("euler_odom",  SEcb, queue_size=1)
    c1=1
    loop_rate = Rate(10)

    while ! is_shutdown()
        #print(X)
        if Getpos == true
            # find the nearest point on the path# get reference in horizon
            if step == T
            car_control_msg = Twist2DStamped()
            car_control_msg.v = 0
            car_control_msg.omega = 0
            #print(v)
            #publish commands
            publish(pub_car_cmd, car_control_msg)
                return
            end
            #Xb[1] = X[1] - l/2*cos(pi/2) + l/2*sin(X[3])
            #Xb[2] = X[2] - l/2*sin(pi/2) + l/2*cos(X[3])
            #Xb[3] = X[3]
            ## run mpc, publish command
            u_vec, z_vec, step = solveMPC(l,n,m,T-step,X,zT,zmax,umax,dt, step)

            println("solve!")
            #print(u_vec[1,1] ,u_vec[1,2])

            # get optimal solutions
            v_opt   = u_vec[1,2]
            d_f_opt = u_vec[1,1]

            #v = v_opt*sqrt(1+c1*c1*d_f_opt*d_f_opt)
            w = -v_opt/l*d_f_opt
            car_control_msg = Twist2DStamped()
            car_control_msg.v = v_opt
            car_control_msg.omega = w
            #print(v)
            #publish commands
            publish(pub_car_cmd, car_control_msg)
        else
            println("unknown position")
        end
        rossleep(loop_rate)
    end
    spin()
end

if ! isinteractive()
    main()
end


function Adjust(da_or_de, beta, alpha)
    return da_or_de^beta + alpha
end

function integrate(a0, e0, p)
    """
    General purpose integrator for Nyadzani & Razzaque eqns for a & e.

    Params:
        a0 - The initial semimajor axis, measured in solar radii
        e0 - The initial eccentricity, dimensionless
        p  - A vector of parameters:
                 p[1] = m1 (units: Solar Mass)
                 p[2] = m2 (units: Solar Mass)
                 p[3] = Beta for LT Adjustment (units: dimensionless)
                 p[4] = Alpha for LT adjustment (units: [unit]/s)
                 p[5] = Lifetime (evolution + rejuvenation)

    Returns:
        A  - An array of the semimajor axes of the binary system over time. (Solar Mass)
        E  - An array of the eccentricities of the binary system over time. (no dim.)
    """

    Solar_Mass   = 1.989e30  # kg
    Solar_Radius = 696340000.0 # m
               G = 6.67e-11  # m^3 kg^-1 s^-2
               c = 299792458.0 # m/s

               A = Float64[]
               E = Float64[]
               H = Float64[]

    m1, m2, expo, alph, evotime = p[1], p[2], p[3], p[4], p[5]

    # number of seconds in a year
    seconds_per_year = 60 * 60 * 24 * 365.25

    #######################
    ##     Unit Check     #
    #######################
    a = a0 * Solar_Radius # Meters
    e = e0                # Dimensionless
    m1 = m1 * Solar_Mass  # Kilogram
    m2 = m2 * Solar_Mass  # Kilogram
    #######################
    ##   End Unit Check   #
    #######################

    # Beta has units m^4 / s
    beta = ((64/5) * G^3 * m1 * m2 * (m1 + m2) / (c^5))

    A = push!(A, a)
    E = push!(E, e)
    H = push!(H, 0.0)

    total_time = 0

    # Integrate until past the end of the universe, or a 10km orbit
    while total_time/seconds_per_year + evotime < 1e11 && a > 1e4
        initial_da = (- beta / ( (a^3) * (1 - e^2)^(7/2) ))
        da = initial_da * (1 + (73/24) * e^2 + (37/96) * e^4)

        intial_de = (((-19/12) * beta) / (a^4*(1-e^2)^(5/2)))
        de = intial_de * (e + (121/304) * e^3) # Units: s^-1

        # da = Adjust(da, expo, alph)
        # de = Adjust(de, expo, alph)

        timeA = abs(1e-2 * a/da)
        if e > 1e-10
            timeE = abs(1e-2 * e/de)
        else
            de = 0
            e = 1e-10
            timeE = timeA * 10
        end

        dt2 = (evotime + total_time/seconds_per_year)*0.23076752*0.5*seconds_per_year

        dt = min(timeE, timeA, dt2)

        a = a + dt * da
        e = e + dt * de

        A = push!(A, a)
        E = push!(E, e)
        H = push!(H, dt)

        total_time = total_time + dt
    end

    stop_reason = "flag_not_set"

    if total_time/seconds_per_year + evotime >= 1e11
        stop_reason = "out_of_time"
    end

    if a <= 1e4
        stop_reason = "merged"
    end

    # Solar Radii, Dimensionless
    return A / Solar_Radius, E, H, stop_reason
end


x_dim = 28^2
z_dim = 2
hidden_dim = 128
batch_size = 100

train_dl = HDF5DataLayer(name="train-data",
        source=ENV["DATASETS"]"/mnist/train.txt",
        batch_size=batch_size)
test_dl = HDF5DataLayer(name="test-data",
        source=ENV["DATASETS"]"/mnist/test.txt",
        batch_size=batch_size)

enc_layers = [
    InnerProductLayer(name="enc1",
        output_dim=hidden_dim,
        neuron=Neurons.ReLU(),
        bottoms=[:data], tops=[:enc1]),
    SplitLayer(name="enc1_split",
        bottoms=[:enc1], tops=[:enc1a, :enc1b]),
    InnerProductLayer(name="z_mean",
        output_dim=z_dim,
        neuron=Neurons.Identity(),
        bottoms=[:enc1a], tops=[:z_mean]),
    InnerProductLayer(name="z_sd",
        output_dim=z_dim,
        bias_init=ConstantInitializer(-1.),
        weight_init=ConstantInitializer(0.),
        neuron=Neurons.Exponential(),
        bottoms=[:enc1b], tops=[:z_sd])]

sampling_layers = [
    SplitLayer(name="z_mean_split",
        bottoms=[:z_mean], tops=[:z_mean1, :z_mean2]),
    SplitLayer(name="z_sd_split",
        bottoms=[:z_sd], tops=[:z_sd1, :z_sd2]),

    RandomNormalLayer(output_dims=[z_dim],
        batch_sizes=[batch_size],
        tops=[:noise01]),
    ElementWiseLayer(operation=ElementWiseFunctors.Multiply(),
        bottoms=[:noise01, :z_sd1],
        tops=[:noise0sd]),
    ElementWiseLayer(name="z",
        operation=ElementWiseFunctors.Add(),
        bottoms=[:z_mean1, :noise0sd],
        tops=[:z]),

    GaussianKLLossLayer(name="encoder-loss",
        bottoms=[:z_mean2, :z_sd2])]

dec_layers = [
    InnerProductLayer(name="dec1", output_dim=hidden_dim,
        neuron=Neurons.Sigmoid(),
        bottoms=[:z], tops=[:dec1]),
    InnerProductLayer(name="x_mean", output_dim=x_dim,
        neuron=Neurons.Sigmoid(),
        bottoms=[:dec1], tops=[:x_mean])]

expected_recon_loss = BernoulliReconLossLayer(bottoms=[:x_mean, :data])

non_data_layers = [
    enc_layers...,
    sampling_layers...,
    dec_layers...,
    expected_recon_loss]


#####################
# AFFINE OPERATIONS #
#####################

+(x::RevisedAffineForm) = x
-(x::RevisedAffineForm) = RevisedAffineForm(-x.c, -x.ϵ, x.Δ)

for op in (:+, :-)
    @eval function $op(x::RevisedAffineForm{N}, y::RevisedAffineForm{N}) where {N}
        c = $op(Interval(x.c), Interval(y.c))
        ϵ = $op(Interval.(x.ϵ), Interval.(y.ϵ))
        Δ = x.Δ ⊕₊ y.Δ
        return RevisedAffineForm(c, ϵ, Δ)
    end

    @eval function $op(x::RevisedAffineForm, a::Real)
        c = $op(Interval(x.c), Interval(a))
        Δ = x.Δ ⊕₊ radius(c)
        return RevisedAffineForm(mid(c), x.ϵ, Δ)
    end
end

+(a::Real, x::RevisedAffineForm) = x + a
-(a::Real, x::RevisedAffineForm) = -x + a

#########################
# NON-AFFINE OPERATIONS #
#########################
for (op, op_up) in ((:*, :mul_round), (:/, :div_round))
    @eval function $op(x::RevisedAffineForm, a::Real)
        c = $op(Interval(x.c), Interval(a))
        ϵ = $op(Interval.(x.ϵ), Interval(a))
        Δ = $op_up(x.Δ, float(abs(a)), RoundUp)
        return RevisedAffineForm(c, ϵ, Δ)
    end
end

*(a::Real, x::RevisedAffineForm) = *(x, a)

"""
Standard multiplication, see eq. (2.2) of section 2.2 of
*Iwona Skalna, Parametric Interval Algebraic Systems, Springer, 2018*
"""
function *(x::RevisedAffineForm{N}, y::RevisedAffineForm{N}) where {N}
    c = Interval(x.c) * Interval(y.c) + 0.5 * dot(Interval.(x.ϵ), Interval.(y.ϵ))
    ϵ = Interval(x.c) * Interval.(y.ϵ) + Interval(y.c) * Interval.(x.ϵ)
    Δ =
        abs(x.c) ⊗₊ y.Δ ⊕₊ abs(y.c) ⊗₊ x.Δ ⊕₊
        (_sum_up(abs, x.ϵ) ⊕₊ x.Δ) ⊗₊ (_sum_up(abs, y.ϵ) ⊕₊ y.Δ) ⊖₊
        0.5 ⊗₋ _dot_down(abs, x.ϵ, y.ϵ)
    return RevisedAffineForm(c, ϵ, Δ)
end

###########
# HELPERS #
###########

_sum_up(a) = reduce(⊕₊, a)
_sum_up(f, a) = mapreduce(f, ⊕₊, a)
_dot_up(a, b) = reduce(⊕₊, aᵢ ⊗₊ bᵢ for (aᵢ, bᵢ) in zip(a, b))
_dot_up(f, a, b) = reduce(⊕₊, f(aᵢ) ⊗₊ f(bᵢ) for (aᵢ, bᵢ) in zip(a, b))

_sum_down(a) = reduce(⊕₋, a)
_sum_down(f, a) = mapreduce(f, ⊕₋, a)
_dot_down(a, b) = reduce(⊕₋, aᵢ ⊗₋ bᵢ for (aᵢ, bᵢ) in zip(a, b))
_dot_down(f, a, b) = reduce(⊕₋, f(aᵢ) ⊗₋ f(bᵢ) for (aᵢ, bᵢ) in zip(a, b))


## Makes the model associated with a particle filter using the "locally optimal
## proposal". Note that this is only optimal in the specific and limited sense
## that the weights are a function of xprev only, and hence the simulated value
## of x is not a source of variability.

mutable struct LGLOPParticle
  x::Float64
  xprev::Float64
  LGLOPParticle() = new()
end

function makeLGLOPModel(theta::LGTheta, ys::Vector{Float64})
  n::Int64 = length(ys)

  RC2v0::Float64 = theta.R + theta.C * theta.C * theta.v0
  v0RoverRC2v0::Float64 = theta.v0*theta.R/RC2v0
  sqrtv0RoverRC2v0::Float64 = sqrt(v0RoverRC2v0)

  mu1::Float64 = v0RoverRC2v0*(theta.x0/theta.v0 + theta.C*ys[1]/theta.R)
  tmp1::Float64 = theta.C * theta.x0 - ys[1]
  lG1::Float64 = -0.5 * log(2 * π * RC2v0) - tmp1 * 0.5/RC2v0 * tmp1

  RC2Q::Float64 = theta.R + theta.C * theta.C * theta.Q
  invRC2Qover2::Float64 = 0.5/RC2Q
  QRoverRC2Q::Float64 = theta.Q * theta.R / RC2Q
  sqrtQRoverRC2Q::Float64 = sqrt(QRoverRC2Q)

  invRover2::Float64 = 0.5/theta.R
  logncG::Float64 = -0.5 * log(2 * π * RC2Q)

  @inline function lG(p::Int64, particle::LGLOPParticle, ::Nothing)
    if p == 1
      return lG1
    else
      v::Float64 = theta.C * theta.A * particle.xprev - ys[p]
      return logncG - v * invRC2Qover2 * v
    end
  end
  @inline function M!(newParticle::LGLOPParticle, rng::RNG, p::Int64,
    particle::LGLOPParticle, ::Nothing)
    if p == 1
      newParticle.x = mu1 + sqrtv0RoverRC2v0*randn(rng)
    else
      mu::Float64 = QRoverRC2Q*(theta.A*particle.x/theta.Q + theta.C*ys[p]/theta.R)
      newParticle.x = mu + sqrtQRoverRC2Q*randn(rng)
      newParticle.xprev = particle.x
    end
  end
  model::SMCModel = SMCModel(M!, lG, n, LGLOPParticle, Nothing)
  return model
end


using Interpolations

@testset "Interpolation" begin

@testset "Testing in-place interpolation" begin
    # make Gaussian line
    x1 = range(5434.0, 5435.0, step=0.01)
    y1 = GRASS.gaussian_line.(x1, mid=5434.5, width=0.1, depth=1.0)

    # new grid to re-sample on
    x2 = range(5433.5, 5435.5, step=0.05)

    # test that the interpolation runs
    itp1 = extrapolate(interpolate!(eltype(x1), (x1,), y1, Gridded(Linear())), 1.0)
    @test typeof(itp1) <: Interpolations.FilledExtrapolation

    # test boundary-conditions
    y2 = itp1.(x2)
    @test y2[1] == 1.0
    @test y2[end] == 1.0
end

end


using LowRankIntegrators, SparseArrays

@testset "Burgers equation" begin
    n = 1000 # spatial discretization

    l = π # length of spatial domain
    Δx = l/n # step size
    x_range = Δx/2:Δx:l-Δx/2 # uniform grid

    # boundary conditions
    left(i) = i > 1 ? i - 1 : n
    right(i) = i < n ? i + 1 : 1

    # discretized diff operators
    # laplacian (times viscosity)
    Δ = spzeros(n, n)
    ν = 0.005
    for i in 1:n
        Δ[i,left(i)] = ν/Δx^2
        Δ[i,i] = -2ν/Δx^2
        Δ[i,right(i)] = ν/Δx^2
    end

    # gradient
    ∇ = spzeros(n, n)
    for i in 1:n
        ∇[i,left(i)] = -1/2/Δx
        ∇[i,right(i)] = 1/2/Δx
    end

    function burgers(ρ, (∇,Δ), t)
        return Δ*ρ - (∇*ρ) .* ρ
    end

    # uncertainty range
    m = 20 # parameter realizations scale as m^2
    σ = [0.5,0.5]
    ξ_range = [(ξ_1,ξ_2) for ξ_1 in range(-1,1,length=m), ξ_2 in range(-1,1,length=m)];

    #initial condition
    ub(x) = 0.5*(exp.(cos.(x)) .- 1.5).*sin.(x .+ 2π*0.37) # deterministic initial condition
    uprime(x,ξ,σ) = σ[1]*ξ[1]*sin.(2π*x) .+ σ[2]*ξ[2]*sin.(3π*x) # stochastic fluctuation
    ρ0_mat = hcat([ub(x_range) + uprime(x_range, ξ, σ) for ξ in ξ_range]...) # full rank initial condition
    r = 5 # approximation rank
    lr_ρ0 = truncated_svd(ρ0_mat, r); # intial condition

    dt = 1e-2 # time step
    lr_prob = MatrixDEProblem((ρ,t) -> burgers(ρ, (∇,Δ), t),  lr_ρ0, (0.0, 1.0)) # defines the matrix differential equation problem
    solvers = [UnconventionalAlgorithm(),
               ProjectorSplitting(PrimalLieTrotter()),
               ProjectorSplitting(DualLieTrotter()),
               ProjectorSplitting(Strang()),
               RankAdaptiveUnconventionalAlgorithm(1e-4, rmax=10)]
    for solver in solvers
        lr_sol = LowRankIntegrators.solve(lr_prob, solver, dt) # solves the low rank approximation
    end
end


module ComplexDiff

include("unicomplex.jl")
include("bicomplex.jl")
include("tricomplex.jl")
include("quadcomplex.jl")
include("pentacomplex.jl")

include("jacobian.jl")

export image2
export bicomplex, tricomplex, quadcomplex, pentacomplex
export derivative, biderivative, triderivative, quadderivative, pentaderivative
export jacobian

end


# This example simulates cluster synchronization using pinning control

import Cairo, Fontconfig
using LinearAlgebra
using DifferentialEquations
using PyPlot
using BlockArrays
using Graphs, GraphPlot
pygui(true)
close("all")

# Helper functions
getwave(x, i, d) = getindex.(x, (i - 1) * d + 1)

# Define node dynamics
h(x, a = -1 / 7, b = 2 / 7) = b * x + 1 / 2 * (a - b) * (abs(x + 1) - abs(x - 1))
function chua(dx, x, α = 9, β = 100 / 7, h = h)
    dx[1] = α * (x[2] - h(x[1]))
    dx[2] = x[1] - x[2] + x[3]
    dx[3] = -β * x[2]
end
d = 3

function lorenz(dx, x, σ = 10, β = 8 / 3, ρ = 35)
    dx[1] = σ * (x[2] - x[1])
    dx[2] = x[1] * (ρ - x[3]) - x[2]
    dx[3] = x[1] * x[2] - β * x[3]
end

# Define clusters
cls = [1:3, 4:6]
ks = length.(cls)
n = sum(ks)
l = length(ks)

# Define passivity index.
f = lorenz
Δ = diagm(ones(d)) * 10
P = diagm(ones(d))

# Define topology of the network
μ(A) = 1 / 2 * maximum(size(A)) * maximum(abs.(A))
Φ = BlockArray([
        -1.0 1 0 0 0 0
        1 -2 1 0 -1 1
        0 1 -1 0 1 -1
        0 0 0 -1 1 0
        0 -1 1 1 -2 1
        0 1 -1 0 1 -1
    ], [3, 3], [3, 3])

μs = zeros(l * l - l)
k = 1
for i = 1:l, j = 1:l
    global k
    if j != i
        μs[k] = μ(getblock(Φ, i, j))
        k += 1
    end
end
μm = maximum(μs)
δm = maximum(diag(Δ))

# Define initial constants
γs = [1, 1] * 200
Γiis = [diagm([zeros(k - 1); γ]) for (k, γ) in zip(ks, γs)]
Φiis = [getblock(Φ, i, i) for i = 1:l]
λs = [-maximum(eigvals(Φii - Γii)) for (Φii, Γii) in zip(Φiis, Γiis)]
βis = (δm + 2 * (l - 1) * μm) ./ λs
β = maximum(βis) * 1  # The condition is not statisfied.
αs = β * γs
E = copy(Φ)
for i = 1:l
    setblock!(E, getblock(E, i, i) * β, i, i)
end
E = [zeros(l, l) zeros(l, n); zeros(n, l) E]
H1 = zeros(n, l)
for (k, (j, α)) in zip(cumsum(ks), enumerate(αs))
    H1[k, j] = -α
end
H2 = zeros(n, n)
for (k, α) in zip(cumsum(ks), αs)
    H2[k, k] = α
end
H = [zeros(l, l + n); [H1 H2]]

# Define coupled network dynamics
function statefunc(dx, x, u, t, f = f, E = E, H = H, P = P)
    n = size(E, 1)
    d = size(P, 1)
    # Individual evolution
    for i = 1:n
        f(view(dx, (i-1)*d+1:i*d), view(x, (i-1)*d+1:i*d))
    end
    # Add coupling and control inputs
    dx .+= kron(E - H, P) * x
end

# Simulation network
tspan = (0.0, 100.0)
x0 = rand((n + l) * d) * 1e-3
sol = solve(ODEProblem(statefunc, x0, tspan), saveat = 0.001)
t = sol.t
x = sol.u

# Plot the results
ni = 1
nf = 50
graph = SimpleDiGraph(E - H)
gplot(graph, nodelabel = 1:nv(graph), layout = circular_layout) |> display
subplot(5, 2, 1)
plot(t, getindex.(x, 1))
subplot(5, 2, 2)
plot(getindex.(x, 1), getindex.(x, 2))
subplot(5, 2, 3)
plot(t, abs.(getwave(x, 1, d) - getwave(x, 3, d)))
subplot(5, 2, 4)
plot(t, abs.(getwave(x, 1, d) - getwave(x, 4, d)))
subplot(5, 2, 5)
plot(t, abs.(getwave(x, 1, d) - getwave(x, 5, d)))
subplot(5, 2, 6)
plot(t, abs.(getwave(x, 2, d) - getwave(x, 7, d)))
subplot(5, 2, 7)
plot(t, abs.(getwave(x, 2, d) - getwave(x, 7, d)))
subplot(5, 2, 8)
plot(t, abs.(getwave(x, 2, d) - getwave(x, 8, d)))
subplot(5, 2, 9)
plot(t, abs.(getwave(x, 1, d) - getwave(x, 2, d)))
subplot(5, 2, 10)
plot(t, abs.(getwave(x, 4, d) - getwave(x, 7, d)))
tight_layout()


# ____________________________________________________________________
# SpecLineGauss
#
mutable struct SpecLineGauss <: AbstractComponent
    norm::Parameter
    center::Parameter
    fwhm::Parameter
    voff::Parameter
    resolution::Float64

    function SpecLineGauss(center::Number)
        out = new(Parameter(1),
                  Parameter(center),
                  Parameter(3000),
                  Parameter(0),
                  0.)
        @assert center > 0
        out.norm.low = 0
        out.center.low = 0
        out.fwhm.low = 0
        out.voff.low = 0
        out.center.fixed = true
        return out
    end
end

function prepare!(comp::SpecLineGauss, domain::Domain{1})
    return fill(NaN, length(domain))
end

function evaluate!(buffer, comp::SpecLineGauss, x::Domain{1},
                   norm, center, fwhm, voff)
    x0 = center - (voff / 3.e5) * center
    σ_res = comp.resolution / 2.355 / 3.e5 * center
    σ     = fwhm            / 2.355 / 3.e5 * center
    σ = sqrt(σ^2 + σ_res^2)
    X = (x .- x0) ./ σ

    function profile(x)
        (abs(x) > 4)  &&  (return 0.)
        return norm * exp(-x^2 / 2) / sqrt(2pi) / σ
    end
    map!(profile, buffer, X)
end


export CosCharFuncPricer, makeCosCharFuncPricer, priceEuropean

struct CosCharFuncPricer{T}
    τ::T
    a::T
    b::T
    uk::Array{T,1}
    phi::Array{T,1}
    pi::T
end

function makeCosCharFuncPricer(
    cf::CharFunc{MAINT,CR},
    τ::T,
    m::Int,
    l::Int,
) where {T,CR,MAINT}
    p = model(cf)
    c1, c2, c4 = computeCumulants(p, τ)
    c2 = c2 + sqrt(abs(c4))
    a = c1 - l * sqrt(abs(c2))
    b = c1 + l * sqrt(abs(c2))
    # println("a ",a," b ",b)
    piHigh = const_pi(cf)
    z = @. (1:m) * piHigh / (b - a)
    phiz = map(z -> evaluateCharFunc(cf, z, τ), z)
    phi = @. real(phiz) * cos(-z * a) - imag(phiz) * sin(-z * a)
    uk = Vector{typeof(piHigh)}(undef, m)
    return CosCharFuncPricer(τ, a, b, uk, phi, piHigh)
end

function makeCosCharFuncPricer(
    cf::CharFunc{MAINT,CR},
    τ::T,
    l::Int;
    tol::T = 1e-8,
) where {T,CR,MAINT}
    p = model(cf)
    c1, c2, c4 = computeCumulants(p, τ)
    c2 = c2 + sqrt(abs(c4))
    a = c1 - l * sqrt(abs(c2))
    b = c1 + l * sqrt(abs(c2))
    c0 = cinf(p, τ)
    lWinv = Float64(2 * c0 / (tol * (b - a)))
    lW = lambertW(lWinv)
    m = ceil(Int, lW / Float64(c0) / Base.pi * Float64(b - a))
    m = max(32, m)
    # println("a ",a," b ",b, " m ",m, " lwi ",lWinv, " c0 ",c0)
    piHigh = const_pi(cf)
    z = @. (1:m) * piHigh / (b - a)
    phiz = map(z -> evaluateCharFunc(cf, z, τ), z)
    phi = @. real(phiz) * cos(-z * a) - imag(phiz) * sin(-z * a)
    uk = Vector{typeof(piHigh)}(undef, m)
    return CosCharFuncPricer(τ, a, b, uk, phi, piHigh)
end

#we adopt here the alternative formula of LeFloch "More Robust Pricing of European Options Based on Fourier Cosine Series Expansions"
function priceEuropean(
    p::CosCharFuncPricer{T},
    isCall::Bool,
    strike::T,
    forward::T,
    τ::T,
    discountDf::T,
) where {T}
    if τ != p.τ
        throw(DomainError(τ, string("maturity is different from pricer maturity ", p.τ)))
    end
    local pricePut
    x = log(forward / strike)
    if x >= -p.a && x >= p.b
        pricePut = 0
    elseif x <= p.a || x <= -p.b
        pricePut = discountDf * (strike - forward)
    else
        uk = p.uk
        a = p.a
        b = p.b
        ea = exp(a)
        f = forward
        piHigh = p.pi
        logStrike = log(strike / f)
        estrike = strike / f
        coeff = (-(estrike - ea) + estrike * (logStrike - a))*2 / (b - a)
        uk0 = coeff

        @inbounds for i = 1:length(uk)
            z = i * piHigh / (b - a)
            kPid = (logStrike - a) * z
            sk, ck = sincos(kPid)
            chi = (ck * estrike - 1 * ea + z * (sk * estrike)) / (1 + z^2)
            psi = sk / z
            coeff = (-chi + estrike * psi)*2 / (b - a)
            uk[i] = coeff
        end
        sumPut = uk0 / 2
        @inbounds for k = 1:length(uk)
            sumPut += p.phi[k] * p.uk[k]
        end
        pricePut = discountDf * f * sumPut
    end
    if isCall
        return pricePut + discountDf * (forward - strike)
    end
    return pricePut
end


using DecisionTree

D = 10
N = 500
NT = 100

x = randn(N, D)
function rastrigin(x)
  D = length(x)
  10 * D + sum( x.^2 ) - 10 * sum( cos( 2 * π * x ) )
end
y = vec(mapslices(rastrigin, x, 2))
xx = randn(NT, D)
yy = vec(mapslices(rastrigin, xx, 2))

model = build_forest(y, x, 3, 1000, 5, 0.7)
ypred2 = apply_forest(model, xx)

medape(ypred, y) = 100.0 * median(abs((ypred .- y) ./ y))
medape(ypred2, yy)


@polly function kernel_syr2k(alpha, beta, C, A, B)
    n,m = size(A)
    for i = 1:n
        for j = 1:i
            C[i,j] += beta
        end
        for k = 1:m, j =1:i
            C[i,j] += A[j,k] * alpha * B[i,k] + B[j,k] * alpha * A[i,k]
        end
    end
end

let
    m = 1000
    n = 1200

    C = zeros(Float32,n,n)
    A = zeros(Float32,n,m)
    B = zeros(Float32,n,m)

    alpha = Float32(1.5)
    beta = Float32(1.2)
    for i = 1:n, j = 1:m
        A[i,j] = ((i*j+1)%n) / n
        B[i,j] = ((i*j+2)%m) / m
    end
    for i = 1:n, j = 1:n
        C[i,j] = ((i*j+3)%n) / m
    end

    SUITE["syr2k"] = @benchmarkable kernel_syr2k($alpha, $beta, C, A, B) setup = (C = copy($C); A = copy($A); B = copy($B))
end


using PyPlot, MRIReco

##### simple example ####

N = 256
numCoils = 8
I = shepp_logan(N)
I = circularShutterFreq!(I,1)

coilsens = birdcageSensitivity(N, 8, 1.5)

# simulation parameters
params = Dict{Symbol, Any}()
params[:simulation] = "fast"
params[:trajName] = "Spiral"
params[:numProfiles] = 6
params[:numSamplingPerProfile] = div(N*N,16)
params[:windings] = div(N,16)
params[:AQ] = 2.0e-2
params[:senseMaps] = coilsens

# do simulation
acqData = simulation(I, params)

# reco parameters
params = Dict{Symbol, Any}()
params[:reco] = "multiCoil"
params[:reconSize] = (N,N)
params[:regularization] = "L2"
params[:λ] = 1.e-3
params[:iterations] = 40
params[:solver] = "cgnr"
params[:senseMaps] = coilsens
Ireco = reconstruction(acqData, params)

# use PyPlot for interactive display
figure(1)
clf()
subplot(1,2,1)
title("Phantom")
imshow(abs.(I))
subplot(1,2,2)
title("Reconstruction")
imshow(abs.(Ireco[:,:,1,1,1]))

# export images
filename = joinpath(dirname(pathof(MRIReco)),"../docs/src/assets/senseReco.png")
exportImage(filename, abs.(Ireco[:,:,1,1,1]) )


export testcase1a, testcase1a_point, testcase1b, testcase1c
export testcase1abis, testcase1ater
export testcase2
export testcase4a, testcase4b, testcase4c

# Test case 1.a
function testcase1a_point()
    x = PointE([Matrix{Float64}(I, 3, 3)], Float64[])
end

function testcase1a(; symmetric=false)
    if symmetric
        cmat = [Symmetric(sparse([1, 2, 3], [1, 2, 3], [1., 1., 1.]))]
        a1mats = [Symmetric(sparse([1], [1], [1.], 3, 3), :L)]
        a2mats = [Symmetric(sparse([2, 3], [2, 1], [1., 1.], 3, 3), :L)]
        a3mats = [Symmetric(sparse([2, 3], [1, 3], [1., 1.], 3, 3), :L)]
    else
        cmat = [sparse([1, 2, 3], [1, 2, 3], [1., 1., 1.])]
        a1mats = [sparse([1], [1], [1.], 3, 3)]
        a2mats = [sparse([1, 2, 3], [3, 2, 1], [1., 1., 1.], 3, 3)]
        a3mats = [sparse([1, 2, 3], [2, 1, 3], [1., 1., 1.], 3, 3)]
    end

    c = PointE(cmat, Vector{Float64}())
    a1 = PointE(a1mats, Vector{Float64}())
    a2 = PointE(a2mats, Vector{Float64}())
    a3 = PointE(a3mats, Vector{Float64}())

    A = [a1, a2, a3]

    b = Vector{Float64}([1, 1, 1])

    problem = SDCOContext(c, A, b)

    x = PointE([Matrix{Float64}(I, 3, 3)], Float64[])
    s = PointE([Matrix{Float64}(I, 3, 3)], Float64[])
    y = zeros(Float64, length(b))

    return problem, PointPrimalDual(x, y, s)
end

function testcase1abis()
    cmat = [sparse([1, 2, 3], [1, 2, 3], [1., 1., 1.])]
    c = PointE(cmat, Vector{Float64}())

    a1mats = [sparse([1], [1], [1.], 3, 3)]
    a1 = PointE(a1mats, Vector{Float64}())

    a2mats = [sparse([2, 3], [2, 1], [1., 1.], 3, 3)]
    a2 = PointE(a2mats, Vector{Float64}())

    a3mats = [sparse([2, 3], [1, 3], [1., 1.], 3, 3)]
    a3 = PointE(a3mats, Vector{Float64}())

    A = [a1, a2, a3]

    b = Vector{Float64}([1, 1, 1])

    problem = SDCOContext(c, A, b)

    x = PointE([Matrix{Float64}(I, 3, 3)], Float64[])

    y = ones(Float64, length(b)) * -1 * (1+sqrt(17)) / 4

    s = c - evaluate(A, y)

    return problem, PointPrimalDual(x, y, s)
end

function testcase1ater()
    cmat = [sparse([1, 2, 3], [1, 2, 3], [1., 1., 1.])]
    c = PointE(cmat, Vector{Float64}())

    a1mats = [sparse([1], [1], [1.], 3, 3)]
    a1 = PointE(a1mats, Vector{Float64}())

    a2mats = [sparse([2, 3], [2, 1], [1., 1.], 3, 3)]
    a2 = PointE(a2mats, Vector{Float64}())

    a3mats = [sparse([2, 3], [1, 3], [1., 1.], 3, 3)]
    a3 = PointE(a3mats, Vector{Float64}())

    A = [a1, a2, a3]

    b = Vector{Float64}([1, 1, 1])

    problem = SDCOContext(c, A, b)

    x = PointE([Matrix{Float64}(I, 3, 3)], Float64[])

    y = Float64[0, -1, -0.5]

    s = c - evaluate(A, y)

    return problem, PointPrimalDual(x, y, s)
end

function testcase1b()
    c = PointE(Dense{Float64}[], Float64[1., 1.])

    A = [PointE(Dense{Float64}[], Float64[1., 2.])]
    b = [1.]

    problem = SDCOContext(c, A, b)

    x = PointE(Dense{Float64}[], Float64[1/3, 1/3])
    y = [0.]
    s = PointE(Dense{Float64}[], Float64[1., 1.])
    return problem, PointPrimalDual(x, y, s)
end

function testcase1c(; symmetric=false)
    if symmetric
        cmat = [Symmetric(sparse(1.0I, 3, 3), :L), Symmetric(sparse(1.0I, 3, 3), :L)]

        mat1 = Symmetric(sparse([1], [1], [1.], 3, 3), :L)
        mat2 = Symmetric(sparse([2, 3], [2, 1], [1., 1.], 3, 3), :L)
        mat3 = Symmetric(sparse([2, 3], [1, 3], [1., 1.], 3, 3), :L)
        matnull = Symmetric(spzeros(3,3), :L)
    else
        cmat = [sparse(1.0I, 3, 3), sparse(1.0I, 3, 3)]

        mat1 = sparse([1], [1], [1.], 3, 3)
        mat2 = sparse([1, 2, 3], [3, 2, 1], [1., 1., 1.], 3, 3)
        mat3 = sparse([1, 2, 3], [2, 1, 3], [1., 1., 1.], 3, 3)
        matnull = spzeros(3,3)
    end

    c = PointE(cmat, Float64[1., 1.])

    a1 = PointE([mat1, matnull], Float64[0, 0])
    a2 = PointE([mat2, matnull], Float64[0, 0])
    a3 = PointE([mat3, matnull], Float64[0, 0])

    a4 = PointE([matnull, mat1], Float64[0, 0])
    a5 = PointE([matnull, mat2], Float64[0, 0])
    a6 = PointE([matnull, mat3], Float64[0, 0])

    a7 = PointE([matnull, matnull], Float64[1., 2.])
    A = [a1, a2, a3, a4, a5, a6, a7]

    b = Vector{Float64}([1, 1, 1, 1, 1, 1, 1])

    problem = SDCOContext(c, A, b)

    y0 = -(1+sqrt(17)) / 4
    y = y0 * ones(7)

    x = PointE([Matrix{Float64}(I, 3, 3), Matrix([1 0 0.25; 0 0.5 0; 0.25 0 1])], Float64[(5-sqrt(17))/2, (-3+sqrt(17))/4])

    s = c - evaluate(A, y)

    return problem, PointPrimalDual(x, y, s)
end

function testcase2(p, q, r; storage=:sparsefull)

    ## Objective
    B0 = rand(q, r)

    cmat::Sparse{Float64} = spzeros(q+r, q+r)
    if storage == :sparsesym
        for i=1:r, j=1:q
            cmat[q+i, j] = B0[j, i]
        end
        c = PointE([Symmetric(cmat, :L)], Float64[])
    elseif storage == :sparsefull
        for i=1:r, j=1:q
            cmat[q+i, j] = B0[j, i]
            cmat[j, i+q] = B0[j, i]
        end
        c = PointE([cmat], Float64[])
    else
        error("Unknown parameter $storage, choose from :sparsesym, :sparsefull")
    end

    if storage == :sparsesym
        A = PointE{Float64, SparseSym{Float64}}[]
    elseif storage == :sparsefull
        A = PointE{Float64, Sparse{Float64}}[]
    end
    b = Float64[]

    if storage == :sparsesym
        push!(A, PointE([Symmetric(sparse(-1.0I, q+r, q+r), :L)], Float64[]))
    elseif storage == :sparsefull
        push!(A, PointE([sparse(-1.0I, q+r, q+r)], Float64[]))
    end
    push!(b, -1.)

    for ctrind=1:p
        aimat = spzeros(q+r, q+r)
        Bi = rand(q, r)
        if storage == :sparsesym
            for i=1:r, j=1:q
                aimat[q+i, j] = Bi[j, i]
            end
            push!(A, PointE([Symmetric(aimat, :L)], Float64[]))
        elseif storage == :sparsefull
            for i=1:r, j=1:q
                aimat[q+i, j] = Bi[j, i]
                aimat[j, q+i] = Bi[j, i]
            end
            push!(A, PointE([aimat], Float64[]))
        end
        push!(b, 0.)
    end

    problem = SDCOContext(c, A, b)

    y = zeros(p+1)
    y[1] = norm(B0) + 1
    x = PointE([1/(q+r) * Matrix(1.0I, q+r, q+r)], Float64[])
    s = c - evaluate(A, y)

    z = PointPrimalDual(x, y, s)

    return problem, z
end

function testcase4a()
    cmat = [sparse([3], [3], [1.], 3, 3)]
    a1mats = [sparse([1, 2, 3], [2, 1, 3], [1., 1., 1.], 3, 3)]
    a2mats = [sparse([2], [2], [1.], 3, 3)]

    c = PointE(cmat, Vector{Float64}())
    a1 = PointE(a1mats, Vector{Float64}())
    a2 = PointE(a2mats, Vector{Float64}())

    A = [a1, a2]

    b = Vector{Float64}([-1, 0])

    return SDCOContext(c, A, b)
end

function testcase4b()
    cmat = [sparse(Float64[0 1; 1 0])]
    a1mats = [sparse(Float64[-1 0; 0 0])]
    a1mats = [sparse(Float64[0 0; 0 -1])]

    c = PointE(cmat, Vector{Float64}())
    a1 = PointE(a1mats, Vector{Float64}())
    a2 = PointE(a2mats, Vector{Float64}())

    A = [a1, a2]

    b = Vector{Float64}([-1, 0])

    return SDCOContext(c, A, b)
end

function testcase4c()
    cmat = [sparse(Float64[0 0; 0 0])]
    a1mats = [sparse(Float64[1 0; 0 0])]
    a1mats = [sparse(Float64[0 1; 1 0])]

    c = PointE(cmat, Vector{Float64}())
    a1 = PointE(a1mats, Vector{Float64}())
    a2 = PointE(a2mats, Vector{Float64}())

    A = [a1, a2]

    b = Vector{Float64}([0, 2])

    return SDCOContext(c, A, b)
end


"""
```
gr(r::Vector{Float64}, count::Vector{Float64}, density::Float64, binstep::Float64)
```

Computes the radial distribution function from the count data and
the density.

This is exactly a conventional g(r)
if a single atom was chosen as the solute and solvent selections.

Returns both the g(r) and the kb(r)

"""
function gr(r::Vector{Float64}, count::Vector{Float64}, density::Float64, binstep::Float64)
  nbins = length(r)
  gr = zeros(nbins)
  kb = zeros(nbins)
  for i in 1:nbins
    gr[i] = (count[i]/sphericalshellvolume(i,binstep))/density
    if i == 1
      kb[i] = 4π*(gr[i]-1)*r[i]^2*binstep
    else
      kb[i] = kb[i-1] + 4π*(gr[i]-1)*r[i]^2*binstep
    end
  end
  @. kb = units.Angs3tocm3permol * kb
  return gr, kb
end

"""

```
gr(R::Result) = gr(R.d,R.rdf_count,R.density.solvent_bulk,R.options.binstep)
```

If a Result structure is provided without further details, use the rdf count
and the bulk solvent density.
"""
gr(R::Result) = gr(R.d,R.rdf_count,R.density.solvent_bulk,R.options.binstep)


using PipeLayout.JuncLoc

using JuMP
using SCS

_scs = JuMP.optimizer_with_attributes(
    SCS.Optimizer,
    "eps" => 1e-6,
    "verbose" => 0
)

@testset "solve junction location for three terminals (SOC)" begin
    # equilateral triangle
    nodes = [Node(0,0), Node(40,0), Node(20, sqrt(3)/2*40)]
    demand = [10, 10, -20]
    bounds = fill(Bounds(40, 80), 3)
    diams = [Diameter(t...) for t in [(0.6, 0.5), (0.8, 0.8),(1.0, 1.2)]]

    # single Steiner node with dummy location
    topo = Topology(vcat(nodes, [Node(20, 20)]),
                    [Arc(3,4), Arc(4,1), Arc(4,2)])

    @testset "little flow, smallest diameter, Steiner node in center" begin
        inst = Instance(nodes, demand, bounds, diams, ploss_coeff_nice)
        model, x, y, t, π = JuncLoc.make_soc(
            inst, topo, JuncLoc.SOC(_scs))
        JuMP.optimize!(model)
	    status = JuMP.termination_status(model)

        @test status in (MOI.OPTIMAL, MOI.ITERATION_LIMIT)

        xsol, ysol = JuMP.value.(x), JuMP.value.(y)
        for i=1:3 # fixed terminals
            @test xsol[i] ≈ nodes[i].x atol=0.005
            @test ysol[i] ≈ nodes[i].y atol=0.005
        end
        @test xsol[4] ≈ 20 atol=0.01
        @test ysol[4] ≈ sqrt(3)/6*40 atol=0.01

        tsol = JuMP.value.(t)
        @test sum(tsol[1]) ≈ sum(tsol[2]) atol=0.01
        @test sum(tsol[1]) ≈ sum(tsol[3]) atol=0.01
    end

    @testset "more flow, mixed diameter, Steiner node towards source" begin
        inst = Instance(nodes, 20*demand, bounds, diams, ploss_coeff_nice)
        model, x, y, t, π = JuncLoc.make_soc(
            inst, topo, JuncLoc.SOC(_scs))
        JuMP.optimize!(model)
	    status = JuMP.termination_status(model)

        @test status == MOI.OPTIMAL

        xsol, ysol = JuMP.value.(x), JuMP.value.(y)
        for i=1:3 # fixed terminals
            @test xsol[i] ≈ nodes[i].x atol=0.005
            @test ysol[i] ≈ nodes[i].y atol=0.005
        end
        @test xsol[4] ≈ 20 atol=0.1
        @test ysol[4] >= sqrt(3)/6*40 # move near source

        tsol = JuMP.value.(t)
        @test tsol[2] ≈ tsol[3] atol=0.01
    end

    @testset "using the optimize function to solve" begin
        inst = Instance(nodes, 20*demand, bounds, diams, ploss_coeff_nice)
        result = optimize(inst, topo, JuncLoc.SOC(_scs))
        @test result.status == MOI.OPTIMAL

        sol = result.sol
        toposol = Topology(sol.nodes, topo.arcs)
        L = pipelengths(toposol)
        c = [d.cost for d in diams]
        obj = L' * sol.lsol * c
        @test result.value ≈ obj[1] atol=0.005
    end
end