"""
    get_optimized_1tree(cost)

Obtain an optimized 1tree given the cost matrix. This calls [`get_1tree`](@ref) and then adds extra
benefits for each point which has only one edge to make it more likely in the next run of [`get_1tree`](@ref)
that the point will have two edges. A negative benefit is used for points which have more than two edges.

Return the tree as a list of edges and the cost of the tree.
"""
function get_optimized_1tree(original_cost; runs=10)
    cost = deepcopy(original_cost)
    N = size(cost, 1)
    tree, lb = get_1tree(cost)
    degrees = zeros(Int, N)
    point_benefit = zeros(Float64, N)
    extra_cost = 0.0
    non_inf_cost = filter(c->!isinf(c), cost)
    max_cost = maximum(non_inf_cost)

    cost_factor = max_cost/N
    for _ in 1:runs
        degrees .= 0
        point_benefit .= 0.0
        for edge in tree
            degrees[edge.src] += 1
            degrees[edge.dst] += 1
        end
        for i in 1:N
            point_benefit[i] = cost_factor*(2-degrees[i])
            cost[i, :] .-= point_benefit[i]
            cost[:, i] .-= point_benefit[i]
        end
        extra_cost = 2*sum(point_benefit)
        tree, lb = get_1tree(cost)
        cost_factor *= 0.9
    end

    return tree, lb+extra_cost
end

"""
    get_1tree(cost)

Get a 1tree and cost of it based on a given cost matrix.
Create an MST first for the first 1 to N-1 points and then add two edges which connects the Nth point to its two nearest neighbors.

Can be optimized by adding extra costs with [`get_optimized_1tree`](@ref) which obtains a better lower bound.

Return the tree as a list of edges and the cost which can be used as the lower bound for the TSP problem.
"""
function get_1tree(cost)
    N = size(cost, 1)

    tree = prim_mst(complete_graph(N-1), cost)

    mst_cost = get_edges_cost(cost, tree)

    # extra cost for the two edges from the last point to the two nearest neighbors
    nearest_neighbors = partialsortperm(cost[N,:], 1:3)
    n_actual_neighbors = 0
    extra_costs = 0.0
    for neighbor in nearest_neighbors
        n_actual_neighbors == 2 && break
        if neighbor != N
            n_actual_neighbors += 1
            push!(tree, Edge(N, neighbor))
            extra_costs += cost[N, neighbor]
        end
    end

    lb = mst_cost + extra_costs

    return tree, lb
end


module HilbertSchmidtIndependenceCriterion

	using StatsFuns, Distances

  # load Base modules
  using Statistics, LinearAlgebra

	# includes
	include("common.jl")
	include("gammaHSIC.jl")

	# function exports
	export gammaHSIC, estimateKernelSize, rbfDotProduct

end # module


"""Align matrix such that in each row the maximum has the position of the maximum of the mean row"""
function align_max{T <: Number}(S::Matrix{T})
    b = mean(S, 1)[:] # compute mean row
    _, max_b = findmax(b)
    ncols = size(S, 2)
    A = Array(eltype(S), size(S)) # output array
    for i = 1:size(A, 1)
        _, max_i = findmax(S[i, :])
        offset = max_b - max_i
        for j = 1:size(A, 2)
            k = (j+offset+ncols-1) % ncols + 1
            A[i,j] = S[i,k]
        end
    end
    return A
end

function remove_b(S::Matrix, nlast=200)
    baseline = mean(S[end-nlast:end, :], 1)
    _, rip_index = findmax(baseline)
    B = S .- baseline
end


# export delaunayWall;
# #===============================================================================
# #
# #	src/deWall.jl
# #
# #	This File Contains:
# #
# #	 - delaunayWall(
# #			P::Matrix,
# #			ax = 1,
# #			Pblack = Float64[],
# #			AFL = Array{Int64,1}[],
# #			tetraDict = DataStructures.Dict{Array{Int64,1},Array{Float64,1}}();
# #			DEBUG = false
# #		)::Array{Array{Int64,1},1}
# #
# #	 - firstDeWallSimplex(
# #			P::Matrix,
# #			ax::Int64,
# #			off::Float64;
# #			DEBUG = false
# #		)::Array{Int64,1}
# #
# #	 - findWallSimplex(
# #			P::Matrix,
# #			blackidx::Int64,
# #			face::Array{Int64,1},
# #			oppoint::Array{Float64,1};
# #			DEBUG = false
# #		)::Union{Array{Int64,1}, Nothing}
# #
# #	 - recursiveDelaunayWall(
# # 			P::Matrix,
# #			Pblack::Matrix,
# # 			tetraDict::DataStructures.Dict{Array{Int64,1},Array{Float64,1}},
# # 			AFL::Array{Array{Int64,1},1},
# # 			ax::Int64,
# # 			off::Float64,
# # 			positive::Bool;
# # 			DEBUG = false
# # 		)::Array{Array{Int64,1},1}
# #
# #	 - updateAFL!(
# #			P::Matrix
# #			new::Array{Int64,1}[],
# #			AFLα = Array{Int64,1}[],
# #			AFLplus = Array{Int64,1}[],
# #			AFLminus = Array{Int64,1}[],
# #			ax::Int64,
# #			off::Float64;
# # 			DEBUG = false
# #		)::Bool
# #
# #	 - updatelist!(
# #			list,
# #			element
# #		)::Bool
# #
# #	 - updateTetraDict!(
# #			P::Matrix,
# #			tetraDict::DataStructures.Dict{Array{Int64,1},Array{Float64,1}},
# #			AFL::Array{Array{Int64,1},1},
# #			σ::Array{Int64,1}
# #		)::Nothing
# #
# ===============================================================================#
#
# """
# 	delaunayWall(
# 		P::Matrix, ax = 1, Pblack::Float64[], AFL = Array{Int64,1}[],
# 		tetraDict = DataStructures.Dict{Array{Int64,1},Array{Float64,1}}();
# 		DEBUG = false
# 	)::Array{Array{Int64,1},1}
#
# Return the Delaunay Triangulation of sites `P` via Delaunay Wall algorithm.
# The optional argument `ax` specify on wich axis it will build the Wall.
# The optional argument `AFL` is used in recursive call.
# If the keyword argument `DEBUG` is set to true than all the procedure is shown.
#
# # Examples
# ```jldoctest
#
# julia> P = [
#  0.0 1.0 0.0 1.0 0.0 1.0 0.0 1.0
#  0.0 0.0 1.0 1.0 0.0 0.0 1.0 1.0
#  0.0 0.0 0.0 0.0 1.0 1.0 1.0 1.0
# ];
#
# julia> DT = AlphaStructures.delaunayWall(P)
# 6-element Array{Array{Int64,1},1}:
#  [1, 2, 4, 5]
#  [1, 3, 4, 5]
#  [2, 4, 5, 6]
#  [3, 4, 5, 7]
#  [4, 5, 6, 7]
#  [4, 6, 7, 8]
#
# ```
# """
# function delaunayWall(
# 		P::Matrix,
# 		ax = 1,
# 		Pblack = Float64[],
# 		AFL = Array{Int64,1}[],
# 		tetraDict = DataStructures.Dict{Array{Int64,1},Array{Float64,1}}();
# 		DEBUG = false
# 	)::Array{Array{Int64,1},1}
#
# 	if DEBUG @show "Delaunay Wall with parameters" P ax AFL tetraDict end
#
# 	# 0 - Data Reading and Container definition
# 	DT = Array{Int64,1}[]		# Delaunay Triangulation
# 	AFLα = Array{Int64,1}[]		# (d-1)faces intersecting the Wall
# 	AFLplus = Array{Int64,1}[]  # (d-1)faces in positive Wall half-space
# 	AFLminus = Array{Int64,1}[] # (d-1)faces in positive Wall half-space
# 	off = AlphaStructures.findMedian(P, ax)
# 	if !isempty(Pblack) Pext = [P Pblack] else Pext = copy(P) end
#
# 	# 1 - Determine first simplex (if necessary)
# 	if isempty(AFL)
# 		@assert isempty(Pblack) "delaunayWall: If AFL is empty => Pblack must be"
# 		@assert isempty(tetraDict) "delaunayWall: If AFL is empty => tetraDict must be"
# 		σ = sort(AlphaStructures.firstDeWallSimplex(P, ax, off, DEBUG = DEBUG))
# 		push!(DT, σ)
# 		AFL = AlphaStructures.simplexFaces(σ)
# 		AlphaStructures.updateTetraDict!(P, tetraDict, AFL, σ)
# 	else
# 		@assert !isempty(Pblack) "delaunayWall: Data missing - Pblack"
# 		@assert !isempty(AFL) "delaunayWall: Data missing - AFL"
# 		@assert !isempty(tetraDict) "delaunayWall: Data missing - tetraDict"
# 	end
#
# 	# 2 - Build `AFL*` according to the axis `ax` with constant term `off`
# 	AlphaStructures.updateAFL!(
# 		P, AFL, AFLα, AFLplus, AFLminus, ax, off, DEBUG = DEBUG
# 	)
#
# 	# 4 - Build simplex Wall
# 	while !isempty(AFLα)
# 		# if face ∈ keys(tetraDict) oppoint = tetraDict[face]
# 		# else Pselection = setdiff([i for i = 1 : n], face) end
# 		σ = AlphaStructures.findWallSimplex(
# 				Pext, AFLα[1], tetraDict[AFLα[1]], size(P, 2), DEBUG = DEBUG
# 			)
# 		if σ != nothing && σ ∉ DT
# 			push!(DT, σ)
# 			AFL = AlphaStructures.simplexFaces(σ)
# 			AlphaStructures.updateTetraDict!(P, tetraDict, AFL, σ)
# 			# Split σ's Faces according in semi-spaces
# 			AlphaStructures.updateAFL!(
# 				P, AFL, AFLα, AFLplus, AFLminus, ax, off, DEBUG=DEBUG
# 			)
# 		else
# 			@assert AlphaStructures.updatelist!(AFLα, AFLα[1]) == false "delaunayWall:
# 				Something unespected happends while removing a face."
# 		end
# 	end
#
# 	# 5 - Change the axis `ax` and repeat until there are no faces but exposed.
# 	#      A.K.A. Divide & Conquer phase.
# 	if !isempty(AFLminus)
# 		union!(DT, recursiveDelaunayWall(
# 			P, Pblack, tetraDict, AFLminus, ax, off, false; DEBUG = DEBUG
# 		))
# 	end
# 	if !isempty(AFLplus)
# 		union!(DT, recursiveDelaunayWall(
# 			P, Pblack, tetraDict, AFLplus, ax, off, true; DEBUG = DEBUG
# 		))
# 	end
#
# 	return DT
# end
#
# #-------------------------------------------------------------------------------
# """
# 	findWallSimplex(
# 		P::Matrix,
# 		face::Array{Int64,1}, oppoint::Array{Float64,1},
# 		blackidx = size(P, 2);
# 		DEBUG = false
# 	)::Union{Array{Int64,1}, Nothing}
#
# Returns the simplex 'σ' build with `face` and a point from `P[:, 1:blackidx]`
# such that it is in the opposite half plane of `oppoint`.
# If such a simplex do not exists it returns `nothing`.
# If `blackidx` is not specified all the points `P` are treaten as valid.
# If the keyword argument `DEBUG` is set to true than all the procedure is shown.
#
# # Examples
# ```jldoctest
#
# julia> P = [
#  0. 1. 0. 0. 2.;
#  0. 0. 1. 0. 2.;
#  0. 0. 0. 1. 2.
# ];
#
# julia> newtetra = AlphaStructures.findWallSimplex(P,[2,3,4],[0., 0., 0.])
# 4-element Array{Int64,1}:
#  2
#  3
#  4
#  5
#
# ```
# """
# function findWallSimplex(
# 		P::Matrix,
# 		face::Array{Int64,1},
# 		oppoint::Array{Float64,1},
# 		blackidx = size(P, 2);
# 		DEBUG = false
# 	)::Union{Array{Int64,1}, Nothing}
#
# 	if DEBUG @show "find Wall Simplex of" face oppoint end
# 	# Find the points in the halfspace defined by `face` that do not
# 	#  containsother the other point of the simplex.
# 	Pselection =
# 		AlphaStructures.oppositeHalfSpacePoints(P, P[:, face], oppoint)
#
# 	if DEBUG @show Pselection end
#
# 	# If there are no such points than the face is part of the convex hull.
# 	if isempty(Pselection)
# 		return nothing
# 	end
#
# 	# Find the Closest Point in the other halfspace with respect to σ
# 	#  according to dd-distance.
# 	idxbase = Pselection[ AlphaStructures.findClosestPoint(
# 		P[:, face], P[:, Pselection], metric = "dd"
# 	) ]
#
# 	# @assert !isnothing(idxbase)
# 	# if isnothing(idxbase)
# 	# 	return nothing
# 	# end
#
# 	# It prevent from adding the same simplex again (cause it has been
# 	#  determined in a previous recursive call in the stacktrace).
# 	if idxbase > blackidx
# 		if DEBUG println("Excluding $face cause simplex already inside.") end
# 		return nothing
# 	end
#
# 	σ = sort([face; idxbase])
# 	if DEBUG @show "Found face" σ end
#
# 	# Check the simplex correctness
# 	radius, center = AlphaStructures.findRadius(P[:, σ], true)
# 	for i = 1 : size(P, 2)
# 		if LinearAlgebra.norm(center - P[:, i]) < radius - 1.e-14
# 			# @assert i ∉ Pselection "ERROR: Numerical error
# 			# 	evaluating minimum radius for $σ"
# 			if DEBUG println("$σ discarded due to a closer point.") end
# 			return nothing
# 		end
# 	end
#
# 	return σ
# end
#
#
# #-------------------------------------------------------------------------------
#
# """
#  	firstDeWallSimplex(
# 		P::Matrix, ax::Int64, off::Float64;
# 		DEBUG = false
# 	)::Array{Int64,1}
#
# Returns the indices array of the points in `P` that form the first thetrahedron
# built over the Wall if the `ax` axes with contant term `off`.
# If the keyword argument `DEBUG` is set to true than all the procedure is shown.
#
# # Examples
# ```jldoctest
#
# julia> V = [
# 	0.0 1.0 0.0 0.0 2.0
# 	0.0 0.0 1.0 0.0 2.0
# 	0.0 0.0 0.0 1.0 2.0
# ];
#
# julia> firstDeWallSimplex(V, 1, AlphaStructures.findMedian(V,1))
# 4-element Array{Int64,1}:
#  1
#  2
#  3
#  4
#
# ```
# """
# function firstDeWallSimplex(
# 		P::Matrix,
# 		ax::Int64,
# 		off::Float64;
# 		DEBUG = false
# 	)::Array{Int64,1}
#
# 	dim = size(P, 1)
#     n = size(P, 2)
#
# 	if DEBUG println("Determine first Simplex with ax = $ax") end
#     # the first point of the simplex is the one with coordinate `ax` maximal
#     #  such that it is less than `off` (closer to α from minus)
#
# 	Pselection = findall(x -> x < off, P[ax, :])
#
# 	# it gives an error if no point are less than `off`
# 	#  in fact it means that all the points are located on the median,
# 	#  with respect to `ax`.
# 	@assert !isempty(Pselection) "firstDeWallSimplex: not able to build the first Delaunay
# 		dimplex; all the points have the same `ax` coordinate."
#     newidx = Pselection[findmax(P[ax, Pselection])[2]]
#     # indices will store the indices of the simplex ...
#     indices = [newidx]                      #Array{Int64,1}
#     # ... and `Psimplex` will store the corresponding points
#     Psimplex = P[:, newidx][:,:]    #Array{Float64,2}
#
#     # the second point must be seeken across those with coordinate `ax`
#     #  grater than `off`
#     Pselection = findall(x -> x > off, P[ax, :])
#
#     for d = 1 : dim
# 		idxbase = AlphaStructures.findClosestPoint(Psimplex, P[:, Pselection])
# 		@assert !isnothing(idxbase) "firstDeWallSimplex:
# 			not able to determine first Delaunay Simplex"
#         newidx = Pselection[idxbase]
#         indices = [indices; newidx]
#         Psimplex = [Psimplex P[:, newidx]]
#         Pselection = [i for i = 1 : n if i ∉ indices]
#     end
#
#     # Correctness check
# 	radius, center = AlphaStructures.findRadius(Psimplex, true)
#     for i = 1 : n
# 		@assert LinearAlgebra.norm(center - P[:, i]) >= radius "firstDeWallSimplex:
# 			Unable to find first Simplex."
# 	end
#
# 	if DEBUG println("First Simplex = $indices") end
#
# 	return indices
# end
#
# #-------------------------------------------------------------------------------
#
# """
# 	recursiveDelaunayWall(
# 		P::Matrix,
# 		Pblack::Matrix,
# 		tetraDict::DataStructures.Dict{Array{Int64,1},Array{Float64,1}},
# 		AFL::Array{Array{Int64,1},1},
# 		ax::Int64,
# 		off::Float64,
# 		positive::Bool;
# 		DEBUG = false
# 	)::Array{Array{Int64,1},1}
#
# Utility function that prepeares the Divide phase for Delaunay Wall.
# Returns the Delaunay Triangulation for the positve or negative subspace of `P`
# (according to `positive`) determined by the hyperplane with normal `ax` and
# constant term `off`.
# If the keyword argument `DEBUG` is set to true than all the procedure is shown.
# """
# function recursiveDelaunayWall(
# 		P::Matrix,
# 		Pblack::Array{Float64},
# 		tetraDict::DataStructures.Dict{Array{Int64,1},Array{Float64,1}},
# 		AFL::Array{Array{Int64,1},1},
# 		ax::Int64,
# 		off::Float64,
# 		positive::Bool;
# 		DEBUG = false
# 	)::Array{Array{Int64,1},1}
#
# 	#DEBUG = true
#
# 	dim, n = size(P)
# 	newaxis = mod(ax, dim) + 1
#
# 	if DEBUG println("Divide Plus/Minus $positive") end
#
# 	Psubset = findall(x -> (x > off) == positive, P[ax, :])
# 	blacklist = setdiff(unique([(keys(tetraDict)...)...]), Psubset)
# 	if !isempty(Pblack)
# 		Pblack = [Pblack P[:, blacklist]]
# 	else
# 	 	Pblack = P[:, blacklist]
# 	end
#
# 	if DEBUG println("Step In") end
#
# 	DT = AlphaStructures.delaunayWall(
# 			P[:, Psubset],
# 			newaxis,
# 			Pblack,
# 			[[findall(Psubset.==p)[1] for p in σ] for σ in AFL],
# 			Dict([
# 				[findall(Psubset.==p)[1] for p in k] => v
# 				for (k,v) in tetraDict
# 					if k ⊆ Psubset
# 			]),
# 			DEBUG = DEBUG
# 		)
#
# 	if DEBUG @show "Step Out with " DT end
#
# 	return [[Psubset[i] for i in σ] for σ in DT]
# end
#
# #-------------------------------------------------------------------------------
#
# """
# 	updateAFL!(
# 		P::Matrix
# 		new::Array{Int64,1}[],
# 		AFLα = Array{Int64,1}[],
# 		AFLplus = Array{Int64,1}[],
# 		AFLminus = Array{Int64,1}[],
# 		ax::Int64, off::Float64;
# 		DEBUG = false
# 	)::Bool
#
# Modify the `AFL*` lists of faces by adding to them the faces inside `new`
# (that refers to the points `P`) according to their position with respect
# to the axis defined by the normal direction `ax` and the contant term `off`.
# The function returns a Bool value that states if the operation was succesfully.
# If the keyword argument `DEBUG` is set to true than all the procedure is shown.
# """
# function updateAFL!(
# 		P::Matrix,
# 		newσ::Array{Array{Int64,1},1},
# 		AFLα::Array{Array{Int64,1},1},
# 		AFLplus::Array{Array{Int64,1},1},
# 		AFLminus::Array{Array{Int64,1},1},
# 		ax::Int64, off::Float64;
# 		DEBUG = false
# 	)::Bool
#
# 	for face in newσ
# 		inters = AlphaStructures.planarIntersection(P, face, ax, off)
#     	if inters == 0 # intersected by plane α
# 			AlphaStructures.updatelist!(AFLα, face)
# 		elseif inters == -1 # in NegHalfspace(α)
#         	AlphaStructures.updatelist!(AFLminus, face)
#     	elseif inters == 1 # in PosHalfspace(α)
#         	AlphaStructures.updatelist!(AFLplus, face)
#     	else
# 			return false
# 		end
# 	end
#
# 	if DEBUG @show AFLα AFLminus AFLplus end
#
# 	return true
#
# end
#
# #-------------------------------------------------------------------------------
#
# """
# 	updatelist!(list, element)::Bool
#
# If `element` is in `list` then it is removed (returns `false`);
# If `element` is not in `list` then it is added (returns `true`).
#
# # Examples
# ```jldoctest
# julia> list = [[1, 2, 3, 4], [2, 3, 4, 5]]
# 2-element Array{Array{Int64,1},1}:
#  [1, 2, 3, 4]
#  [2, 3, 4, 5]
#
# julia> updatelist!(list, [1, 2, 4, 5])
#  true
#
# julia> list
# 3-element Array{Array{Int64,1},1}:
#  [1, 2, 3, 4]
#  [2, 3, 4, 5]
#  [1, 2, 4, 5]
#
# julia> updatelist!(list, [1, 2, 4, 5])
#  false
#
# julia> list
# 2-element Array{Array{Int64,1},1}:
#  [1, 2, 3, 4]
#  [2, 3, 4, 5]
#
# ```
# """
#
# function updatelist!(list, element)::Bool
# 	if element ∈ list
# 		setdiff!(list, [element])
# 		return false
# 	else
# 		push!(list, element)
# 		return true
# 	end
# end
#
# #-------------------------------------------------------------------------------
#
# """
# 	updateTetraDict!(
# 		P::Matrix,
# 		tetraDict::DataStructures.Dict{Array{Int64,1},Array{Float64,1}},
# 		AFL::Array{Array{Int64,1},1},
# 		σ::Array{Int64,1}
# 	)::Nothing
#
# Update the content of `tetraDict` by adding the outer points of the faces of `σ`
# in the dictionary.
# """
# function updateTetraDict!(
# 		P::Matrix,
# 		tetraDict::DataStructures.Dict{Array{Int64,1},Array{Float64,1}},
# 		AFL::Array{Array{Int64,1},1},
# 		σ::Array{Int64,1}
# 	)::Nothing
# 	for cell in AFL
# 		point = setdiff(σ, cell)
# 		@assert length(point) == 1 "updateTetraDict!: Error during update of TetraDict $σ, $cell"
# 		tetraDict[ cell ] = P[:, point[1]]
# 	end
# end


"""
    refine_triangulation(points, simplex_inds, maxsize, k, factor)

Refine a triangulation recursively. Continue splitting some fraction of the simplices
with size reduction factor of 2 until until the maximum simplex size is below some
reference size.

Returns a tuple containing the refined triangulation and some information about it, as
follows: (points, simplex_inds, centroids, radii)
"""
function refine_recursive(points, simplex_inds, maxsize, k; niter = 1)
    if niter == 1
        println(size(points, 2), "D triangulation containing ", size(simplex_inds, 1), " simplices.")
        print("Target simplex radius = ", round(maxsize, 4), ". ")
        println("Split simplices into subsimplices with radii reduced by  factor of ", k, ".")
    end

    centroids, radii = centroids_radii2(points, simplex_inds)

    # If convergence is reached, return the triangulation and the centroids and radii of
    # the simplices furnishing the triangulation
    maxradius = maximum(radii)
    print("Refinement #", niter, "\tMaximum simplex radius = ", round(maxradius, 4),  ". ")

    if maxradius < maxsize || niter > 100
        if niter > 100
            println("\n\t\tMaximum number of iterations (100) reached. Ending refinement.")
        else
            println("\n\t\tRefinement finished.\n")
        end
        simplexvolumes = simplex_volumes(points, simplex_inds)

        return points, simplex_inds, centroids, radii, simplexvolumes
    end

    # If no convergence, continue splitting some fraction of the largest simplices.
    if length(radii) == 1
        percentile = 0.0
        split_indices = [1]
    else
        # What portion of the largest simplices to split (1 - percentile)?
        percentile = 0.999
        split_indices = []

        # If the simplices in the triangulation are very regular, there might not be
        # any simplices larger than a given percentile. If so, reduce percentile iteratively
        # until there are simplices larger than the percentile.
        while length(split_indices) == 0
            # Check if all elements are approximately the same size. If so, we must split all
            # of them.
            if all(y-> isapprox(radii[1], y), radii)
                split_indices =  find(radii .> 0)
            else
                split_indices = find(radii .> quantile(radii, percentile))
            end
            percentile = percentile - 0.02
        end
    end

    # The number of simplices to split
    n_split_simplices = length(split_indices)

    # The dimension of the space
    E = size(points, 2)

    # Generic rules for splitting a simplex
    splitting_rules = simplicial_subdivision(k, E)

    # Rules for forming the strictly new vertices of the subtriangulation
    rules = splitting_rules[1]

    # Array where each row represents one of the new simplices in the splitted simplex
    subtriangulation = splitting_rules[2]

    # How many new vertices are created each split?
    n_newvertices_eachsplit = size(rules, 1)

    println(" Splitting ", n_split_simplices, "/", size(simplex_inds, 1), " (", @sprintf("%.4f", 1.0 - percentile) ," %)", " simplices into ", n_split_simplices * n_newvertices_eachsplit - 1, " subsimplices.")

    # We need an array that can accomodate all of them. Each row in this
    # array will be a new vertex. Stacks of n_newvertices_eachsplit * E arrays.
    # We have as many as we have simplices to split.
    new_vertices = zeros(n_newvertices_eachsplit * n_split_simplices, E)

    # Fill the array by looping over all simplices that we want to split
    for i = 1:n_split_simplices
        # Figure out what the row indices corresponding to the ith simplex
        # must be. Marks the beginning of each of the simplex stacks in new_vertices
        ind = n_newvertices_eachsplit * (i -1)

        # Index of the simplex we need to split
        simplex_idx = split_indices[i]

        # Get the vertices of the simplex currently being splitted. Each of the
        # n_newvertices_eachsplit new vertices will be a linear combination
        # of these vertices. Each row is a vertex.
        vertices = points[simplex_inds[simplex_idx, :], :]

        # Generate the strictly new vertices for each sub
        for j = 1:n_newvertices_eachsplit
            # The index for a particular new vertex of the ith new simplex
            ind_newvertex = ind + j

            # Compute the jth vertex of the subtriangulation of the ith simplex

            # Go the jth new subsimplex of the ith splitted simplex. The entries of this
            # vector
            jth_subsimplex = rules[j, :]

            # Pick the corresponding original vertices with indices contained in rules[j, :]
            original_vertices = vertices[rules[j, :], :]

            new_vertices[ind_newvertex, :] = sum(original_vertices, dims=1) ./ k

        end
    end

    # Find the unique new vertices
    new_vertices_noreps = unique(new_vertices, 1)
    Ind = Vector{Int}(size(new_vertices, 1))
    for i = 1:size(new_vertices_noreps, 1)
        for j = 1:size(new_vertices, 1)
            if new_vertices_noreps[i, :] == new_vertices[j, :]
                Ind[j] = i
            end
        end
    end

    # Combine old and newly introduced vertices
    num_vertices_beforesplit = size(points, 1)
    allpoints = vcat(points, new_vertices_noreps)

    # Update the Ind array, so that we start at the new vertices
    Ind = Ind + num_vertices_beforesplit
    num_simplices_each_split = size(subtriangulation, 1)

    # The subsimplices formed by the splitting. Each row contains E + 1 indices referencing
    # the vertices furnishing that particular subsimplex (now found in the updated
    # points array).
    newtriangulation = Array{Float64}(num_simplices_each_split * n_split_simplices, E + 1)

    # For each simplex that we need to split,
    for i = 1:n_split_simplices
        # The beginning of the stack we need to fill.
        index = num_simplices_each_split * (i - 1)

        # Figure out what the row indices corresponding to the ith simplex
        # must be. Marks the beginning of each of the simplex stacks in new_vertices
        ind = n_newvertices_eachsplit * (i -1)

        # Index of the simplex we need to split
        simplex_idx = split_indices[i]

        # Pick the indices of the original vertices. Should be a column vector.
        inds_original_vertices = simplex_inds[simplex_idx, :]

        # Indices of the new vertices. Should be a column vector
        inds_new_vertices = Ind[(ind + 1):(ind + n_newvertices_eachsplit)]
        inds_all_vertices = vcat(inds_original_vertices, inds_new_vertices)

        # Go through each of the subsimplices formed by splitting that simplex
        for j = 1:num_simplices_each_split
            newtriangulation[index + j, :] = inds_all_vertices[subtriangulation[j, :]]
        end
    end

    # Indices of the simplices that are not split
    untouched_indices = setdiff(1:size(simplex_inds, 1), split_indices)

    all_simplex_inds = round.(Int, vcat(simplex_inds[untouched_indices, :],
                                  newtriangulation))

    refine_recursive(allpoints, all_simplex_inds, maxsize, k, niter = niter + 1)
end


using Random

function _benchmark_opt(tfopt, adcmeopt)
    reset_default_graph()
    n = 100
    Random.seed!(233)
    θ = Variable(ae_init([2,10,10,2]))
    y = fc(rand(10,2), [10,10,2], θ)
    loss = sum(y^2)
    opt = tfopt().minimize(loss)

    global sess = Session(); init(sess)
    qs = zeros(10)
    for i = 1:10
        qs[i] = run(sess, loss)
        run(sess, opt)
    end

# ------------------ ADCME ----------------------
    init(sess)
    opt = adcmeopt()
    uo = UnconstrainedOptimizer(sess, loss, vars=[θ])
    x0 = getInit(uo)
    fs = zeros(10)
    for i = 1:10
        fs[i], df = getLossAndGrad(uo, x0)
        Δ = apply!(opt, x0, df)
        x0 -= Δ
    end
    flag =  maximum(abs.(fs-qs)./abs.(qs))<1e-2
    @info maximum(abs.(fs-qs)./abs.(qs))
    println(fs)
    println(qs)
    return flag
end

function _print_opt(adcmeopt)
    reset_default_graph()
    n = 100
    Random.seed!(233)
    θ = Variable(ae_init([2,10,10,2]))
    y = fc(rand(10,2), [10,10,2], θ)
    loss = sum(y^2)
    global sess = Session(); init(sess)
    opt = adcmeopt()
    uo = UnconstrainedOptimizer(sess, loss, vars=[θ])
    x0 = getInit(uo)
    fs = zeros(10)
    for i = 1:10
        fs[i], df = getLossAndGrad(uo, x0)
        Δ = apply!(opt, x0, df)
        x0 -= Δ
    end
    println("$adcmeopt: ")
    println(fs)
end

@testset "Optimizers" begin
    @test _benchmark_opt(GradientDescentOptimizer, Descent)
    @test _benchmark_opt(AdamOptimizer, ADAM)
    @test _benchmark_opt(AdagradOptimizer, ADAGrad)
    @test _benchmark_opt(AdadeltaOptimizer, ADADelta)
    _print_opt(RMSProp)
    _print_opt(AMSGrad)
    _print_opt(NADAM)
    _print_opt(Momentum)
    _print_opt(Nesterov)
    _print_opt(RADAM)
    _print_opt(AdaMax)
end


@testset "rationalquad" begin
    rng = MersenneTwister(123456)
    x = rand(rng) * 2
    v1 = rand(rng, 3)
    v2 = rand(rng, 3)

    @testset "RationalQuadraticKernel" begin
        α = 2.0
        k = RationalQuadraticKernel(; α=α)

        @testset "RQ ≈ EQ for large α" begin
            @test isapprox(
                RationalQuadraticKernel(; α=1e9)(v1, v2),
                SqExponentialKernel()(v1, v2);
                atol=1e-6,
                rtol=1e-6,
            )
        end

        @test metric(RationalQuadraticKernel()) == SqEuclidean()
        @test metric(RationalQuadraticKernel(; α=2.0)) == SqEuclidean()
        @test repr(k) == "Rational Quadratic Kernel (α = $(α))"

        # Standardised tests.
        TestUtils.test_interface(k, Float64)
        test_ADs(x -> RationalQuadraticKernel(; alpha=x[1]), [α])
        test_params(k, ([α],))
    end

    @testset "GammaRationalQuadraticKernel" begin
        k = GammaRationalQuadraticKernel()

        @test repr(k) == "Gamma Rational Quadratic Kernel (α = 2.0, γ = 2.0)"

        @testset "Default GammaRQ ≈ RQ with rescaled inputs" begin
            @test isapprox(
                GammaRationalQuadraticKernel()(v1 ./ sqrt(2), v2 ./ sqrt(2)),
                RationalQuadraticKernel()(v1, v2),
            )
            a = 1.0 + rand()
            @test isapprox(
                GammaRationalQuadraticKernel(; α=a)(v1 ./ sqrt(2), v2 ./ sqrt(2)),
                RationalQuadraticKernel(; α=a)(v1, v2),
            )
        end

        @testset "GammaRQ ≈ EQ for large α with rescaled inputs" begin
            v1 = randn(2)
            v2 = randn(2)
            @test isapprox(
                GammaRationalQuadraticKernel(; α=1e9)(v1 ./ sqrt(2), v2 ./ sqrt(2)),
                SqExponentialKernel()(v1, v2);
                atol=1e-6,
                rtol=1e-6,
            )
        end

        @testset "GammaRQ(γ=1) ≈ Exponential with rescaled inputs for large α" begin
            v1 = randn(4)
            v2 = randn(4)
            @test isapprox(
                GammaRationalQuadraticKernel(; α=1e9, γ=1.0)(v1, v2),
                ExponentialKernel()(v1, v2);
                atol=1e-6,
                rtol=1e-6,
            )
        end

        @testset "GammaRQ ≈ GammaExponential for same γ and large α" begin
            v1 = randn(3)
            v2 = randn(3)
            γ = rand() + 0.5
            @test isapprox(
                GammaRationalQuadraticKernel(; α=1e9, γ=γ)(v1, v2),
                GammaExponentialKernel(; γ=γ)(v1, v2);
                atol=1e-6,
                rtol=1e-6,
            )
        end

        @test metric(GammaRationalQuadraticKernel()) == Euclidean()
        @test metric(GammaRationalQuadraticKernel(; γ=2.0)) == Euclidean()
        @test metric(GammaRationalQuadraticKernel(; γ=2.0, α=3.0)) == Euclidean()

        # Standardised tests.
        TestUtils.test_interface(k, Float64)
        a = 1.0 + rand()
        test_ADs(x -> GammaRationalQuadraticKernel(; α=x[1], γ=x[2]), [a, 1 + 0.5 * rand()])
        test_params(GammaRationalQuadraticKernel(; α=a, γ=x), ([a], [x]))
    end
end


using NiLang, NiLang.AD, Test
using NiLang.AD: hessian_numeric

@testset "hessian" begin
    h1 = hessian_backback(PlusEq(*), (0.0, 2.0, 3.0); iloss=1)
    h2 = hessian_numeric(PlusEq(*), (0.0, 2.0, 3.0); iloss=1)
    @test h1 ≈ h2

    @i function test(a,b,c,d)
        a += b*c
        a += b^d
        c += b/d
        ROT(a, c, d)
        b += d ^ 2
        a += c * d
    end
    h1 = hessian_backback(test, (0.0, 2.0, 1.0, 3.0); iloss=1)
    h2 = hessian_numeric(test, (0.0, 2.0, 1.0, 3.0); iloss=1)
    @show h2
    @test isapprox(h1, h2, atol=1e-8)
end


using ContactStructures

plotlyjs()
plot_along_xy_grid(-2:0.5:2,-2:0.5:2,0,standard_structure;  markersize = 2.5,ε = 0.25)
plot!(xlims=(-3, 3), ylims=(-3,3), zlims=(-3,3),legend=false)


# Transformation of tensors

"""
    rot3(C, a, b, x) -> Cr

Return a rotated tensor `Cr`, the result of rotating in turn the tensor `C`:

1. About the x1 axis by `a` degrees clockwise when looking down the x1 axis
2.   "    "  x2   "   " `b`    "        "       "     "      "   "  x2   "
3.   "    "  x3   "   " `b`    "        "       "     "      "   "  x3   "

Rotations are performed in that order,
"""
rot3(C, a, b, c) = transform(C, RotXYZ(deg2rad(a), deg2rad(b), deg2rad(c)))

"""
    transform(C, M) -> C′

Apply a transformation matrix `M` to a 6x6 matrix `C`, returning the transformed matrix `C′`.
"""
function transform(C, M)
    K = _transformation_matrix(M)
    return K * (C * transpose(K))
end

"""
    _transformation_matrix(M) -> K

Return the matrix `K` which represents the matrix with which to transform a Voigt
6x6 matrix by the 3x3 transformation matrix `M`.
"""
@inline function _transformation_matrix(M)
    K = MMatrix{6, 6, Float64}(undef)
    @inbounds for i = 1:3
        i1 = (i + 1)%3
        i1 = i1 == 0 ? 3 : i1
        i2 = (i + 2)%3
        i2 = i2 == 0 ? 3 : i2
        for j = 1:3
            j1 = (j + 1)%3
            j1 = j1 == 0 ? 3 : j1
            j2 = (j + 2)%3
            j2 = j2 == 0 ? 3 : j2
            K[i,j] = M[i,j]^2
            K[i,j+3] = 2*M[i,j1]*M[i,j2]
            K[i+3,j] = M[i1,j]*M[i2,j]
            K[i+3,j+3] = M[i1,j1]*M[i2,j2] + M[i1,j2]*M[i2,j1]
        end
    end
    K
end


#    Modified Bessel functions of the second kind of order zero and one
#                       besselk0, besselk1
#
#    Scaled modified Bessel functions of the second kind of order zero and one
#                       besselk0x, besselk1x
#
#    (Scaled) Modified Bessel functions of the second kind of order nu
#                       besselk, besselkx
#
#
#    Calculation of besselk0 is done in two branches using polynomial approximations [1]
#
#    Branch 1: x < 1.0
#              besselk0(x) + log(x)*besseli0(x) = P7(x^2)
#                            besseli0(x) = [x/2]^2 * P6([x/2]^2) + 1
#    Branch 2: x >= 1.0
#              sqrt(x) * exp(x) * besselk0(x) = P22(1/x) / Q2(1/x)
#    where P7, P6, P22, and Q2 are 7, 6, 22, and 2 degree polynomials respectively.
#
#
#
#    Calculation of besselk1 is done in two branches using polynomial approximations [2]
#
#    Branch 1: x < 1.0
#              besselk1(x) - log(x)*besseli1(x) - 1/x = x*P8(x^2)
#                            besseli1(x) = [x/2]^2 * (1 + 0.5 * (*x/2)^2 + (x/2)^4 * P5([x/2]^2))
#    Branch 2: x >= 1.0
#              sqrt(x) * exp(x) * besselk1(x) = P22(1/x) / Q2(1/x)
#    where P8, P5, P22, and Q2 are 8, 5, 22, and 2 degree polynomials respectively.
#
#
#    The polynomial coefficients are taken from boost math library [3].
#    Evaluation of the coefficients using Remez.jl is prohibitive due to the increase
#    precision required in ArbNumerics.jl.
#
#    Horner's scheme is then used to evaluate all polynomials.
#
#    Calculation of besselk and besselkx can be done with recursion starting with
#    besselk0 and besselk1 and using upward recursion for any value of nu (order).
#
#                    K_{nu+1} = (2*nu/x)*K_{nu} + K_{nu-1}
#
#    When nu is large, a large amount of recurrence is necesary.
#    We consider uniform asymptotic expansion for large orders to more efficiently
#    compute besselk(nu, x) when nu is larger than 100 (Let's double check this cutoff)
#    The boundary should be carefully determined for accuracy and machine roundoff.
#    We use 10.41.4 from the Digital Library of Math Functions [5].
#    This is also 9.7.8 in Abramowitz and Stegun [6].
#    K_{nu}(nu*z) = sqrt(pi / 2nu) *exp(-nu*n)/(1+z^2)^1/4 * sum((-1^k)U_k(p) /nu^k)) for k=0 -> infty
#    The U polynomials are the most tricky. They are listed up to order 4 in Table 9.39
#    of [6]. For Float32, >=4 U polynomials are usually necessary. For Float64 values,
#    >= 8 orders are needed. However, this largely depends on the cutoff of order you need.
#    For moderatelly sized orders (nu=50), might need 11-12 orders to reach good enough accuracy
#    in double precision.
#
#    However, calculation of these higher order U polynomials are tedious. These have been hand
#    calculated and somewhat crosschecked with symbolic math. There could be errors. They are listed
#    in src/U_polynomials.jl as a reference as higher orders are impossible to find while being needed for any meaningfully accurate calculation.
#    For large orders these formulas will converge much faster than using upward recurrence.
#
#
# [1] "Rational Approximations for the Modified Bessel Function of the Second Kind
#     - K0(x) for Computations with Double Precision" by Pavel Holoborodko
# [2] "Rational Approximations for the Modified Bessel Function of the Second Kind
#     - K1(x) for Computations with Double Precision" by Pavel Holoborodko
# [3] https://github.com/boostorg/math/tree/develop/include/boost/math/special_functions/detail
# [4] "Computation of Bessel Functions of Complex Argument and Large Order" by Donald E. Amos
#      Sandia National Laboratories
# [5] https://dlmf.nist.gov/10.41
# [6] Abramowitz, Milton, and Irene A. Stegun, eds. Handbook of mathematical functions with formulas, graphs, and mathematical tables.
#     Vol. 55. US Government printing office, 1964.
#

"""
    besselk0(x::T) where T <: Union{Float32, Float64}

Modified Bessel function of the second kind of order zero, ``K_0(x)``.
"""
function besselk0(x::T) where T <: Union{Float32, Float64}
    x <= zero(T) && return throw(DomainError(x, "`x` must be nonnegative."))
    if x <= one(T)
        a = x * x / 4
        s = muladd(evalpoly(a, P1_k0(T)), inv(evalpoly(a, Q1_k0(T))), T(Y_k0))
        a = muladd(s, a, 1)
        return muladd(-a, log(x), evalpoly(x * x, P2_k0(T)))
    else
        s = exp(-x / 2)
        a = muladd(evalpoly(inv(x), P3_k0(T)), inv(evalpoly(inv(x), Q3_k0(T))), one(T)) * s / sqrt(x)
        return a * s
    end
end
"""
    besselk0x(x::T) where T <: Union{Float32, Float64}

Scaled modified Bessel function of the second kind of order zero, ``K_0(x)*e^{x}``.
"""
function besselk0x(x::T) where T <: Union{Float32, Float64}
    x <= zero(T) && return throw(DomainError(x, "`x` must be nonnegative."))
    if x <= one(T)
        a = x * x / 4
        s = muladd(evalpoly(a, P1_k0(T)), inv(evalpoly(a, Q1_k0(T))), T(Y_k0))
        a = muladd(s, a, 1)
        return muladd(-a, log(x), evalpoly(x * x, P2_k0(T))) * exp(x)
    else
        return muladd(evalpoly(inv(x), P3_k0(T)), inv(evalpoly(inv(x), Q3_k0(T))), one(T)) / sqrt(x)
    end
end
"""
    besselk1(x::T) where T <: Union{Float32, Float64}

Modified Bessel function of the second kind of order one, ``K_1(x)``.
"""
function besselk1(x::T) where T <: Union{Float32, Float64}
    x <= zero(T) && return throw(DomainError(x, "`x` must be nonnegative."))
    if x <= one(T)
        z = x * x
        a = z / 4
        pq = muladd(evalpoly(a, P1_k1(T)), inv(evalpoly(a, Q1_k1(T))), Y_k1(T))
        pq = muladd(pq * a, a, (a / 2 + 1))
        a = pq * x / 2
        pq = muladd(evalpoly(z, P2_k1(T)) / evalpoly(z, Q2_k1(T)), x, inv(x))
        return muladd(a, log(x), pq)
    else
        s = exp(-x / 2)
        a = muladd(evalpoly(inv(x), P3_k1(T)), inv(evalpoly(inv(x), Q3_k1(T))), Y2_k1(T)) * s / sqrt(x)
        return a * s
    end
end
"""
    besselk1x(x::T) where T <: Union{Float32, Float64}

Scaled modified Bessel function of the second kind of order one, ``K_1(x)*e^{x}``.
"""
function besselk1x(x::T) where T <: Union{Float32, Float64}
    x <= zero(T) && return throw(DomainError(x, "`x` must be nonnegative."))
    if x <= one(T)
        z = x * x
        a = z / 4
        pq = muladd(evalpoly(a, P1_k1(T)), inv(evalpoly(a, Q1_k1(T))), Y_k1(T))
        pq = muladd(pq * a, a, (a / 2 + 1))
        a = pq * x / 2
        pq = muladd(evalpoly(z, P2_k1(T)) / evalpoly(z, Q2_k1(T)), x, inv(x))
        return muladd(a, log(x), pq) * exp(x)
    else
        return muladd(evalpoly(inv(x), P3_k1(T)), inv(evalpoly(inv(x), Q3_k1(T))), Y2_k1(T)) / sqrt(x)
    end
end
#=
If besselk0(x) or besselk1(0) is equal to zero
this will underflow and always return zero even if besselk(x, nu)
is larger than the smallest representing floating point value.
In other words, for large values of x and small to moderate values of nu,
this routine will underflow to zero.
For small to medium values of x and large values of nu this will overflow and return Inf.
=#
"""
    besselk(x::T) where T <: Union{Float32, Float64}

Modified Bessel function of the second kind of order nu, ``K_{nu}(x)``.
"""
function besselk(nu, x::T) where T <: Union{Float32, Float64, BigFloat}
    T == Float32 ? branch = 20 : branch = 50
    if nu < branch
        return up_recurrence(x, besselk0(x), besselk1(x), nu)[1]
    else
        return besselk_large_orders(nu, x)
    end
end

"""
    besselk(x::T) where T <: Union{Float32, Float64}

Scaled modified Bessel function of the second kind of order nu, ``K_{nu}(x)*e^{x}``.
"""
function besselkx(nu::Int, x::T) where T <: Union{Float32, Float64}
    T == Float32 ? branch = 20 : branch = 50
    if nu < branch
        return up_recurrence(x, besselk0x(x), besselk1x(x), nu)[1]
    else
        return besselk_large_orders_scaled(nu, x)
    end
end
function besselk_large_orders(v, x::T) where T <: Union{Float32, Float64, BigFloat}
    S = promote_type(T, Float64)
    x = S(x)
    z = x / v
    zs = hypot(1, z)
    n = zs + log(z) - log1p(zs)
    coef = SQPIO2(S) * sqrt(inv(v)) * exp(-v*n) / sqrt(zs)
    p = inv(zs)
    p2  = v^2/fma(max(v,x), max(v,x), min(v,x)^2)

    return T(coef*Uk_poly_Kn(p, v, p2, T))
end
function besselk_large_orders_scaled(v, x::T) where T <: Union{Float32, Float64, BigFloat}
    S = promote_type(T, Float64)
    x = S(x)
    z = x / v
    zs = hypot(1, z)
    n = zs + log(z) - log1p(zs)
    coef = SQPIO2(S) * sqrt(inv(v)) * exp(-v*n + x) / sqrt(zs)
    p = inv(zs)
    p2  = v^2/fma(max(v,x), max(v,x), min(v,x)^2)

    return T(coef*Uk_poly_Kn(p, v, p2, T))
end


A = diagm([1, -0.5, 1])

ws = DgeesWs(3)

dgees!(ws, A)
println(ws.eigen_values)

dgees!(ws, A, >=, 1.0)
println(ws.eigen_values)


# lplagorder.m
# Luca Brugnolini 2014
# Based on Kilian
# This program selects the lag order for local projections using AIC, SIC , AICC and HCQ

function lplagorder(z,pbar,H)

# Inputs : data, maximum lag order to consider, IRFs horizons
# Outputs: lag-orders for H horizons selected by AIC and SIC (Hx1 vectors)

(T,K) = size(z);
t     = T-pbar;
paic  = zeros(H,1);
# psic  = zeros(H,1);                       # FOR SIC
# paicc = zeros(H,1);                       # FOR HURVICH AND TSAI
# phqc  = zeros(H,1);                       # FOR HQ
Ys = z[pbar+1:T,:];
Yt = z[pbar:T-1,:];                  # RHS variable of interest: y(t)
X = ones(T-pbar,1);                  # constant term

    for j=2:pbar
       X = [X z[pbar+1-j:T-j,:]];           # other RHS variables: y(t-1)~y(t-p+1)

    end

    for j=1:H                                          # loop for horizon h of IRF
        AIC = zeros(pbar,1);                       # the vector of AIC(p)
        # SIC = zeros(pbar,1);                     # the vector of SIC(p)
        # AICC = zeros(pbar,1);                    # FOR HURVICH AND TSAI
        # HQC = zeros(pbar, 1);                    # FOR HQ
        ys = Ys[j:end,:];                          # dependent variable: yt+s
        yt = Yt[1:end-j+1,:];                      # regressor of interest: yt

    for m=1:pbar                             # loop for lag order selection
        x = X[1:end-j+1,1:(m-1)*K+1];        # other independent variables
        Mx = eye(size(x,1))-x/(x'*x)*x';     # annhilation matrix **
        B1 = (yt'*Mx*yt)\(yt'*Mx*ys);        # IRF by Local Projection **
        U = Mx*ys - Mx*yt*B1;                # residual from LPs
        SIG = U'*U/t;                        # variance of errors
        # b = t/(t-(m*K+K+1));
        AIC[m]  = log(det(SIG)) + 2*(K^2*m)/t;                        # AIC statistic
        # SIC[m]  = log(det(SIG)) + (K^2*m)*log(t)/t;                 # SIC statistic
        # AICC[m] = t*(log(det(SIG))+K) + 2*b*(K^2*m+K*(K+1)/2);      # FOR HURVICH AND TSAI
        # HQC[m]  = log(det(SIG)) + 2*log(log(t))*K^2*m/t;            # FOR HQ
    end

        paic[j] = indmin(AIC)                      # paic is the lag-order by AIC
        # psic[j]  = indmin(SIC);                  # paic is the lag-order by AIC
        # paicc[j] = indmin(AICC);                 # FOR HURVICH AND TSAI
        # phqc[j]  = indmin(HQC);                  # FOR HQ

    end

return paic

end


module StatLearnTest
using OnlineStats, Distributions, Base.Test

function linearmodeldata(n, p, corr = 0)
    # linear model data with correlated predictors
    V = zeros(p, p)
    for j in 1:p, i in 1:p
        V[i, j] = corr^abs(i - j)
    end
    x = rand(MvNormal(ones(p), V), n)'
    β = vcat(1.:5, zeros(p-5))
    y = x*β + randn(n)
    (β, x, y)
end

@testset "StatLearn" begin
    n, p = 500, 5
    x = randn(n, p)
    β = collect(linspace(-1, 1, p))
    β_with_intercept = vcat(0.0, β)
    xβ = x*β

    generate(::LinearRegression, xβ) = xβ + randn(size(xβ, 1))
    generate(::L1Regression, xβ) = xβ + randn(size(xβ, 1))
    generate(::LogisticRegression, xβ) = [rand(Bernoulli(1 / (1 + exp(-η)))) for η in xβ]
    generate(::PoissonRegression, xβ) = [rand(Poisson(exp(η))) for η in xβ]
    generate(::QuantileRegression, xβ) = xβ + randn(size(xβ, 1))
    generate(::SVMLike, xβ) = [rand(Bernoulli(1 / (1 + exp(-η)))) for η in xβ]
    generate(::HuberRegression, xβ) = xβ + randn(size(xβ, 1))

    @testset "methods" begin
        y = x*β + randn(n)
        o = StatLearn(x, y)
        @test predict(o, x) ≈ x * o.β + o.β0
        @test coef(o) == vcat(o.β0, o.β)
    end
    @testset "loss" begin
        y = generate(LinearRegression(), xβ)
        o = StatLearn(x, y, LinearRegression())
        @test loss(o, x, y) ≈ .5 * mean(abs2(y - predict(o, x)))

        y = generate(L1Regression(), xβ)
        o = StatLearn(x, y, L1Regression())
        @test loss(o, x, y) ≈ mean(abs(y - predict(o, x)))

        y = generate(LogisticRegression(), xβ)
        o = StatLearn(x, y, LogisticRegression())
        η = o.β0 + x * o.β
        l = mean([-y[i] * η[i] + log(1.0 + exp(η[i])) for i in 1:length(η)])
        @test loss(o, x, y) ≈ l

        y = generate(PoissonRegression(), xβ)
        o = StatLearn(x, y, PoissonRegression())
        η = o.β0 + x * o.β
        # TODO
        # @test loss(o, x, y) ≈ mean(-y .* η + exp(η))

        y = generate(QuantileRegression(.7), xβ)
        o = StatLearn(x, y, QuantileRegression(.7))
        r = y - o.β0 - x * o.β
        @test loss(o, x, y) ≈ mean([r[i] * (o.model.τ - (r[i]<0)) for i in 1:n])

        y = generate(SVMLike(), xβ)
        o = StatLearn(x, y, SVMLike())
        η = o.β0 + x * o.β
        @test loss(o, x, y) ≈ mean([max(0.0, 1.0 - y[i] * η[i]) for i in 1:n])

        y = generate(HuberRegression(4.), xβ)
        o = StatLearn(x, y, HuberRegression(4.))
        δ = o.model.δ
        r = y - o.β0 - x * o.β
        v = [abs(r[i]) < δ? 0.5 * r[i]^2 : δ * (abs(r[i]) - 0.5 * δ) for i in 1:n]
        @test loss(o, x, y) ≈ mean(v)
    end
end

end #module


#==========================================================================================#

# TYPE

mutable struct Probit <: MLE

    sample::Microdata
    β::Vector{Float64}
    V::AbstractMatrix{Float64}

    Probit() = new()
end

#==========================================================================================#

# CONSTRUCTOR

function Probit(MD::Microdata)
    obj        = Probit()
    obj.sample = MD
    return obj
end

#==========================================================================================#

# ESTIMATION

function _fit!(obj::Probit, ::UnitWeights)

    y  = iszero.(getvector(obj, :response))
    x  = getmatrix(obj, :control)
    μ  = Array{Float64}(undef, length(y))
    xx = Array{Float64}(undef, size(x)...)

    p  = mean(y)
    p  = 1.0 / normpdf(norminvcdf(p))
    β₀ = lmul!(p, x \ y)

    function L(β::Vector)

        mul!(μ, x, β)
        ll = 0.0

        @inbounds for (yi, μi) in zip(y, μ)
            ll -= (yi ? normlogccdf(μi) : normlogcdf(μi))
        end

        return ll
    end

    function G!(g::Vector, β::Vector)

        mul!(μ, x, β)

        @inbounds for (i, (yi, μi)) in enumerate(zip(y, μ))
            ηi   = (yi ? (normcdf(μi) - 1.0) : normcdf(μi))
            μ[i] = - normpdf(μi) / ηi
        end

        mul!(g, transpose(x), μ)
    end

    function LG!(g::Vector, β::Vector)

        mul!(μ, x, β)
        ll = 0.0

        @inbounds for (i, (yi, μi)) in enumerate(zip(y, μ))
            ηi   = (yi ? (normcdf(μi) - 1.0) : normcdf(μi))
            μ[i] = - normpdf(μi) / ηi
            ll  -= (yi ? log(- ηi) : log(ηi))
        end

        mul!(g, transpose(x), μ)

        return ll
    end

    function H!(h::AbstractMatrix, β::Vector)

        mul!(μ, x, β)

        @inbounds for (i, (yi, μi)) in enumerate(zip(y, μ))
            ηi   = (yi ? (normcdf(μi) - 1.0) : normcdf(μi))
            ηi   = normpdf(μi) / ηi
            μ[i] = muladd(μi, ηi, abs2(ηi))
        end

        xx .= x .* μ
        mul!(h, transpose(x), xx)
    end

    res = optimize(TwiceDifferentiable(L, G!, LG!, H!, β₀), β₀, Newton())

    if Optim.converged(res)
        obj.β = Optim.minimizer(res)
    else
        throw("likelihood maximization did not converge")
    end
end

function _fit!(obj::Probit, w::AbstractWeights)

    y  = getvector(obj, :response)
    x  = getmatrix(obj, :control)
    μ  = Array{Float64}(undef, length(y))
    xx = Array{Float64}(undef, size(x)...)

    p  = mean(y)
    p  = 1.0 / normpdf(norminvcdf(p))
    β₀ = lmul!(p, x \ y)

    function L(β::Vector)

        mul!(μ, x, β)
        ll = 0.0

        @inbounds for (yi, μi, wi) in zip(y, μ, w)
            ll -= wi * (yi ? normlogccdf(μi) : normlogcdf(μi))
        end

        return ll
    end

    function G!(g::Vector, β::Vector)

        mul!(μ, x, β)

        @inbounds for (i, (yi, μi, wi)) in enumerate(zip(y, μ, w))
            ηi   = (yi ? (normcdf(μi) - 1.0) : normcdf(μi))
            μ[i] = - wi * normpdf(μi) / ηi
        end

        mul!(g, transpose(x), μ)
    end

    function LG!(g::Vector, β::Vector)

        mul!(μ, x, β)
        ll = 0.0

        @inbounds for (i, (yi, μi, wi)) in enumerate(zip(y, μ, w))
            ηi   = (yi ? (normcdf(μi) - 1.0) : normcdf(μi))
            μ[i] = - wi * normpdf(μi) / ηi
            ll  -= wi * (yi ? log(- ηi) : log(ηi))
        end

        mul!(g, transpose(x), μ)

        return ll
    end

    function H!(h::AbstractMatrix, β::Vector)

        mul!(μ, x, β)

        @inbounds for (i, (yi, μi, wi)) in enumerate(zip(y, μ, w))
            ηi   = (yi ? (normcdf(μi) - 1.0) : normcdf(μi))
            ηi   = normpdf(μi) / ηi
            μ[i] = wi * muladd(μi, ηi, abs2(ηi))
        end

        xx .= x .* μ
        mul!(h, transpose(x), xx)
    end

    res = optimize(TwiceDifferentiable(L, G!, LG!, H!, β₀), β₀, Newton())

    if Optim.converged(res)
        obj.β = Optim.minimizer(res)
    else
        throw("likelihood maximization did not converge")
    end
end

#==========================================================================================#

# SCORE (DERIVATIVE OF THE LIKELIHOOD FUNCTION)

function score(obj::Probit)

    y = getvector(obj, :response)
    x = getmatrix(obj, :control)
    v = x * obj.β

    @inbounds for (i, (yi, vi)) in enumerate(zip(y, v))
        ηi   = (iszero(yi) ? (normcdf(vi) - 1.0) : normcdf(vi))
        v[i] = normpdf(vi) / ηi
    end

    return Diagonal(v) * x
end

# EXPECTED JACOBIAN OF SCORE × NUMBER OF OBSERVATIONS

function jacobian(obj::Probit, ::UnitWeights)

    y = getvector(obj, :response)
    x = getmatrix(obj, :control)
    v = x * obj.β

    @inbounds for (i, (yi, vi)) in enumerate(zip(y, v))
        ηi   = (iszero(yi) ? (normcdf(vi) - 1.0) : normcdf(vi))
        ηi   = normpdf(vi) / ηi
        v[i] = muladd(vi, ηi, abs2(ηi))
    end

    return - crossprod(x, v)
end

function jacobian(obj::Probit, w::AbstractWeights)

    y = getvector(obj, :response)
    x = getmatrix(obj, :control)
    v = x * obj.β

    @inbounds for (i, (yi, vi, wi)) in enumerate(zip(y, v, w))
        ηi   = (iszero(yi) ? (normcdf(vi) - 1.0) : normcdf(vi))
        ηi   = normpdf(vi) / ηi
        v[i] = wi * muladd(vi, ηi, abs2(ηi))
    end

    return - crossprod(x, v)
end

#==========================================================================================#

# LINEAR PREDICTOR

function linear_predictor(obj::Probit, MD::Microdata)
    (getnames(obj, :control) != getnames(MD, :control)) && throw("missing variables")
    getmatrix(MD, :control) * obj.β
end

# FITTED VALUES

predict(obj::Probit, MD::Microdata) = normcdf.(linear_predictor(obj, MD))

# DERIVATIVE OF FITTED VALUES

function jacobexp(obj::Probit)
    x  = getmatrix(obj, :control)
    v  = x * obj.β
    v .= normpdf.(v)
    return Diagonal(v) * x
end

#==========================================================================================#

# UTILITIES

coefnames(obj::Probit) = getnames(obj, :control)

# LIKELIHOOD FUNCTION

function _loglikelihood(obj::Probit, ::UnitWeights)

    y  = getvector(obj, :response)
    μ  = linear_predictor(obj)
    ll = 0.0

    @inbounds for (yi, μi) in zip(y, μ)
        ll += (iszero(yi) ? normlogccdf(μi) : normlogcdf(μi))
    end

    return ll
end

function _loglikelihood(obj::Probit, w::AbstractWeights)

    y  = getvector(obj, :response)
    μ  = linear_predictor(obj)
    ll = 0.0

    @inbounds for (yi, μi, wi) in zip(y, μ, w)
        ll += wi * (iszero(yi) ? normlogccdf(μi) : normlogcdf(μi))
    end

    return ll
end

# LIKELIHOOD FUNCTION UNDER NULL MODEL

function _nullloglikelihood(obj::Probit, w::AbstractWeights)
    y = getvector(obj, :response)
    μ = mean(y, w)
    return nobs(obj) * (μ * log(μ) + (1.0 - μ) * log(1.0 - μ))
end

# DEVIANCE

deviance(obj::Probit) = - 2.0 * _loglikelihood(obj, getweights(obj))

# DEVIANCE UNDER NULL MODEL

nulldeviance(obj::Probit) = - 2.0 * _nullloglikelihood(obj, getweights(obj))


# The basis definition
@variables u[1:2]
basis = Basis([polynomial_basis(u, 5); sin.(u); cos.(u)], u)

function pendulum(u, p, t)
    x = u[2]
    y = -9.81sin(u[1]) - 0.1u[2]^3 - 0.2 * cos(u[1])
    return [x;y]
end

u0 = [0.99π; -1.0]
tspan = (0.0, 20.0)
dt = 0.1
prob = ODEProblem(pendulum, u0, tspan)
sol = solve(prob, Tsit5(), saveat=dt)

X = sol[:,:]
t = sol.t

DX = similar(sol[:,:])

for (i, xi) in enumerate(eachcol(sol[:,:]))
    DX[:,i] = pendulum(xi, [], 0.0)
end

##
@testset "Ideal data" begin
    dd_prob = ContinuousDataDrivenProblem(
        sol
        )

    opts = [
    STLSQ(1e-2), ADMM(1e-2, 1e-2), SR3(1e-2, SoftThreshold()),
    SR3(1e-4)
    ]

res = solve(dd_prob, basis, opts[1], maxiter = 10000)

    for opt in opts
        m = DataDrivenDiffEq.metrics(res)
        @test all(m[:L₂] .< 1e-1*size(X, 2))
        @test all(m[:AIC] .>= 200) # Perfect Match or close
        @test all(m[:R²] .>= 0.9)
    end
end

Random.seed!(1234)
X = X .+ 1e-1*randn(size(X))

@testset "Noisy data" begin

    dd_prob_noisy = ContinuousDataDrivenProblem(
        X, t, GaussianKernel()
        )

    opts = [
            STLSQ(1e-1:5e-1:1e3), ADMM(1e-1:5e-1:1e5, 1.0), SR3(1e-1:5e-1:1e5, 2.0, SoftThreshold()),
            SR3((1e-1:5e-1:1e5), 2.0, HardThreshold()), SR3(1e-3:5e-1:1e5, 2.0, ClippedAbsoluteDeviation())
        ]

    for opt in opts
        res = solve(dd_prob_noisy, basis, opt, maxiter = 50000, denoise = true, normalize = true)
        m = DataDrivenDiffEq.metrics(res)
        @test all(m[:L₂]./size(X,2) .< [30.; 800])
        @test all(-500.0 .< m[:AIC])
        @test all(m[:R²] .>= 0.9)
    end

end


 # module ComplexDPR1

import Base: *, getindex, size, Matrix
import LinearAlgebra: eigen, eigvals, inv, transpose, mul!

T=Complex{Float64}

# export DPR1

# Define general Complex Diagonal+Rank-One Type D+ρ u*transpose(u)
struct CSymDPR1{T} <: AbstractMatrix{T}
    D::Vector{T} # diagonal
    u::Vector{T} # rank one, length n
    ρ::T  # rho
end

# Define its size
size(A::CSymDPR1, dim::Integer) = length(A.D)
size(A::CSymDPR1) = size(A,1), size(A,1)

# Index into a CSymDPR1
function getindex(A::CSymDPR1,i::Integer,j::Integer)
    Aij = A.ρ * (A.u[i] * A.u[j])
    return i == j ? A.D[i] + Aij : Aij
end # getindex

Matrix(A::CSymDPR1)= Matrix(Symmetric(BLAS.syr!('U',A.ρ,A.u,diagm(0=>A.D))))
transpose(A::CSymDPR1{T}) where T = A

# Define CSymDPR1 x vector in O(n) operations
function *(A::CSymDPR1{T}, x::Vector{T}) where T
    dotd= T==ComplexF64 ? BLAS.dotu : BLAS.dot
    y=Diagonal(A.D)*x
    BLAS.axpy!(A.ρ*dotd(A.u, x),A.u,y)
    # return y
end
function mul!(y::Vector{T}, A::CSymDPR1{T}, x::Vector{T}) where T
    dotd= T==ComplexF64 ? BLAS.dotu : BLAS.dot
    mul!(y,Diagonal(A.D),x)
    BLAS.axpy!(A.ρ*dotd(A.u, x),A.u,y)
end

# CSymDPR1 x Matrix
function *(A::CSymDPR1{T}, x::Matrix{T}) where T
    y=Diagonal(A.D)*x
    BLAS.ger!(A.ρ,A.u,conj(transpose(x)*A.u),y)
end
function mul!(y::Matrix{T}, A::CSymDPR1{T}, x::Matrix{T}) where T
    mul!(y,Diagonal(A.D),x)
    BLAS.ger!(A.ρ,A.u,conj(transpose(x)*A.u),y)
end

function inv(A::CSymDPR1{T}) where {T<:T}
    xₕ=A.u ./A.D
    Δₕ=one(T)./A.D
    ρₕ=-A.ρ/(one(T)+BLAS.dotu(xₕ,A.u)*A.ρ)
    return CSymDPR1(Δₕ,xₕ,ρₕ)
end

function inv(A::CSymDPR1{T}) where {T<:Float64}
    xₕ=A.u ./A.D
    Δₕ=one(T)./A.D
    ρₕ=-A.ρ/(one(T)+BLAS.dot(xₕ,A.u)*A.ρ)
    return CSymDPR1(Δₕ,xₕ,ρₕ)
end

function xtAx(A::CSymDPR1{T}, x::Vector{T}) where {T}
    xdx = zero(T)
    xu  = zero(T)
    @inbounds for i in 1:length(x)
        xdx += A.D[i]*x[i]^2
        xu  += x[i]*A.u[i]
    end
    return xdx + A.ρ*xu^2
end

function xtAx(A::CSymDPR1{T}, x::Vector{T}, μ::T) where {T}
    xdx = zero(T)
    xu  = zero(T)
    @inbounds for i in 1:length(x)
        xdx += (A.D[i]-μ)*x[i]^2
        xu  += x[i]*A.u[i]
    end
    return xdx + A.ρ*xu^2
end

function xtAxc(A::CSymDPR1{T}, x::Vector{T}, μ::T) where {T}
    xdx = zero(T)
    xu  = zero(T)
    xuc = zero(T)
    @inbounds for i in 1:length(x)
        xdx += (A.D[i]-μ)*x[i]*conj(x[i])
        xu  += A.u[i]*x[i]
        xuc += A.u[i]*conj(x[i])
    end
    return xdx + A.ρ*xu*xuc
end

function xtAxc(A::CSymDPR1{T}, x::Vector{T}) where {T}
    xdx = zero(T)
    xu  = zero(T)
    xuc = zero(T)
    @inbounds for i in 1:length(x)
        xdx += A.D[i]*x[i]*conj(x[i])
        xu  += A.u[i]*x[i]
        xuc += A.u[i]*conj(x[i])
    end
    return xdx + A.ρ*xu*xuc
end

function dpr1vector!(v::Vector{T}, A::CSymDPR1{T}, λ::T) where  T
    n = length(A.D)
    @inbounds for l in 1:n
        τ=A.D[l] - λ
        if τ==zero(T)
            for k=1:n
                v[k]=zero(T)
            end
            v[l]=one(T)
            return v
        end
        v[l] = A.u[l]/τ #  (A.D[l] - λ)
    end
    normalize!(v)
    # v
end

function mrqi!(z::Vector{T}, Du::Vector{T}, A::CSymDPR1{T}, λ::T) where T
    # Modified Rayleigh Quotient Iteration
    # Returns z=inv(A.D-λ)*A.u if γ is small or too large
    # and z=inv(A.D-λI)*z otherwise
    n=length(z)
    @inbounds for l in 1:n
        τ=A.D[l] - λ
        if τ==zero(T)
            for k=1:n
                z[k]=zero(T)
            end
            z[l]=one(T)
            return
        end
        Du[l] = A.u[l]/τ #  (A.D[l] - λ)
    end
    dotd= (T==Float64) ? BLAS.dot : BLAS.dotu
    γ=A.ρ*dotd(z,Du)/(one(T)+A.ρ*dotd(A.u,Du))
    if abs(γ)<1e-4 || abs(γ)>1e4 || isnan(γ)
        @inbounds for l=1:n
            z[l]=Du[l]
        end
    else
        @inbounds for l=1:n
            z[l]=z[l]/(A.D[l]-λ)-γ*Du[l]
        end
    end
    normalize!(z)
end

function rqi(z::Vector{T}, z₁::Vector{T}, τ::Vector{T}, A::CSymDPR1{T} ,λ::T) where T
    # Rayleigh Quotient Iteration
    # Returns λ=z'*A*z/z'*z
    # and z=inv(A-λI)*z otherwise
    dotd= (T==Float64) ? BLAS.dot : BLAS.dotu
    if count(isequal(λ),A.D)>0
        return λ
    end
    n=length(A.D)
    dpr1vector!(z,A,λ)
    δ = xtAxc(A, z)
    if count(isequal(δ),A.D)>0
        λ=δ
        return λ
    else
        copy!(z₁,A.u)
        copy!(τ,A.D)
        τ.-=δ
        z₁./=τ
        μ=dotd(A.u,z₁)
        γ=-A.ρ/(one(T)+μ*A.ρ)
        ξ=dotd(z₁,z)
        γ*=ξ
        z./=τ
        BLAS.axpy!(γ,z₁,z)
        return xtAxc(A, z)/BLAS.dotc(z,z)
    end
end

function eigvals(A₁::CSymDPR1)
    # A is complex and unreduced, that is, A.u.!=0 and
    # all elements of A.D are different.
    # It is better to use z^T A z / z^T z (modified RQI) instead
    # of standard RQI using z^*
    # This Standard Rayleigh Quotient Iteration, but solving the system
    # (multiplication by the inverse) is replaced by the CSymDPR1()
    # eigenvector formula

    # Preliminaries
    A=deepcopy(A₁)
    n = length(A.D)
    m = n

    T = typeof(A.D[1])

    # Eigenvalues
    λ = Vector{T}(undef,n)
    # Auxiliary arrays
    d = Vector{T}(undef,n)
    z = Vector{T}(undef,n)
    Du = Vector{T}(undef,n)
    AD=Vector{T}(undef,n)
    dotd= (T==Float64) ? BLAS.dot : BLAS.dotu
    # Tolerance
    tol = 100.0*eps()
    # Maxiter
    maxiter = 101

    for l in 1:n
        copyto!(AD,A.D)
        # Shift to the absolutely largest pole
        t=1
        μ = A.D[t]
        for i in 1:m
            A.D[i] -= μ
        end
        for i=1:m
            z[i]=zero(T)
        end
        z[t]=one(T)
        k = 1
        δ = one(T)
        γ = zero(T)
        while k < maxiter && abs(δ)>tol
            # use z^T
            ν = dotd(z, z)
            δ = xtAx(A, z)/ν
            γ += δ
            mrqi!(z, Du, A, δ)
            for i in 1:m
                A.D[i] -= δ
            end

            k += 1
        end
        if k==maxiter
            println(l," ",abs(δ)," ",tol)
        end
        λ[l] = μ + γ

        # Deflation - eliminating absolutely largest pole
        @inbounds for i in 1:(t - 1)
            d[i] = A.u[i]/sqrt(one(T) - γ/(AD[i] - μ))
            if isnan(d[i])
                println(l," nan")
                d[i]=zero(T)
            end
        end
        @inbounds for i in (t + 1):m
            d[i-1] = A.u[i]/sqrt(one(T) - γ/(AD[i] - μ))
            if isnan(d[i-1])
                d[i-1]=zero(T)
            end
        end
        m -= 1
        deleteat!(A.D,t)
        deleteat!(AD,t)
        deleteat!(z,t)
        deleteat!(Du,t)
        copyto!(A.D,AD)
        deleteat!(A.u,t)
        for i=1:m
            A.u[i]=d[i]
        end
    end
    # Two steps of RQI on the original CSymDPR1 matrix
    # Generate workspace for each thread

    nthreads=Threads.nthreads()
    # nthreads=1
    z₀ = [Vector{T}(undef,n) for i = 1:nthreads]
    z₁ = [Vector{T}(undef,n) for i = 1:nthreads]
    τ = [Vector{T}(undef,n) for i = 1:nthreads]
    # Threaded loop
    Threads.@threads for l=1:n
        tid=Threads.threadid()
        μ=λ[l]
        λ[l]=rqi(z₀[tid],z₁[tid],τ[tid],A₁,μ)
    end
    return λ
end

# Scaling
function scale!(X::CauchyLike{T}) where T
    n=length(X.x)
    dotd= (T==Float64) ? BLAS.dot : BLAS.dotu
    a=[Vector{T}(undef,n) for i=1:Threads.nthreads()]
    Ψ=Vector{T}(undef,Threads.nthreads())
    rzero=count(iszero,X.r)
    Threads.@threads for i=1:n
        tid=Threads.threadid()
        getindex!(a[tid],X,:,i)
        Ψ[tid]=dotd(a[tid],a[tid])
        if !isfinite(Ψ[tid]) || count(iszero,a[tid])>rzero
            X.s[i]=zero(T)
        else
            X.s[i]/=conj(sqrt(Ψ[tid]))
        end
    end
end

function eigen(A::CSymDPR1{T}) where T
    # A is complex and unreduced, that is, A.u.!=0 and
    # all elements of A.D are different.
    # eig() is just eigvals with normalized eigenvectors returned
    # as Cauchy-like matrix
    Λ=eigvals(A)
    n=length(A.D)
    # Boley-Golub Lemma
    z=Vector{T}(undef,n)
    Threads.@threads for i=1:n
        δ=A.D[i]
        τ=Λ[i]-δ
        for j=1:i-1
            τ*=(Λ[j]-δ)/(A.D[j]-δ)
        end
        for j=i+1:n
            τ*=(Λ[j]-δ)/(A.D[j]-δ)
        end
        τ/=A.ρ
        τ=sqrt(τ)
        if isnan(τ)
            # println(" nan ")
            z[i]=A.u[i]
        else
            if T==Float64
                z[i]=copysign(τ,A.u[i])
            else
                z[i]=copysign(real(τ),real(A.u[i]))+im*copysign(imag(τ),imag(A.u[i]))
            end
        end
    end

    nz=norm(A.u-z,Inf)
    if nz>100*eps()
        # println("warning: nz=",nz)
    end
    X=CauchyLike(A.D,Λ,z,ones(T,length(A.u)))
    scale!(X)
    return Λ,X
end

function traceX(S::CauchyLike{T},s::Int) where T
    n=div(length(S.x),2)
    G=Matrix{T}(undef,2n,2s)
    a=[Vector{T}(undef,2n) for i=1:Threads.nthreads()]
    b=[Vector{T}(undef,2n) for i=1:Threads.nthreads()]
    Threads.@threads for i=1:2n
        tid=Threads.threadid()
        getindex!(a[tid],S,:,i)
        G[i,:].=view(a[tid],[1:s;n+1:n+s])
    end
    Y=CauchyLikeS(S.y,G,conj(G))
    # trace(X)=trace(SYS')=trace(S'(SY))
    S₁=S*Y
    traceX=zeros(T,Threads.nthreads())
    Threads.@threads for i=1:2n
        tid=Threads.threadid()
        getindex!(a[tid],S,:,i)
        getindex!(b[tid],S₁,:,i)
        traceX[tid]+=BLAS.dotc(a[tid],b[tid])
    end
    return sum(traceX)
end


using CurveProximityQueries
using ConvexBodyProximityQueries
using IntervalArithmetic
using Plots
using StaticArrays

curve = Bernstein([
    [-0.6, 0.0],
    [-0.6, 0.0],
    [0.1, -0.4],
    [0.4, 1.6],
    [-0.2, -1.6],
    [-0.1, 0.4],
    [0.8, 0.0],
    [0.8, 0.0],
])
#ell = Ellipse(curve, 0.2..0.8)

curve2 = Bernstein([[0.6, 0.6], [0.4, 1.0], [0.2, 0.2], [0.0, 0.6]])
#ell2 = Ellipse(curve2, 0.2..0.9)

pts = (curve(0.5), curve2(0.55))

points = curve.(collect(0.0:0.01:1.0))
points2 = curve2.(collect(0.0:0.01:1.0))

plot(first.(points), last.(points), label = "1")
plot!(first.(points2), last.(points2), label = "2")

closest = closest_points(curve,curve2)
scatter!([i for i in first.(closest)], [i for i in last.(closest)])
# plot!(curve, 0.2..0.8,
#       linewidth=2.0,
#       linecolor=:black,
#      )
# plot!(curve, 0.8..1.0,
#       linewidth=2.0,
#       linestyle=:dash,
#       linecolor=Gray(0.5),
#      )
# scatter!(curve(0.2), markersize=4, markercolor=:black)
# scatter!(curve(0.8), markersize=4, markercolor=:black)
#
# plot!(curve2, 0.0..0.2,
#       linewidth=2.0,
#       linestyle=:dash,
#       linecolor=Gray(0.5),
#      )
# plot!(curve2, 0.2..0.9,
#       linewidth=2.0,
#       linecolor=:black,
#      )
# plot!(curve2, 0.9..1.0,
#       linewidth=2.0,
#       linestyle=:dash,
#       linecolor=Gray(0.5),
#      )
# scatter!(curve2(0.2), markersize=4, markercolor=:black)
# scatter!(curve2(0.9), markersize=4, markercolor=:black)
#
# plot!(pts,
#       linestyle=:dash,
#       markersize=8.0,
#       linewidth=2.0,
#      )
# savefig("out/fig5b.pdf")


using Rubin
using Tests
using Elliptic
using HypergeometricFunctions
using Polylogarithms
using SpecialFunctions

@test integrate(cot(x)^4, x) == :(x+-1//3*cot(x)^3+cot(x))
@test integrate(x^-4*(1+x^2)^-1, x) == :(x^-1+-1//3*x^-3+arctan(x))
@test integrate(x^-1//2*(x+x^2), x) == :(2//3*x^3//2+2//5*x^5//2)
@test integrate(cos(x), x) == :(sin(x))
@test integrate(x*exp(x^2), x) == :((1/2)*exp(x^2))
@test integrate(sec(x)^2*tan(x), x) == :((1/2)*sec(x)^2)
@test integrate(x*(1+x^2)^(1/2), x) == :(1//3*(1+x^2)^3//2)
@test integrate(exp(x)*sin(x), x) == :((1/2)*exp(x)*sin(x)+-1//2*cos(x)*exp(x))
@test integrate(csc(x)^2*sin(x)^-2*cos(x), x) == :(-1//3*csc(x)^3)
@test integrate(sin(exp(x)), x) == :(sinintegral(exp(x)))
@test integrate(y^-1*sin(y), y) == :(sinintegral(y))
@test integrate(exp(x)+sin(x), x) == :(-1*cos(x)+exp(x))
@test integrate(2*x^2*exp(x^2)+exp(x^2), x) == :(x*exp(x^2))
@test integrate((x+exp(x))^2, x) == :((1/2)*exp(2x)+-2*exp(x)+1//3*x^3+2*x*exp(x))
@test integrate(x^2+2*exp(x)+exp(2x), x) == :((1/2)*exp(2x)+2*exp(x)+1//3*x^3)
@test integrate(cos(x)*sin(x), x) == :((1/2)*sin(x)^2)
@test integrate(x*exp(x^2), x) == :((1/2)*exp(x^2))
@test integrate(x*(1+x^2)^(1/2), x) == :(1//3*(1+x^2)^3//2)
@test integrate((1+exp(x))^-1*exp(x), x) == :(log(1+exp(x)))
@test integrate(x^3//2, x) == :(2//5*x^5//2)
@test integrate(cos(3+2x), x) == :((1/2)*sin(3+2x))
@test integrate(2*y*z*exp(2x), x) == :(y*z*exp(2x))
@test integrate(cos(exp(x))^2*exp(x)*sin(exp(x)), x) == :(-1//3*cos(exp(x))^3)
@test integrate(x*(1+x)^(1/2), x) == :(-2//3*(1+x)^3//2+2//5*(1+x)^5//2)
@test integrate((-1+x^4)^-1, x) == :(-1//2*arctan(x)+-1//2*arctanh(x))
@test integrate((2+3*exp(2x))^-1*exp(x), x) == :(1//6*6^(1/2)*arctan((1/2)*6^(1/2)*exp(x)))
@test integrate((A+B*exp(4x))^-1*exp(2x), x) == :((1/2)*A^-1//2*B^-1//2*arctan(A^-1//2*B^(1/2)*exp(2x)))
@test integrate((1+exp(x))^-1*exp(1+x), x) == :(ℯ*log(1+exp(x)))
@test integrate(10^x*exp(x), x) == :((10ℯ)^x*(1+log(10))^-1)
@test integrate(x^3*sin(x^2), x) == :((1/2)*sin(x^2)+-1//2*x^2*cos(x^2))
@test integrate(x^7*(1+x^12)^-1, x) == :(-1//12*log(1+x^4)+1//24*log(1+x^8+-1*x^4)+-1//12*3^(1/2)*arctan(1//3*3^(1/2)*(1+-2*x^4)))
@test integrate(x^(3a)*sin(x^(2a)), x) == :(-1//4*im*a^-1*x^(1+3a)*(im*x^(2a))^(-1//2*a^-1*(1+3a))*SpecialFunctions.gamma(3//2+(1/2)*a^-1,im*x^(2a))+1//4*im*a^-1*x^(1+3a)*(-1*im*x^(2a))^(-1//2*a^-1*(1+3a))*SpecialFunctions.gamma(3//2+(1/2)*a^-1,-1*im*x^(2a)))
@test integrate(cos(x^(1/2)), x) == :(2*cos(x^(1/2))+2*x^(1/2)*sin(x^(1/2)))
@test integrate(x*(1+x)^(1/2), x) == :(-2//3*(1+x)^3//2+2//5*(1+x)^5//2)
@test integrate((x^(1/2)+x^1//3)^-1, x) == :(-6*log(1+x^1//6)+-3*x^1//3+2*x^(1/2)+6*x^1//6)
@test integrate(((3+2x)^-1*(1+x))^(1/2), x) == :((1/2)*(1+x)^(1/2)*(3+2x)^(1/2)+-1//4*2^(1/2)*arcsinh(2^(1/2)*(1+x)^(1/2)))
@test integrate(x^4*(1+-1*x^2)^-5//2, x) == :(-1*x*(1+-1*x^2)^-1//2+1//3*x^3*(1+-1*x^2)^-3//2+arcsin(x))
@test integrate(x^(1/2)*(1+x)^5//2, x) == :(-5//64*arcsinh(x^(1/2))+1//4*x^3//2*(1+x)^5//2+5//24*x^3//2*(1+x)^3//2+5//32*x^3//2*(1+x)^(1/2)+5//64*x^(1/2)*(1+x)^(1/2))
@test integrate(x^4*(1+-1*x^2)^-5//2, x) == :(-1*x*(1+-1*x^2)^-1//2+1//3*x^3*(1+-1*x^2)^-3//2+arcsin(x))
@test integrate((1+-1*y^2)^-1*(A^2+B^2+-1*B^2*y^2)^(1/2), y) == :(A*arctanh(A*y*(A^2+B^2+-1*B^2*y^2)^-1//2)+B*arctan(B*y*(A^2+B^2+-1*B^2*y^2)^-1//2))
@test integrate(sin(x)^2, x) == :((1/2)*x+-1//2*cos(x)*sin(x))
@test integrate((1+cos(x))^-1, x) == :((1+cos(x))^-1*sin(x))
@test integrate(x*exp(x), x) == :(-1*exp(x)+x*exp(x))
@test integrate(x*(1+x)^-2*exp(x), x) == :((1+x)^-1*exp(x))
@test integrate((1+2*x^2)*exp(x^2), x) == :(x*exp(x^2))
@test integrate(exp(x^2), x) == :((1/2)*pi^(1/2)*erfi(x))
@test integrate(x^-1*exp(x), x) == :(expintegralei(x))
@test integrate(x*(1+x^3)^-1, x) == :(-1//3*log(1+x)+1//6*log(1+x^2+-1x)+-1//3*3^(1/2)*arctan(1//3*3^(1/2)*(1+-2x)))
@test integrate((A^4+x^2*(B^2+-1*A^2)+-1*A^2*B^2)^-1, x) == :(A^-1*(A^2+-1*B^2)^-1*arctanh(x*A^-1))
@test integrate(x*log(x), x) == :(-1//4*x^2+(1/2)*x^2*log(x))
@test integrate(x^2*arcsin(x), x) == :(-1//9*(1+-1*x^2)^3//2+1//3*(1+-1*x^2)^(1/2)+1//3*x^3*arcsin(x))
@test integrate((1+x^2+2x)^-1, x) == :(-1*(1+x)^-1)
@test integrate((1+log(x))^-2*log(x), x) == :(x*(1+log(x))^-1)
@test integrate(x^-1*(1+log(x)^2)^-1, x) == :(arctan(log(x)))
@test integrate(log(x)^-1, x) == :(logintegral(x))
@test integrate(x*(cos(x)+sin(x)), x) == :(x*sin(x)+-1*x*cos(x)+cos(x)+sin(x))
@test integrate((x+exp(x))*exp(-1x), x) == :(x+-1*exp(-1x)+-1*x*exp(-1x))
@test integrate(x*(1+exp(x))^2, x) == :((1/2)*x^2+-2*exp(x)+-1//4*exp(2x)+(1/2)*x*exp(2x)+2*x*exp(x))
@test integrate(x*cos(x), x) == :(x*sin(x)+cos(x))
@test integrate(cos(x^(1/2)), x) == :(2*cos(x^(1/2))+2*x^(1/2)*sin(x^(1/2)))
@test integrate(x*cos(x), x) == :(x*sin(x)+cos(x))
@test integrate(x*log(x)^2, x) == :(1//4*x^2+(1/2)*x^2*log(x)^2+-1//2*x^2*log(x))
@test integrate((1+sin(x)^3)*cos(x), x) == :(1//4*sin(x)^4+sin(x))
@test integrate(x^-1*(1+log(x)^2)^-1, x) == :(arctan(log(x)))
@test integrate((1+arcsin(x)^2)^-1*(1+-1*x^2)^-1//2, x) == :(arctan(arcsin(x)))
@test integrate((cos(x)+sin(x))^-1*sin(x), x) == :((1/2)*x+-1//2*log(cos(x)+sin(x)))
@test integrate(-1*(1+-1*y^2)^-1*(A^2+B^2*(1+-1*y^2))^(1/2), y) == :(-1*A*arctanh(A*y*(A^2+B^2+-1*B^2*y^2)^-1//2)+-1*B*arctan(B*y*(A^2+B^2+-1*B^2*y^2)^-1//2))
@test integrate(B^-1*(1+-1*B^-2*sin(z)^2*(A^2+B^2))^-1*cos(z)^2*(-1*A^2+-1*B^2), z) == :(-1*A*arctanh(A*B^-1*tan(z))+-1*B*z)
@test integrate(B^-1*(1+w^2)^-2*(1+-1*B^-2*w^2*(1+w^2)^-1*(A^2+B^2))^-1*(-1*A^2+-1*B^2), w) == :(-1*A*arctanh(A*w*B^-1)+-1*B*arctan(w))
@test integrate(-1*B*(1+w^2)^-1*(B^2+-1*A^2*w^2)^-1*(A^2+B^2), w) == :(-1*A*arctanh(A*w*B^-1)+-1*B*arctan(w))
@test integrate(x^4*(1+-1*x^2)^-5//2, x) == :(-1*x*(1+-1*x^2)^-1//2+1//3*x^3*(1+-1*x^2)^-3//2+arcsin(x))
@test integrate(cos(y)^-4*sin(y)^4, y) == :(y+-1*tan(y)+1//3*tan(y)^3)
@test integrate(z^4*(1+z^2)^-1, z) == :(-1z+1//3*z^3+arctan(z))
@test integrate((1+2*x^2)*exp(x^2), x) == :(x*exp(x^2))
@test integrate((1+x^2)^-2*(1+x^3+2*x^6+4*x^2+5*x^4)*exp(x^2), x) == :(x*exp(x^2)+(2+2*x^2)^-1*exp(x^2))
@test integrate(exp(-1)*exp(-1x), x) == :(-1*exp(-1+-1x))
@test integrate((x+x^-1)*log(x), x) == :((1/2)*log(x)^2+-1//4*x^2+(1/2)*x^2*log(x))
@test integrate(x*(1+x^4)^-1, x) == :((1/2)*arctan(x^2))
@test integrate(x^5*(1+x^4)^-1, x) == :((1/2)*x^2+-1//2*arctan(x^2))
@test integrate((1+tan(x)^2)^-1, x) == :((1/2)*x+(1/2)*cos(x)*sin(x))
@test integrate(x^4*(1+-1*x^2)^-5//2, x) == :(-1*x*(1+-1*x^2)^-1//2+1//3*x^3*(1+-1*x^2)^-3//2+arcsin(x))
@test integrate(-1*x^2*(1+-1*x^2)^-3//2, x) == :(-1*x*(1+-1*x^2)^-1//2+arcsin(x))
@test integrate(exp(x)*sin(x), x) == :((1/2)*exp(x)*sin(x)+-1//2*cos(x)*exp(x))
@test integrate(x^-1, x) == :(log(x))
@test integrate((1+sec(t)^2+3*tan(t))^-1*sec(2t), t) == :(-1*(2+2*tan(t))^-1+-1//4*log(cos(t)+sin(t))+-1//12*log(-1*sin(t)+cos(t))+1//3*log(2*cos(t)+sin(t)))
@test integrate(sec(x)^-2, x) == :((1/2)*x+(1/2)*cos(x)*sin(x))
@test integrate(x^-1//2*(1+x^2), x) == :(2*x^(1/2)+2//5*x^5//2)
@test integrate(x*(5+x^2+2x)^-1//2, x) == :((5+x^2+2x)^(1/2)+-1*arcsinh(1/2+(1/2)*x))
@test integrate(sin(x)^2*cos(x), x) == :(1//3*sin(x)^3)
@test integrate((1+exp(x))^-1*exp(x), x) == :(log(1+exp(x)))
@test integrate((1+exp(x))^-1*exp(2x), x) == :(-1*log(1+exp(x))+exp(x))
@test integrate((1+-1*cos(x))^-1, x) == :(-1*(1+-1*cos(x))^-1*sin(x))
@test integrate(sec(x)^2*tan(x), x) == :((1/2)*sec(x)^2)
@test integrate(x*log(x), x) == :(-1//4*x^2+(1/2)*x^2*log(x))
@test integrate(cos(x)*sin(x), x) == :((1/2)*sin(x)^2)
@test integrate((-1*x^2+2x)^-1//2*(1+x), x) == :(-1*(-1*x^2+2x)^(1/2)+-2*arcsin(1+-1x))
@test integrate(2*(2+3*exp(2x))^-1*exp(x), x) == :(1//3*6^(1/2)*arctan((1/2)*6^(1/2)*exp(x)))
@test integrate(x^4*(1+-1*x^2)^-5//2, x) == :(-1*x*(1+-1*x^2)^-1//2+1//3*x^3*(1+-1*x^2)^-3//2+arcsin(x))
@test integrate((1+exp(4x))^-1*exp(6x), x) == :((1/2)*exp(2x)+-1//2*arctan(exp(2x)))
@test integrate(log(2+3*x^2), x) == :(-2x+x*log(2+3*x^2)+2//3*6^(1/2)*arctan((1/2)*x*6^(1/2)))
@test integrate(r^-1*(-1*a^2+2*H*r^2)^-1//2, x) == :(x*r^-1*(-1*a^2+2*H*r^2)^-1//2)
@test integrate(r^-1*(-1*a^2+-1*e^2+2*H*r^2)^-1//2, x) == :(x*r^-1*(-1*a^2+-1*e^2+2*H*r^2)^-1//2)
@test integrate(r^-1*(-1*a^2+-2*K*r^4+2*H*r^2)^-1//2, x) == :(x*r^-1*(-1*a^2+-2*K*r^4+2*H*r^2)^-1//2)
@test integrate(r^-1*(-1*a^2+-1*e^2+-2*K*r^4+2*H*r^2)^-1//2, x) == :(x*r^-1*(-1*a^2+-1*e^2+-2*K*r^4+2*H*r^2)^-1//2)
@test integrate(r^-1*(-1*a^2+-2*K*r+2*H*r^2)^-1//2, x) == :(x*r^-1*(-1*a^2+-2*r*(K+-1*H*r))^-1//2)
@test integrate(r*(-1*a^2+2*ℯ*r^2)^-1//2, x) == :(r*x*(-1*a^2+2*ℯ*r^2)^-1//2)
@test integrate(r*(-1*a^2+-1*e^2+2*ℯ*r^2)^-1//2, x) == :(r*x*(-1*a^2+-1*e^2+2*ℯ*r^2)^-1//2)
@test integrate(r*(-1*a^2+-2*K*r^4+2*ℯ*r^2)^-1//2, x) == :(r*x*(-1*a^2+-2*K*r^4+2*ℯ*r^2)^-1//2)
@test integrate(r*(-1*a^2+-1*e^2+-2*K*r^4+2*ℯ*r^2)^-1//2, x) == :(r*x*(-1*a^2+-1*e^2+-2*K*r^4+2*ℯ*r^2)^-1//2)

test/0 Independent test suites/Moses Problems.jl


"""
    FunctionCurveSpace(m, grid)

Space of curves in manifold `m`. When needed for calculations, uses grid `grid`
but does not discretize the curve when not necessary.
They have no embedding in a finite-dimensional euclidean space
(use `DiscretizedCurves` if you need this feature).
"""
struct FunctionCurveSpace{F,TM<:AbstractManifold{F},R<:AbstractRange} <: AbstractManifold{F}
    manifold::TM
    approx_grid::R
end

function FunctionCurveSpace(M::TM) where {F,TM<:AbstractManifold{F}}
    approx_grid = 0:0.01:1.0
    return FunctionCurveSpace{F,TM,typeof(approx_grid)}(M, approx_grid)
end

"""
    VectorizedFunction(f)

A Function with vector space operations defined.
"""
struct VectorizedFunction{F}
    f::F
end

*(a::Number, f::VectorizedFunction) = VectorizedFunction(t -> a * f.f(t))
function +(f1::VectorizedFunction, f2::VectorizedFunction)
    return VectorizedFunction(t -> f1.f(t) + f2.f(t))
end
function -(f1::VectorizedFunction, f2::VectorizedFunction)
    return VectorizedFunction(t -> f1.f(t) - f2.f(t))
end
-(f::VectorizedFunction) = VectorizedFunction(t -> -f.f(t))

(f::VectorizedFunction)(t) = f.f(t)

function distance(M::FunctionCurveSpace, p1, p2)
    # Calculates simple L2-like distance
    # There are also other reasonable Riemannian structures for this space
    reltol, abstol =
        concretize_tols(M, p1, p2, reltol=PARAMS.quad_rel_tol, abstol=PARAMS.quad_abs_tol)

    return sqrt(
        quadgk(
            t -> distance(M.manifold, p1(t), p2(t))^2,
            0.0,
            1.0,
            rtol=reltol,
            atol=abstol,
        )[1],
    )
end

function exp(M::FunctionCurveSpace, p, X)
    return t -> exp(M.manifold, p(t), X(t))
end

function geodesic(M::FunctionCurveSpace, p, X)
    return t -> (s -> geodesic(M.manifold, p(t), X(t), s))
end

function injectivity_radius(M::FunctionCurveSpace)
    # TODO: check this
    return injectivity_radius(M.manifold)
end

function inner(M::FunctionCurveSpace, p, X1, X2)
    reltol, abstol =
        concretize_tols(M, X1, X2, reltol=PARAMS.quad_rel_tol, abstol=PARAMS.quad_abs_tol)
    I, err = QuadGK.quadgk(
        s -> inner(M.manifold, p(s), X1(s), X2(s)),
        0.0,
        1.0,
        rtol=reltol,
        atol=abstol,
    )
    return I
end

function inverse_retract(
    M::FunctionCurveSpace,
    p,
    q,
    method::AbstractInverseRetractionMethod,
)
    return VectorizedFunction(t -> inverse_retract(M.manifold, p(t), q(t), method))
end

function inverse_retract(M::FunctionCurveSpace, p, q)
    return VectorizedFunction(t -> inverse_retract(M.manifold, p(t), q(t)))
end

function isapprox(
    M::FunctionCurveSpace,
    p1,
    p2;
    atol=atoldefault(M, p1, p2),
    rtol=rtoldefault(M, p1, p2),
)
    #=for i in M.approx_grid
        if !(isapprox(M.manifold, p1(i), p2(i), atol = atol, rtol = rtol))
            println("*** $i, $(p1(i)), $(p2(i)).")
        end
    end=#
    return all(
        isapprox(M.manifold, p1(i), p2(i), atol=atol, rtol=rtol) for i in M.approx_grid
    )
end

function isapprox(
    M::FunctionCurveSpace,
    p,
    X1,
    X2;
    atol=atoldefault(M, X1, X2),
    rtol=rtoldefault(M, X1, X2),
)
    #TODO add tolerance parameters to isapprox for this quadrature?
    I, err = quadgk(t -> norm(M.manifold, p(t), X1(t) - X2(t)), 0.0, 1.0)
    return I <= 2 * err + atol && I >= -2 * err - atol
end

function log(M::FunctionCurveSpace, p, q)
    return VectorizedFunction(t -> log(M.manifold, p(t), q(t)))
end

function manifold_dimension(M::FunctionCurveSpace)
    return Inf
end

function mid_point(M::FunctionCurveSpace, p, q)
    return t -> mid_point(M.manifold, p(t), q(t))
end

function retract(M::FunctionCurveSpace, p, X)
    return VectorizedFunction(t -> retract(M.manifold, p(t), X(t)))
end

function retract(M::FunctionCurveSpace, p, X, method::AbstractRetractionMethod)
    return VectorizedFunction(t -> retract(M.manifold, p(t), X(t), method))
end

function transport_srvf(M::FunctionCurveSpace, c_p, c_X, p)
    return t -> vector_transport_to(M.manifold, c_p(t), c_X(t), p)
end

function vector_transport_direction(
    M::FunctionCurveSpace,
    p,
    X,
    d,
    method::AbstractVectorTransportMethod,
)
    return VectorizedFunction(
        t -> vector_transport_direction(M.manifold, p(t), X(t), d(t), method),
    )
end

function zero_vector(M::FunctionCurveSpace, p)
    return VectorizedFunction(t -> zero_vector(M.manifold, p(t)))
end

function zero_vector(M::Manifolds.TangentBundleFibers{<:FunctionCurveSpace}, p)
    return VectorizedFunction(t -> zero_vector(M.manifold.manifold, p(t)))
end


module ER

using Random: GLOBAL_RNG
using Distributions

function coin_flip(n, k, m, rng=GLOBAL_RNG)
    size = n^k
    p = m/size
    out = NTuple{k, Int}[]
    sizehint!(out, m+ceil(Integer,sqrt(m)))
    coin_flip_kernal(Iterators.product([1:n for _ in 1:k]...), rng, p, out)
    out
end
function coin_flip_kernal(iter, rng, p, out)
    for i in iter
        if rand(rng) < p
            push!(out, i)
        end
    end
    out
end

function grass_hop(n, k, m, rng=GLOBAL_RNG)
    size = n^k
    p = m/size
    out = Vector{Int}[]
    sizehint!(out, m+ceil(Integer,sqrt(m)))
    d = Geometric(p)
    i = ones(Int, k)
    i[1] = 0
    while true
        skip = rand(rng, d)+1
        j = 1
        while skip > 0
            if j > k
                return out
            end
            skip += i[j]
            i[j] = (skip-1)%n+1
            skip = (skip-1)÷n
            j += 1
        end
        push!(out, copy(i))
    end
end

end


function Base.fft!{T<:BigFloat}(data::Vector{T})
	nn=int(length(data)/2)
	Tpi=convert(T,pi)
	n=nn << 1
	j=1
	for i=1:2:n-1
		if j>i
			data[j],data[i]=data[i],data[j]
			data[j+1],data[i+1]=data[i+1],data[j+1]
		end
		m=nn
		while m >= 2 && j > m
			j -= m
			m >>= 1
		end
		j += m
	end
	mmax=2
	while n > mmax
		istep=mmax << 1
		theta=-2Tpi/mmax
		wtemp=sin(theta/2)
		wpr = -2wtemp*wtemp
		wpi=sin(theta)
		wr=one(T)
		wi=zero(T)
		for m=1:2:mmax-1
			for i=m:istep:n
				j=i+mmax
				tempr=wr*data[j]-wi*data[j+1]
				tempi=wr*data[j+1]+wi*data[j]
				data[j]=data[i]-tempr
				data[j+1]=data[i+1]-tempi
				data[i] += tempr
				data[i+1] += tempi
			end
			wr=(wtemp=wr)*wpr-wi*wpi+wr
			wi=wi*wpr+wtemp*wpi+wi
		end
        mmax=istep
	end
	return data
end

function Base.fft{T<:BigFloat}(x::Vector{Complex{T}})
	n=length(x)
	y = Array(T,2n)
	for i = 1:n
		y[2i-1] = x[i].re
		y[2i] = x[i].im
	end
	Base.fft!(y)
	return complex(y[1:2:2n-1],y[2:2:2n])
end
Base.fft{T<:BigFloat}(x::Vector{T}) = Base.fft(complex(x))

function Base.ifft{T<:BigFloat}(x::Vector{Complex{T}})
	n=length(x)
	y = Array(T,2n)
	for i = 1:n
		y[2i-1] = x[i].re
		y[2i] = -x[i].im
	end
	Base.fft!(y)
	x = complex(y[1:2:2n-1],-y[2:2:2n])/n
end


using Petri
import Petri: evaluate
using LabelledArrays
using ModelingToolkit
using MacroTools
import MacroTools: postwalk, striplines
using Test
import GeneralizedGenerated: mk_function

@testset "Stochastic Simulation" begin
    @variables S,E,I,R, β,γ,μ
    @testset "SIR" begin
        u0 = @LArray [100, 1, 0] (:S, :I, :R)
        params = @LArray [0.15, 0.55/101] (:β, :μ)
        sir = Petri.Model([S,I,R], [(S+I, 2*I), (I,R)])
        sir′ = funckit(sir)
        m′ = evaluate(sir′)
        p = Petri.ParamProblem(m′, u0, params, 250)
        soln = Petri.solve(p)
        @test soln.S <= 10
        @test sum(soln) == 101
    end
    @testset "SEIRS" begin
        seirs = Petri.Model([S,E,I,R],[(S+I, E+I), (E,I), (I,R), (R,S)])

        # Δ1 = @show Δ(seirs)
        # Λ1 = @show Λ(seirs)
        m′ = funckit(seirs)

        @show m′
        # @test all(Δ1 .== m′.Δ)
        # @test all(Λ1 .== m′.Λ)

        m′′ = Petri.evaluate(m′)
        u0 = @LArray [100, 1, 0, 0] (:S, :E, :I, :R)
        params = @LArray [0.55/101, 0.15, 0.15, 0.15] (:β, :γ, :μ, :η)
        p = Petri.ParamProblem(m′′, u0, params, 250)
        seirs_soln = Petri.solve(p)
        @test seirs_soln.S >= 10
        @test sum(seirs_soln) == 101
    end
end

function stepfunc(m::Model)
    transitionnames = map(m.Δ) do f
        f.args[1].args[1]
    end
    n = length(m.Λ)
    ratecalc = map(enumerate(m.Λ)) do (i,f)
        body = f.args[2] |> striplines
        if length(body.args) == 1
            body = body.args[1]
        end

        quote λ[$i] = $body end
    end
    actionchain = map(enumerate(m.Δ)) do (i, f)
        body = f.args[2] |> striplines
        if length(body.args) == 1
            body = body.args[1]
        end
        # push!(body.args, :(return state))
        quote if nexti == $i
            $body
        end
        end
    end
    fdef = quote
        (p, state) -> begin
            params = p.param
            λ = zeros(Float64, $n)
            $(ratecalc...)
            nexti = Petri.sample(λ)
            if nexti == nothing
                return state
            end
            $(actionchain...)
            return state
        end
    end
    fdef
end

@testset "Unrolled Stochastic Solve" begin
    @variables S,E,I,R, β,γ,μ
    seirs = Petri.Model([S,E,I,R],[(S+I, E+I), (E,I), (I,R), (R,S)])
    m′ = funckit(seirs)
    u0 = @LArray [100, 1, 0, 0] (:S, :E, :I, :R)
    params = @LArray [0.55/101, 0.15, 0.15, 0.15] (:β, :γ, :μ, :η)
    p = Petri.ParamProblem(m′, u0, params, 250)
    fexp = stepfunc(p.m)
    arglist = [:p,:state]
    body = fexp.args[2]
    # f1 = mk_function(@__MODULE__, arglist, [], body)
    # f = (p, state) -> f1(p, state)
    f = eval(body)
    @show state′ = f(p, p.initial)
    sol = Petri.solve(p, f)
    @show sol
end


Pkg.add("Clustering")

Pkg.update()

using Clustering
using RDatasets

xclara = dataset("cluster", "xclara")

names!(xclara, [symbol(i) for i in ["x", "y"]])

using Clustering
using Gadfly
iris = dataset("datasets", "iris")
features = array(iris[:, 1:4])'
# group the data onto 3 clusters
result = kmeans( features, 3 )
plot(iris, x = "PetalLength", y = "PetalWidth",
        color = result.assignments, Geom.point)


module TestApproxDistinct
    using StreamStats
    using Distributions
    using Base.Test

    for n in rand(1:1_000_000, 100)
        xs = rand(1:n, n)
        stat = StreamStats.ApproxDistinct()
        for x in xs
            update!(stat, x)
        end
        online_d = state(stat)
        online_n = nobs(stat)
        batch_d = length(unique(xs))
        @test abs(online_d - batch_d) / batch_d < 0.03
        @test online_n == n
    end
end