##
# LowRankFun represents f(x,y) by r column and row slices stored as Funs.
##

export LowRankFun

"""
`LowRankFun` gives an approximation to a bivariate function in low rank form.
"""
mutable struct LowRankFun{S<:Space,M<:Space,SS<:AbstractProductSpace,T<:Number} <: BivariateFun{T}
    A::Vector{VFun{S,T}}
    B::Vector{VFun{M,T}}
    space::SS

    function LowRankFun{S,M,SS,T}(A::Vector{VFun{S,T}},
                                  B::Vector{VFun{M,T}},
                                  space::SS) where {S,M,SS,T}
        @assert length(A) == length(B)
        @assert length(A) > 0
        new{S,M,SS,T}(A,B,space)
    end
end

LowRankFun(A::Vector{VFun{S,T}},B::Vector{VFun{M,T}},space::SS) where {S,M,SS,T} =
    LowRankFun{S,M,SS,T}(A,B,space)
LowRankFun(A::Vector{VFun{S,T}},B::Vector{VFun{M,T}}) where {S,M,T} =
    LowRankFun(A,B,space(first(A))⊗space(first(B)))
LowRankFun(A::Vector{VFun{S,T}},B::Vector{VFun{M,V}}) where {S,M,T,V} =
    LowRankFun(convert(Vector{VFun{S,promote_type(T,V)}},A),
               convert(Vector{VFun{M,promote_type(T,V)}},B),
               space(first(A))⊗space(first(B)))
rank(f::LowRankFun) = length(f.A)
size(f::LowRankFun,k::Integer) = k==1 ? mapreduce(length,max,f.A) : mapreduce(length,max,f.B)
size(f::LowRankFun) = size(f,1),size(f,2)

## Construction via a Matrix of coefficients

function LowRankFun(X::Array{T},dx::S,dy::M) where {S<:Space,M<:Space,T<:Number}
    U,Σ,V=svd(X)
    m=max(1,count(s->s>10eps(T),Σ))

    A=VFun{S,T}[Fun(dx,U[:,k].*sqrt(Σ[k])) for k=1:m]
    B=VFun{M,T}[Fun(dy,conj(V[:,k]).*sqrt(Σ[k])) for k=1:m]

    LowRankFun(A,B)
end

## Construction in a TensorSpace via a Vector of Funs

function LowRankFun(X::Vector{VFun{S,T}},d::TensorSpace{SV,DD}) where {S,T,DD<:EuclideanDomain{2},SV}
    @assert d[1] == space(X[1])
    LowRankFun(X,d[2])
end

function LowRankFun(X::Vector{VFun{S,T}},dy::Space) where {S,T}
    m=mapreduce(ncoefficients,max,X)
    M=zeros(T,m,length(X))
    for k=1:length(X)
        M[1:ncoefficients(X[k]),k]=X[k].coefficients
    end

    LowRankFun(M,space(X[1]),dy)
end

## Adaptive constructor selector

function LowRankFun(fin::Function,dx::Space,dy::Space;
                    method::Symbol=:standard,tolerance::Union{Symbol,Tuple{Symbol,Number}}=:relative,
                    retmax::Bool=false,gridx::Integer=64,gridy::Integer=64,maxrank::Integer=100)
    f = dynamic(fin)
    if method == :standard
        F,maxabsf=standardLowRankFun(f,dx,dy;tolerance=tolerance,gridx=gridx,gridy=gridy,maxrank=maxrank)
    elseif method == :Cholesky
        @assert domain(dx) == domain(dy)
        if dx == dy
            F,maxabsf=CholeskyLowRankFun(f,dx;tolerance=tolerance,grid=max(gridx,gridy),maxrank=maxrank)
        else
            G,maxabsf=CholeskyLowRankFun(f,dx;tolerance=tolerance,grid=max(gridx,gridy),maxrank=maxrank)
            F=LowRankFun(copy(G.A),map(b->Fun(b,dy),G.B))
        end
    end
    retmax ? (F,maxabsf) : F
end

## Standard adaptive construction

function standardLowRankFun(f::Function,dx::Space,dy::Space;tolerance::Union{Symbol,Tuple{Symbol,Number}}=:relative,gridx::Integer=64,gridy::Integer=64,maxrank::Integer=100)
    xy = checkpoints(dx⊗dy)
    T = promote_type(eltype(f(first(xy)...)),prectype(dx),prectype(dy))

    # We start by sampling on the given grid, find the approximate maximum and create the first rank-one approximation.
    ptsx,ptsy=points(dx,gridx),points(dy,gridy)
    X = zeros(T,gridx,gridy)
    maxabsf,r=findapproxmax!(f,X,ptsx,ptsy,gridx,gridy)
    if maxabsf < eps(zero(T))/eps(T) return LowRankFun([Fun(dx,[zero(T)])],[Fun(dy,[zero(T)])]),maxabsf end
    a,b=Fun(x->f(x,r[2]),dx),Fun(y->f(r[1],y),dy)

    # If necessary, we resize the grid to be at least as large as the
    # lengths of the first row and column Funs and we recompute the values of X.
    if gridx < ncoefficients(a) || gridy < ncoefficients(b)
        gridx,gridy = max(gridx,ncoefficients(a)),max(gridy,ncoefficients(b))
        ptsx,ptsy=points(dx,gridx),points(dy,gridy)
        X = zeros(T,gridx,gridy)
        maxabsf,r=findapproxmax!(f,X,ptsx,ptsy,gridx,gridy)
        a,b=Fun(x->f(x,r[2]),dx),Fun(y->f(r[1],y),dy)
    end

    A,B=typeof(a)[],typeof(b)[]
    if tolerance == :relative
        tol = 100maxabsf*eps(T)
    elseif tolerance[1] == :absolute
        tol = 100*tolerance[2]*eps(T)
    end
    tol10 = tol/10
    Avals,Bvals = zeros(T,gridx),zeros(T,gridy)
    p₁,p₂ = plan_transform(dx,Avals),plan_transform(dy,Bvals)

    # Eat, drink, subtract rank-one, repeat.
    for k=1:maxrank
        if (norm(a.coefficients,Inf) < tol || norm(b.coefficients,Inf) < tol)
            return LowRankFun(A,B),maxabsf
        end
        A,B =[A;a/sqrt(abs(a(r[1])))],[B;b/(sqrt(abs(b(r[2])))*sign(b(r[2])))]
        r=findapproxmax!(A[k],B[k],X,ptsx,ptsy,gridx,gridy)
        Ar,Br=evaluate(A,r[1]),evaluate(B,r[2])
        for i=1:gridx
            @inbounds Avals[i] = f(ptsx[i],r[2])
        end
        for j=1:gridy
            @inbounds Bvals[j] = f(r[1],ptsy[j])
        end
        a,b = Fun(dx,p₁*Avals) - dotu(Br,A),Fun(dy,p₂*Bvals) - dotu(Ar,B)
        chop!(a,tol10),chop!(b,tol10)
    end
    @warn "Maximum rank of " * string(maxrank) * " reached"
    return LowRankFun(A,B),maxabsf
end

## Adaptive Cholesky decomposition, when f is Hermitian positive (negative) definite

function CholeskyLowRankFun(f::Function,dx::Space;tolerance::Union{Symbol,Tuple{Symbol,Number}}=:relative,grid::Integer=64,maxrank::Integer=100)
    xy = checkpoints(dx⊗dx)
    T = promote_type(eltype(f(first(xy)...)),prectype(dx))

    # We start by sampling on the given grid, find the approximate maximum and create the first rank-one approximation.
    pts=points(dx,grid)
    X = zeros(T,grid)
    maxabsf,r=findcholeskyapproxmax!(f,X,pts,grid)
    if maxabsf < eps(zero(T))/eps(T) return LowRankFun([Fun(dx,[zero(T)])],[Fun(dx,[zero(T)])]),maxabsf end
    a=Fun(x->f(x,r),dx)

    # If necessary, we resize the grid to be at least as large as the
    # ncoefficients of the first row/column Fun and we recompute the values of X.
    if grid < ncoefficients(a)
        grid = max(grid,ncoefficients(a))
        pts=points(dx,grid)
        X = zeros(T,grid)
        maxabsf,r=findcholeskyapproxmax!(f,X,pts,grid)
        a=Fun(x->f(x,r),dx)
    end

    A,B=typeof(a)[],typeof(a)[]
    if tolerance == :relative
        tol = 100maxabsf*eps(T)
    elseif tolerance[1] == :absolute
        tol = 100*tolerance[2]*eps(T)
    end
    tol10 = tol/10
    Avals = zeros(T,grid)
    p₁ = plan_transform(dx,Avals)

    # Eat, drink, subtract rank-one, repeat.
    for k=1:maxrank
        if norm(a.coefficients,Inf) < tol return LowRankFun(A,B),maxabsf end
        A,B = [A;a/sqrt(abs(a(r)))],[B;a/(sqrt(abs(a(r)))*sign(a(r)))]
        r=findcholeskyapproxmax!(A[k],B[k],X,pts,grid)
        Br=evaluate(B,r)
        for i=1:grid
            @inbounds Avals[i] = f(pts[i],r)
        end
        a = Fun(dx,p₁*Avals) - dotu(Br,A)
        chop!(a,tol10)
    end
    @warn "Maximum rank of " * string(maxrank) * " reached"
    return LowRankFun(A,B),maxabsf
end

## Construction via TensorSpaces and ProductDomains

LowRankFun(f::Function,S::TensorSpace{SV,DD,RR};kwds...) where {SV,DD<:EuclideanDomain{2},RR} =
    LowRankFun(dynamic(f),factor(S,1),factor(S,2);kwds...)
LowRankFun(f::Function,dx::Domain,dy::Domain;kwds...) =
    LowRankFun(dynamic(f),Space(dx),Space(dy);kwds...)
LowRankFun(f::Function,d::ProductDomain;kwds...) =
    LowRankFun(dynamic(f),factors(d)...;kwds...)

LowRankFun(f::Function;kwds...) = LowRankFun(dynamic(f),ChebyshevInterval(),ChebyshevInterval();kwds...)

## Construction from values

LowRankFun(A::Array{T}) where {T<:Number} = LowRankFun(A,Interval{T}(),Interval{T}())
LowRankFun(c::Number,etc...) = LowRankFun((x,y)->c,etc...)

## Construction from other LowRankFuns

LowRankFun(f::LowRankFun,d1::IntervalOrSegment,d2::IntervalOrSegment) =
    LowRankFun(map(g->Fun(d1,g.coefficients),f.A),
               map(g->Fun(d2,g.coefficients),f.B))
LowRankFun(f::LowRankFun) = LowRankFun(f,ChebyshevInterval(),ChebyshevInterval())

## Utilities

function findapproxmax!(f::Function,X::AbstractMatrix,ptsx::AbstractVector,ptsy::Vector,gridx,gridy)
    for j=1:gridy
        ptsyj = ptsy[j]
        @simd for k=1:gridx
            @inbounds X[k,j]+=f(ptsx[k],ptsyj)
        end
    end
    maxabsf,impt = findmaxabs(X)
    imptple = CartesianIndices((gridx,gridy))[impt]
    maxabsf,[ptsx[imptple[1]],ptsy[imptple[2]]]
end

function findapproxmax!(A::Fun,B::Fun,X::AbstractMatrix,ptsx::Vector,ptsy::Vector,gridx,gridy)
    Ax,By = A.(ptsx),B.(ptsy)
    subtractrankone!(Ax,By,X,gridx,gridy)
    maxabsf,impt = findmaxabs(X)
    imptple = CartesianIndices((gridx,gridy))[impt]
    [ptsx[imptple[1]],ptsy[imptple[2]]]
end

function findcholeskyapproxmax!(f::Function,X::AbstractVector,pts::Vector,grid)
    @simd for k=1:grid
        @inbounds X[k]+=f(pts[k],pts[k])
    end
    maxabsf,impt = findmaxabs(X)
    maxabsf,pts[impt]
end

function findcholeskyapproxmax!(A::Fun,B::Fun,X::Vector,pts::Vector,grid)
    Ax,By = A.(pts),B.(pts)
    subtractrankone!(Ax,By,X,grid)
    maxabsf,impt = findmaxabs(X)
    pts[impt]
end

function subtractrankone!(A::AbstractVector,B::AbstractVector,X::AbstractMatrix,gridx::Int,gridy::Int)
    for j=1:gridy
        @inbounds Bj = B[j]
        @simd for k=1:gridx
            @inbounds X[k,j] -= A[k]*Bj
        end
    end
end

function subtractrankone!(A::AbstractVector,B::AbstractVector,X::AbstractVector,grid::Int)
    @simd for k=1:grid
        @inbounds X[k] -= A[k]*B[k]
    end
end

## TODO: in Julia base?
function findmaxabs(a)
    if isempty(a)
        throw(ArgumentError("collection must be non-empty"))
    end
    m = abs(a[1])
    mi = 1
    for i in eachindex(a)
        ai = abs(a[i])
        if ai > m || m!=m
            m = ai
            mi = i
        end
    end
    return (m, mi)
end

(f::LowRankFun)(x,y)=evaluate(f,x,y)

domain(f::LowRankFun,k::Integer) = k==1 ? domain(first(f.A)) : domain(first(f.B))
space(f::LowRankFun,k::Integer) = k==1 ? space(first(f.A)) : space(first(f.B))
space(f::LowRankFun)=f.space

transpose(f::LowRankFun{S,M,SS,T}) where {S,M,SS,T}=LowRankFun(f.B,f.A,transpose(space(f)))

function values(f::LowRankFun)
    xm=mapreduce(ncoefficients,max,f.A)
    ym=mapreduce(ncoefficients,max,f.B)
    ret=zeros(xm,ym)
    for k=1:length(f.A)
        ret+=values(pad(f.A[k],xm))*transpose(values(pad(f.B[k],ym)))
    end
    ret
end

#TODO: this is inconsistent with 1D where it does canonical
function coefficients(f::LowRankFun)
    xm=mapreduce(ncoefficients,max,f.A)
    ym=mapreduce(ncoefficients,max,f.B)
    ret=zeros(xm,ym)
    for k=1:length(f.A)
        ret+=pad(f.A[k].coefficients,xm)*transpose(pad(f.B[k].coefficients,ym))
    end
    ret
end

function coefficients(f::LowRankFun,n::Space,m::Space)
    xm=mapreduce(ncoefficients,max,f.A)
    ym=mapreduce(ncoefficients,max,f.B)
    ret=zeros(xm,ym)
    for k=1:length(f.A)
        ret+=pad(coefficients(f.A[k],n),xm)*transpose(pad(coefficients(f.B[k],m),ym))
    end
    ret
end

function vecpoints(f::LowRankFun,k::Integer)
    if k==1
        xm=mapreduce(ncoefficients,max,f.A)
        points(space(first(f.A)),xm)
    else
        ym=mapreduce(ncoefficients,max,f.B)
        points(space(first(f.B)),ym)
    end
end

evaluate(A::Vector{T},B::Vector{M},x,y) where {T<:Fun,M<:Fun}=dotu(evaluate(A,x),evaluate(B,y))
evaluate(A::Vector{T},B::Vector{M},x::AbstractVector,y::AbstractVector) where {T<:Fun,M<:Fun}=evaluate.(transpose(A),x)*evaluate.(B,transpose(y))

evaluate(f::LowRankFun,x,y)=evaluate(f.A,f.B,x,y)
evaluate(f::LowRankFun,::Colon,::Colon)=f
evaluate(f::LowRankFun,x::Number,::Colon)=dotu(f.B,evaluate(f.A,x))

function evaluate(f::LowRankFun,::Colon,y::Number)
    m = maximum(map(ncoefficients,f.A))
    r=rank(f)
    ret = zeros(m)

    for k=1:r
        for j=1:ncoefficients(f.A[k])
            @inbounds ret[j] += f.A[k].coefficients[j]*f.B[k](y)
        end
    end

    Fun(first(f.A).space,ret)
end

"""
    f.(x::AbstractVector, y':::AbstractMatrix)
Fast evaluation of a LowRankFun on a cartesian grid x ⨂ y.
"""
function broadcast(f::LowRankFun, x::AbstractVector, y::AbstractMatrix)
    ret = zeros(length(y), length(x))
    #println(InRed * "FROM BROADCAST" * InDefault)
    for k = 1:rank(f)
        ret +=  (f.A[k].(x) * f.B[k].(y))'
    end
    ret
end

## Truncate
#TODO: should reduce rank if needed
chop(f::LowRankFun,tol)=LowRankFun(map(g->chop(g,tol),f.A),map(g->chop(g,tol),f.B),f.space)
function pad(f::LowRankFun,m::Integer,n::Integer)
    A,B = deepcopy(f.A),deepcopy(f.B)

    for k=1:rank(f)
        pad!(A[k],m);pad!(B[k],n)
    end

    LowRankFun(A,B,f.space)
end

## Algebra

for op = (:*,:/)
    @eval ($op)(A::Array{T,1},c::Number) where {T<:Fun}=map(f->($op)(f,c),A)
    @eval ($op)(f::LowRankFun,c::Number) = LowRankFun(($op)(f.A,c),f.B)
    @eval ($op)(c::Number,f::LowRankFun) = LowRankFun(($op)(c,f.A),f.B)
end

# Let K be a LowRankFun and f be a Fun.
# op(f,K) acts as operating in the x variable, and
# op(K,f) acts as operating in the y variable.

for op = (:*,:/)
    @eval ($op)(f::Fun,K::LowRankFun) = LowRankFun(($op).(f,K.A),K.B)
    @eval ($op)(K::LowRankFun,f::Fun) = LowRankFun(K.A,($op).(K.B,f))
end

+(f::LowRankFun,g::LowRankFun) = LowRankFun([f.A;g.A],[f.B;g.B])
-(f::LowRankFun) = LowRankFun(-f.A,f.B)
-(f::LowRankFun,g::LowRankFun) = f+(-g)

## QR factorization of a LowRankFun

function qr(f::LowRankFun)
    sp,r = space(f),rank(f)
    Q,R = qr(coefficients(f.A))
    BR = coefficients(f.B)*transpose(R)
    LowRankFun(map(i->Fun(sp[1],Q[:,i]),1:r),map(i->Fun(sp[2],BR[:,i]),1:r),sp)
end

## Special functions

real(u::LowRankFun)=LowRankFun([map(real,u.A),map(imag,u.A)],[map(real,u.B),-map(imag,u.B)])
imag(u::LowRankFun)=LowRankFun([map(real,u.A),map(imag,u.A)],[map(imag,u.B),map(real,u.B)])

## Calculus

sum(g::LowRankFun)=dotu(map(sum,g.A),map(sum,g.B))
sum(g::LowRankFun,n::Integer)=(n==1) ? dotu(g.B,map(sum,g.A)) : dotu(g.A,map(sum,g.B))
cumsum(g::LowRankFun,n::Integer)=(n==1) ? LowRankFun(map(cumsum,g.A),copy(g.B)) : LowRankFun(copy(g.A),map(cumsum,g.B))
differentiate(g::LowRankFun,n::Integer)=(n==1) ? LowRankFun(map(differentiate,g.A),copy(g.B)) : LowRankFun(copy(g.A),map(differentiate,g.B))
integrate(g::LowRankFun,n::Integer)=(n==1) ? LowRankFun(map(integrate,g.A),copy(g.B)) : LowRankFun(copy(g.A),map(integrate,g.B))


using Statistics
using FFTW
using Plots
using BenchmarkTools
using Profile
using LinearAlgebra
using Measures
using ProgressMeter
using Plots
theme(:dark)
using Revise
push!(LOAD_PATH, pwd()*"/main")
using DHC_2DUtils
using DHC_tests

function fink_filter_bank_p(c, L; nx=256, wd=2, t=1, shift=false, Omega=false, safety_on=true, wd_cutoff=1, p=2)
    #c     - sets the scale sampling rate (1 is dyadic, 2 is half dyadic)
    #L     - number of angular bins (usually 8*t or 16*t)
    #wd    - width of the wavelets (default 1, wd=2 for a double covering)
    #t    - plane coverage (default 1, full 2pi 2)
    #shift - shift in θ by 1/2 of the θ spacing
    #Omega - true= append Omega filter (all power beyond Nyquist) so the sum of filters is 1.0

    # -------- assertion errors to make sure arguments are reasonable
    #@test wd <= L/2

    # -------- set parameters
    dθ   = t*π/L
    θ_sh = shift ? dθ/2 : 0.0
    dx   = nx/2-1

    im_scale = convert(Int8,log2(nx))
    # -------- number of bins in radial direction (size scales)
    J = (im_scale-3)*c + 1
    normj = 1/sqrt(c)

    # -------- allocate output array of zeros
    filt      = zeros(nx, nx, J*L+(Omega ? 2 : 1))
    psi_index = zeros(Int32, J, L)
    psi_ind_in= zeros(Int32, J*L+(Omega ? 2 : 1), 2)
    psi_ind_L = zeros(Int32, J*L+(Omega ? 2 : 1))
    theta     = zeros(Float64, L)
    j_value   = zeros(Float64, J)
    info=Dict{String,Any}()

    # -------- compute the required wd
    j_rad_exp = zeros(J)
    for j_ind = 1:J
        j = (j_ind-1)/c
        jrad  = im_scale-j-2
        j_rad_exp[j_ind] = 2^(jrad)
    end

    wd_j = max.(ceil.(wd_cutoff.*L./(t.*π.*j_rad_exp)),wd)

    if !safety_on
        wd_j.=wd
    end

    # loop over wd from small to large
    ## there is some uneeded redundancy added by doing this esp in l loop
    for wd in sort(unique(wd_j))
        # -------- allocate theta and logr arrays
        θ    = zeros(nx, nx)
        logr = zeros(nx, nx)

        wdθ  = wd*dθ
        norm = 1.0/((wd).^(1/p))
        # -------- loop over l
        for l = 0:L-1
            θ_l        = dθ*l+θ_sh
            theta[l+1] = θ_l

        # -------- allocate anggood BitArray
            anggood = falses(nx, nx)

        # -------- loop over pixels
            for x = 1:nx
                sx = mod(x+dx,nx)-dx -1    # define sx,sy so that no fftshift() needed
                for y = 1:nx
                    sy = mod(y+dx,nx)-dx -1
                    θ_pix  = mod(atan(sy, sx)+π -θ_l, 2*π)
                    θ_good = abs(θ_pix-π) <= wdθ

                    # If this is a pixel we might use, calculate log2(r)
                    if θ_good
                        anggood[y, x] = θ_good
                        θ[y, x]       = θ_pix
                        r2            = sx^2 + sy^2
                        logr[y, x]    = 0.5*log2(max(1,r2))
                    end
                end
            end
            angmask = findall(anggood)
        # -------- compute the wavelet in the Fourier domain
        # -------- the angular factor is the same for all j
            F_angular = norm .* cos.((θ[angmask].-π).*(L/(2*wd*t))).^(2/p)

        # -------- loop over j for the radial part
        #    for (j_ind, j) in enumerate(1/c:1/c:im_scale-2)
            j_ind_w_wd = findall(wd_j.==wd)
            for j_ind in j_ind_w_wd
                j = (j_ind-1)/c
                j_value[j_ind] = 1+j  # store for later
                jrad  = im_scale-j-2
                Δj    = abs.(logr[angmask].-jrad)
                rmask = (Δj .<= 1) #deprecating the 1/c to 1, constant width

        # -------- radial part
                F_radial = normj .* cos.(Δj[rmask] .* (π/2)).^(2/p) #deprecating c*π/2 to π/2
                ind      = angmask[rmask]
        #      Let's have these be (J,L) if you reshape...
        #        f_ind    = (j_ind-1)*L+l+1
                f_ind    = j_ind + l*J
                filt[ind, f_ind] = F_radial .* F_angular[rmask]
                psi_index[j_ind,l+1] = f_ind
                psi_ind_in[f_ind,:] = [j_ind-1,l]
                psi_ind_L[f_ind] = 1
            end
        end
    end

    # -------- phi contains power near k=0 not yet accounted for
    filter_power = (sum(filt.^p, dims=3))[:,:,1]

    # -------- for plane half-covered (t=1), add other half-plane
    if t == 1
        filter_power .+= circshift(filter_power[end:-1:1,end:-1:1],(1,1))
    end

    # -------- compute power required to sum to 1.0
    i0 = round(Int16,nx/2-2)
    i1 = round(Int16,nx/2+4)
    center_power = 1.0 .- fftshift(filter_power)[i0:i1,i0:i1]
    zind = findall(center_power .< 1E-15)
    center_power[zind] .= 0.0  # set small numbers to zero
    phi_cen = zeros(nx, nx)
    phi_cen[i0:i1,i0:i1] = (center_power).^(1/p)

    # -------- before adding ϕ to filter bank, renormalize ψ if t=1
    if t==1 filt .*= 2.0^(1/p) end  # double power for half coverage

    # -------- add result to filter array
    phi_index  = J*L+1
    filt[:,:,phi_index] .= fftshift(phi_cen)
    psi_ind_in[phi_index,:] = [J,0]
    psi_ind_L[phi_index] = 0

    if Omega     # append a filter containing the rest (outside Nyquist)
        filter_power += filt[:,:,phi_index].^p
        edge_power    = 1.0 .- filter_power
        zind          = findall(edge_power .< 1E-15)
        edge_power[zind]     .= 0.0  # set small numbers to zero
        Omega_index           = J*L+2
        info["Omega_index"]   = Omega_index
        filt[:,:,Omega_index] = (edge_power).^(1/p)
        psi_ind_in[Omega_index,:] = [J,1]
        psi_ind_L[Omega_index] = 0
    end

    # -------- metadata dictionary
    info["npix"]         = nx
    info["j_value"]      = j_value
    info["theta_value"]  = theta
    info["psi_index"]    = psi_index
    info["phi_index"]    = phi_index
    info["J_L"]          = psi_ind_in
    info["t"]            = t
    info["wd"]           = wd_j
    info["wd_cutoff"]    = wd_cutoff
    info["fs_center_r"]  = j_rad_exp
    info["psi_ind_L"]    = psi_ind_L
    info["p"]            = p

    return filt, info
end

testtest_p = fink_filter_bank_p(1,8)
test_bank = fink_filter_bank(1,8)
test_bank .== test_p

using PyCall
using PyPlot
plt.matplotlib.style.use("dark_background")

plot_filter_bank_QA(test_p[1], test_p[2]; fname="/Users/saydjari/Dropbox/GradSchool_AKS/Doug/Projects/DHC/scratch_AKS/images/filter_bank_QA_p2.png")

test_p1 = fink_filter_bank_p(1,8,p=1)

disc = plot_filter_bank_QA(test_p1[1], test_p1[2]; fname="/Users/saydjari/Dropbox/GradSchool_AKS/Doug/Projects/DHC/scratch_AKS/images/filter_bank_QA_p1.png",p=1)

heatmap(disc)

test_pp5 = fink_filter_bank_p(1,8,p=0.5)

disc = plot_filter_bank_QA(test_pp5[1], test_pp5[2]; fname="/Users/saydjari/Dropbox/GradSchool_AKS/Doug/Projects/DHC/scratch_AKS/images/filter_bank_QA_pp5.png",p=0.5)

heatmap(disc)

test_p3 = fink_filter_bank_p(1,8,p=3)

disc = plot_filter_bank_QA(test_p3[1], test_p3[2]; fname="/Users/saydjari/Dropbox/GradSchool_AKS/Doug/Projects/DHC/scratch_AKS/images/filter_bank_QA_p3.png",p=3)

heatmap(disc)

function DHC_compute_p(image::Array{Float64,2}, filter_hash::Dict, filter_hash2::Dict=filter_hash;
    doS2::Bool=true, doS20::Bool=false, norm=true, iso=false, FFTthreads=2, normS1::Bool=false, normS1iso::Bool=false,
    p=2)
    # image        - input for WST
    # filter_hash  - filter hash from fink_filter_hash
    # filter_hash2 - filters for second order.  Default to same as first order.
    # doS2         - compute S2 coeffs
    # doS20        - compute S2 coeffs
    # norm         - scale to mean zero, unit variance
    # iso          - sum over angles to obtain isotropic coeffs

    # Use 2 threads for FFT
    FFTW.set_num_threads(FFTthreads)

    # array sizes
    (Nx, Ny)  = size(image)
    if Nx != Ny error("Input image must be square") end
    (Nf, )    = size(filter_hash["filt_index"])
    if Nf == 0  error("filter hash corrupted") end
    @assert Nx==filter_hash["npix"] "Filter size should match npix"
    @assert Nx==filter_hash2["npix"] "Filter2 size should match npix"
    @assert (normS1 && normS1iso) != 1 "normS1 and normS1iso are mutually exclusive"

    # allocate coeff arrays
    out_coeff = []
    S0  = zeros(Float64, 2)
    S1  = zeros(Float64, Nf)
    if doS2  S2  = zeros(Float64, Nf, Nf) end  # traditional 2nd order
    if doS20 S20 = zeros(Float64, Nf, Nf) end  # real space correlation
    anyM2 = doS2 | doS20

    # allocate image arrays for internal use
    im_rd_0_1  = Array{Float64, 3}(undef, Nx, Ny, Nf)

    ## 0th Order
    S0[1]   = mean(image)
    norm_im = image.-S0[1]
    S0[2]   = sum(abs.(norm_im).^p)/(Nx*Ny)
    if norm
        norm_im ./= (Nx*Ny*S0[2]).^(1/p)
    else
        norm_im = copy(image)
    end

    append!(out_coeff,S0[:])

    ## 1st Order
    im_fd_0 = fft(norm_im)  # total power=1.0

    # unpack filter_hash
    f_ind   = filter_hash["filt_index"]  # (J, L) array of filters represented as index value pairs
    f_val   = filter_hash["filt_value"]

    zarr = zeros(ComplexF64, Nx, Ny)  # temporary array to fill with zvals

    # make a FFTW "plan" for an array of the given size and type
    P = plan_ifft(im_fd_0)  # P is an operator, P*im is ifft(im)

    ## Main 1st Order and Precompute 2nd Order
    for f = 1:Nf
        f_i = f_ind[f]  # CartesianIndex list for filter
        f_v = f_val[f]  # Values for f_i
        if length(f_i) > 0
            zarr[f_i] = f_v.*im_fd_0[f_i]
            im_rd_0_1[:,:,f] .= abs.(P*zarr)
            S1[f] = sum(im_rd_0_1[:,:,f].^(p))  # image power
            #zero out the intermediate arrays
            zarr[f_i] .= 0
        end
    end

    append!(out_coeff, iso ? filter_hash["S1_iso_mat"]*S1 : S1)

    if normS1iso
        S1iso = vec(reshape(filter_hash["S1_iso_mat"]*S1,(1,:))*filter_hash["S1_iso_mat"])
    end

    Mat2 = filter_hash["S2_iso_mat"]
    if doS2
        f_ind2   = filter_hash2["filt_index"]  # (J, L) array of filters represented as index value pairs
        f_val2   = filter_hash2["filt_value"]

        ## Traditional second order
        for f1 = 1:Nf
            thisim = fft(im_rd_0_1[:,:,f1])  # Could try rfft here
            # Loop over f2 and do second-order convolution
            if normS1
                normS1pwr = S1[f1]
            elseif normS1iso
                normS1pwr = S1iso[f1]
            else
                normS1pwr = 1
            end
            for f2 = 1:Nf
                f_i = f_ind2[f2]  # CartesianIndex list for filter
                f_v = f_val2[f2]  # Values for f_i
                zarr[f_i] = f_v .* thisim[f_i]
                S2[f1,f2] = sum(abs.(P*zarr).^(p))/normS1pwr
                #zero out the intermediate arrays
                zarr[f_i] .= 0
            end
        end
        append!(out_coeff, iso ? Mat2*S2[:] : S2[:])
    end

    # Real domain 2nd order
    if doS20
        Amat = reshape(im_rd_0_1, Nx*Ny, Nf)
        S20  = Amat' * Amat
        append!(out_coeff, iso ? Mat2*S20[:] : S20[:])
    end

    return out_coeff
end

test = zeros(16,16,6)
for i=1:6
    a = rand(16,16)
    test[:,:,i] .= a
end

test

filter_hash = fink_filter_hash(1,8)
f_ind   = filter_hash["filt_index"]  # (J, L) array of filters represented as index value pairs
f_val   = filter_hash["filt_value"]

temp_mat = zeros(Float64, 256, 256)
f_i = f_ind[10]  # CartesianIndex list for filter
f_v = f_val[10]  # Values for f_i

temp_mat[f_i] = f_v
temp_mat
@benchmark temp_mat[f_i] = f_v

@benchmark for i = 1:length(f_i)
    ind       = f_i[i]
    temp_mat[ind] = f_v[i]        # filter*image in Fourier domain
end

a = rand(256,256)
temp_mat[f_i] = f_v
temp_mat
@benchmark temp_mat[f_i] = f_v.*a[f_i]

@benchmark for i = 1:length(f_i)
    ind       = f_i[i]
    temp_mat[ind] = f_v[i]*a[ind]       # filter*image in Fourier domain
end

a = rand(256,256)

original = DHC_compute(a,filter_hash)

new = DHC_compute_p(a,filter_hash)

@benchmark DHC_compute_p(a,filter_hash)

@benchmark DHC_compute(a,filter_hash)

6/.178

# so we are slower by slightly less than a factor of Nf... not sure we can help that

function fink_filter_hash_p(c, L; nx=256, wd=2, t=1, shift=false, Omega=false, safety_on=true, wd_cutoff=1, p=2)
    # -------- compute the filter bank
    filt, hash = fink_filter_bank_p(c, L; nx=nx, wd=wd, t=t, shift=shift, Omega=Omega, safety_on=safety_on, wd_cutoff=wd_cutoff, p=p)

    # -------- list of non-zero pixels
    flist = fink_filter_list(filt)

    # -------- pack everything you need into the info structure
    hash["filt_index"] = flist[1]
    hash["filt_value"] = flist[2]

    # -------- compute matrix that projects iso coeffs, add to hash
    S1_iso_mat = S1_iso_matrix(hash)
    hash["S1_iso_mat"] = S1_iso_mat
    S2_iso_mat = S2_iso_matrix(hash)
    hash["S2_iso_mat"] = S2_iso_mat
    hash["num_iso_coeff"] = size(S1_iso_mat)[1] + size(S2_iso_mat)[1] + 2
    hash["num_coeff"] = size(S1_iso_mat)[2] + size(S2_iso_mat)[2] + 2

    return hash
end

filter_hash = fink_filter_hash(1,8)
filter_hashp2 = fink_filter_hash_p(1,8,p=2)

filter_hashp2 == filter_hash

filter_hashp1 = fink_filter_hash_p(1,8,p=1)

@benchmark DHC_compute_p(a,filter_hashp1,p=1)

filter_hashp3 = fink_filter_hash_p(1,8,p=3)

@benchmark DHC_compute_p(a,filter_hashp3,p=3)

filter_hashpp5 = fink_filter_hash_p(1,8,p=0.5)

@benchmark DHC_compute_p(a,filter_hashpp5,p=0.5)

b = [-1, 2, 3, 4]

norm(b)

norm(b,1)

norm(b,3)

norm(b,1/3)

sum(abs.(b).^1)

sqrt(sum(abs.(b).^2))

@benchmark sum(abs.(b).^1)

@benchmark norm(b,1)

@benchmark sum(abs.(b).^2)

@benchmark norm(b,2)^2

Profile.clear()
@profile DHC_compute_p(a,filter_hashp3,p=3)

Juno.profiler()

b = rand(ComplexF64,256,256)

sum(abs.(b).^3)

@benchmark sum(abs.(b).^3)

@benchmark norm(b,3)^3

@benchmark sum(abs2.(b).^(3/2))

sum(abs2.(b).^(3/2))

ref = fink_filter_bank(1,8)
test = fink_filter_bank_p(1,8)

ref == test

a = rand(256,256)

original = DHC_compute(a,filter_hash)

new = DHC_compute_p(a,filter_hash)

original[2:5]./new[2:5]

256*256

original == new

original ≈ new

nearlysame(x, y) = x ≈ y || (isnan(x) & isnan(y))
nearlysame(A::AbstractArray, B::AbstractArray) = all(map(nearlysame, A, B))

nearlysame(original[1:6*8+1],new[1:6*8+1])

maximum(abs.(new.-original))

original[100:105]./new[100:105]

new[100:105]

sum(original[3:end])

sum(original[3:6*8+1])

function rod_image(xcen, ycen, length, pa, fwhm; nx=256)
    # returns image of a rod with some (x,y) position, length,
    #   position angle, and FWHM in an (nx,nx) image.
    rodimage = zeros(nx,nx)

    x=0
    y=0
    sig = fwhm/2.355
    dtor = π/180
    # -------- define a unit vector in direction of rod at position angle pa
    ux = sin(pa*dtor)   #  0 deg is up
    uy = cos(pa*dtor)   # 90 deg to the right

    for i=1:nx
        for j=1:nx
            x=i-nx/2+xcen
            y=j-nx/2+ycen

            # -------- distance parallel and perpendicular to
            dpara =  ux*x + uy*y
            dperp = -uy*x + ux*y

            if abs(dpara)-length <0
                dpara= 0
            end
            dpara = abs(dpara)
            dpara = min(abs(dpara-length),dpara)

            rodimage[i,j] = exp(-(dperp^2+dpara^2)/(2*sig^2))
        end
    end
    rodimage ./= sqrt(sum(rodimage.^2))
    return rodimage
end

rod_test = rod_image(1,1,30,35,8)

heatmap(rod_test)

original = DHC_compute(rod_test,filter_hash)

new = DHC_compute_p(rod_test,filter_hash)

original ≈ new

sum(original[3:end])

sum(new[3:end])

sum(new[3:end])-2 < 1e-6
sum(original[3:end])-2 < 1e-6


import Base: ==, ≈, copy, eltype
import Random.rand

export LazySet,
       basetype,
       ρ, support_function,
       σ, support_vector,
       dim,
       norm,
       radius,
       diameter,
       an_element,
       isbounded, isbounded_unit_dimensions,
       neutral,
       absorbing,
       tosimplehrep,
       isuniversal,
       translate,
       affine_map,
       reflect,
       is_interior_point,
       isoperation,
       isoperationtype,
       isequivalent,
       isconvextype,
       area,
       surface

"""
    LazySet{N}

Abstract type for convex sets, i.e., sets characterized by a (possibly infinite)
intersection of halfspaces, or equivalently, sets ``S`` such that for any two
elements ``x, y ∈ S`` and ``0 ≤ λ ≤ 1`` it holds that ``λ·x + (1-λ)·y ∈ S``.

### Notes

`LazySet` types should be parameterized with a type `N`, typically `N<:Real`,
for using different numeric types.

Every concrete `LazySet` must define the following functions:
- `σ(d::AbstractVector{N}, S::LazySet{N}) where {N<:Real}` -- the support vector
    of `S` in a given direction `d`; note that the numeric type `N` of `d` and
    `S` must be identical; for some set types `N` may be more restrictive than
    `Real`
- `dim(S::LazySet)` -- the ambient dimension of `S`

The function
- `ρ(d::AbstractVector{N}, S::LazySet{N}) where {N<:Real}` -- the support
    function of `S` in a given direction `d`; note that the numeric type `N` of
    `d` and `S` must be identical; for some set types `N` may be more
    restrictive than `Real`
is optional because there is a fallback implementation relying on `σ`.
However, for unbounded sets (which includes most lazy set types) this fallback
cannot be used and an explicit method must be implemented.

The subtypes of `LazySet` (including abstract interfaces):

```jldoctest; setup = :(using LazySets: subtypes)
julia> subtypes(LazySet, false)
14-element Array{Any,1}:
 AbstractAffineMap
 AbstractCentrallySymmetric
 AbstractPolyhedron
 Bloating
 CachedMinkowskiSumArray
 CartesianProduct
 CartesianProductArray
 ConvexHull
 ConvexHullArray
 EmptySet
 Intersection
 IntersectionArray
 MinkowskiSum
 MinkowskiSumArray
```

If we only consider *concrete* subtypes, then:

```jldoctest; setup = :(using LazySets: subtypes)
julia> concrete_subtypes = subtypes(LazySet, true);

julia> length(concrete_subtypes)
39

julia> println.(concrete_subtypes);
AffineMap
Ball1
Ball2
BallInf
Ballp
Bloating
CachedMinkowskiSumArray
CartesianProduct
CartesianProductArray
ConvexHull
ConvexHullArray
Ellipsoid
EmptySet
ExponentialMap
ExponentialProjectionMap
HPolygon
HPolygonOpt
HPolyhedron
HPolytope
HalfSpace
Hyperplane
Hyperrectangle
Intersection
IntersectionArray
Interval
Line
LineSegment
LinearMap
MinkowskiSum
MinkowskiSumArray
ResetMap
Singleton
SymmetricIntervalHull
Translation
Universe
VPolygon
VPolytope
ZeroSet
Zonotope
```
"""
abstract type LazySet{N} end

# --- common LazySet functions ---

"""
    eltype(::Type{<:LazySet{N}}) where {N}

Return the numeric type (`N`) of the given set type.

### Input

- `T` -- set type, used for dispatch

### Output

The numeric type of `T`.
"""
eltype(::Type{<:LazySet{N}}) where {N} = N

"""
    eltype(::LazySet{N}) where {N}

Return the numeric type (`N`) of the given set.

### Input

- `X` -- set instance, used for dispatch

### Output

The numeric type of `X`.
"""
eltype(::LazySet{N}) where {N} = N

"""
    basetype(T::Type{<:LazySet})

Return the base type of the given set type (i.e., without type parameters).

### Input

- `T` -- set type, used for dispatch

### Output

The base type of `T`.
"""
basetype(T::Type{<:LazySet}) = Base.typename(T).wrapper

"""
    basetype(S::LazySet)

Return the base type of the given set (i.e., without type parameters).

### Input

- `S` -- set instance, used for dispatch

### Output

The base type of `S`.

### Examples

```jldoctest
julia> z = rand(Zonotope);

julia> basetype(z)
Zonotope

julia> basetype(z + z)
MinkowskiSum

julia> basetype(LinearMap(rand(2, 2), z + z))
LinearMap
```
"""
basetype(S::LazySet) = Base.typename(typeof(S)).wrapper

"""
    ρ(d::AbstractVector{N}, S::LazySet{N}) where {N<:Real}

Evaluate the support function of a set in a given direction.

### Input

- `d` -- direction
- `S` -- convex set

### Output

The support function of the set `S` for the direction `d`.

### Notes

The numeric type of the direction and the set must be identical.
"""
function ρ(d::AbstractVector{N}, S::LazySet{N}) where {N<:Real}
    return dot(d, σ(d, S))
end

"""
    support_function

Alias for the support function ρ.
"""
const support_function = ρ

"""
    σ

Function to compute the support vector σ.
"""
function σ end

"""
    support_vector

Alias for the support vector σ.
"""
const support_vector = σ

"""
    isbounded(S::LazySet)

Determine whether a set is bounded.

### Input

- `S` -- set

### Output

`true` iff the set is bounded.

### Algorithm

We check boundedness via [`isbounded_unit_dimensions`](@ref).
"""
function isbounded(S::LazySet)
    return isbounded_unit_dimensions(S)
end

"""
    isbounded_unit_dimensions(S::LazySet{N}) where {N<:Real}

Determine whether a set is bounded in each unit dimension.

### Input

- `S` -- set

### Output

`true` iff the set is bounded in each unit dimension.

### Algorithm

This function performs ``2n`` support function checks, where ``n`` is the
ambient dimension of `S`.
"""
function isbounded_unit_dimensions(S::LazySet{N}) where {N<:Real}
    n = dim(S)
    @inbounds for i in 1:n
        for o in [one(N), -one(N)]
            d = SingleEntryVector(i, n, o)
            if ρ(d, S) == N(Inf)
                return false
            end
        end
    end
    return true
end

"""
    norm(S::LazySet, [p]::Real=Inf)

Return the norm of a convex set.
It is the norm of the enclosing ball (of the given ``p``-norm) of minimal volume
that is centered in the origin.

### Input

- `S` -- convex set
- `p` -- (optional, default: `Inf`) norm

### Output

A real number representing the norm.
"""
function norm(S::LazySet, p::Real=Inf)
    if p == Inf
        return norm(Approximations.ballinf_approximation(S), p)
    else
        error("the norm for this value of p=$p is not implemented")
    end
end

"""
    radius(S::LazySet, [p]::Real=Inf)

Return the radius of a convex set.
It is the radius of the enclosing ball (of the given ``p``-norm) of minimal
volume with the same center.

### Input

- `S` -- convex set
- `p` -- (optional, default: `Inf`) norm

### Output

A real number representing the radius.
"""
function radius(S::LazySet, p::Real=Inf)
    if p == Inf
        return radius(Approximations.ballinf_approximation(S), p)
    else
        error("the radius for this value of p=$p is not implemented")
    end
end

"""
    diameter(S::LazySet, [p]::Real=Inf)

Return the diameter of a convex set.
It is the maximum distance between any two elements of the set, or,
equivalently, the diameter of the enclosing ball (of the given ``p``-norm) of
minimal volume with the same center.

### Input

- `S` -- convex set
- `p` -- (optional, default: `Inf`) norm

### Output

A real number representing the diameter.
"""
function diameter(S::LazySet, p::Real=Inf)
    return radius(S, p) * 2
end

"""
    affine_map(M::AbstractMatrix, X::LazySet, v::AbstractVector)

Compute a concrete affine map.

### Input

- `M` -- linear map
- `X` -- convex set
- `v` -- translation vector

### Output

A set representing the affine map of `X`.

### Algorithm

The implementation applies the functions `linear_map` and `translate`.
"""
function affine_map(M::AbstractMatrix, X::LazySet, v::AbstractVector)
    return translate(linear_map(M, X), v)
end

"""
    an_element(S::LazySet{N}) where {N<:Real}

Return some element of a convex set.

### Input

- `S` -- convex set

### Output

An element of a convex set.

### Algorithm

An element of the set is obtained by evaluating its support vector along
direction ``[1, 0, …, 0]``.
"""
function an_element(S::LazySet{N}) where {N<:Real}
    e₁ = SingleEntryVector(1, dim(S), one(N))
    return σ(e₁, S)
end

"""
    ==(X::LazySet, Y::LazySet)

Return whether two LazySets of the same type are exactly equal.

### Input

- `X` -- any `LazySet`
- `Y` -- another `LazySet` of the same type as `X`

### Output

- `true` iff `X` is equal to `Y`.

### Notes

The check is purely syntactic and the sets need to have the same base type.
For instance, `X::VPolytope == Y::HPolytope` returns `false` even if `X` and `Y`
represent the same polytope.
However `X::HPolytope{Int64} == Y::HPolytope{Float64}` is a valid comparison.

### Algorithm

We recursively compare the fields of `X` and `Y` until a mismatch is found.

### Examples

```jldoctest
julia> HalfSpace([1], 1) == HalfSpace([1], 1)
true

julia> HalfSpace([1], 1) == HalfSpace([1.0], 1.0)
true

julia> Ball1([0.], 1.) == Ball2([0.], 1.)
false
```
"""
function ==(X::LazySet, Y::LazySet)
    # if the common supertype of X and Y is abstract, they cannot be compared
    if isabstracttype(promote_type(typeof(X), typeof(Y)))
        return false
    end

    for f in fieldnames(typeof(X))
        if getfield(X, f) != getfield(Y, f)
            return false
        end
    end

    return true
end
"""
    ≈(X::LazySet, Y::LazySet)

Return whether two LazySets of the same type are approximately equal.

### Input

- `X` -- any `LazySet`
- `Y` -- another `LazySet` of the same type as `X`

### Output

- `true` iff `X` is equal to `Y`.

### Notes

The check is purely syntactic and the sets need to have the same base type.
For instance, `X::VPolytope ≈ Y::HPolytope` returns `false` even if `X` and `Y`
represent the same polytope.
However `X::HPolytope{Int64} ≈ Y::HPolytope{Float64}` is a valid comparison.

### Algorithm

We recursively compare the fields of `X` and `Y` until a mismatch is found.

### Examples

```jldoctest
julia> HalfSpace([1], 1) ≈ HalfSpace([1], 1)
true

julia> HalfSpace([1], 1) ≈ HalfSpace([1.00000001], 0.99999999)
true

julia> HalfSpace([1], 1) ≈ HalfSpace([1.0], 1.0)
true

julia> Ball1([0.], 1.) ≈ Ball2([0.], 1.)
false
```
"""
function ≈(X::LazySet, Y::LazySet)
    # if the common supertype of X and Y is abstract, they cannot be compared
    if isabstracttype(promote_type(typeof(X), typeof(Y)))
        return false
    end

    for f in fieldnames(typeof(X))
        if !_isapprox(getfield(X, f), getfield(Y, f))
            return false
        end
    end

    return true
end

# hook into random API
function rand(rng::AbstractRNG, ::SamplerType{T}) where T<:LazySet
    rand(T, rng=rng)
end

"""
    copy(S::LazySet)

Return a deep copy of the given set by copying its values recursively.

### Input

- `S` -- any `LazySet`

### Output

A copy of `S`.

### Notes

This function performs a `deepcopy` of each field in `S`, resulting in a
completely independent object. See the documentation of `?deepcopy` for further
details.
"""
copy(S::LazySet) = deepcopy(S)

"""
    tosimplehrep(S::LazySet)

Return the simple H-representation ``Ax ≤ b`` of a set from its list of linear
constraints.

### Input

- `S` -- set

### Output

The tuple `(A, b)` where `A` is the matrix of normal directions and `b` is the
vector of offsets.

### Notes

This function only works for sets that can be represented exactly by a finite
list of linear constraints.
This fallback implementation relies on `constraints_list(S)`.
"""
tosimplehrep(S::LazySet) = tosimplehrep(constraints_list(S))

"""
    reflect(P::LazySet{N}) where {N<:Real}

Concrete reflection of a convex set `P`, resulting in the reflected set `-P`.

### Note

This function requires that the list of constraints of the set `P` is
available, i.e. such that it can be written as
``P = \\{z ∈ ℝⁿ: ⋂ sᵢᵀz ≤ rᵢ, i = 1, ..., N\\}.``

This function can be used to implement the alternative definition of the
Minkowski Difference, which writes as
```math
A ⊖ B = \\{a − b | a ∈ A, b ∈ B\\} = A ⊕ (-B)
```
by calling `minkowski_sum(A, reflect(B))`.
"""
function reflect(P::LazySet)
    @assert applicable(constraints_list, P)  "this function " *
        "requires that the list of constraints is available, but it is not; " *
        "if the set is bounded, try overapproximating with an `HPolytope` first"

    F, g = tosimplehrep(P)
    T = isbounded(P) ? HPolytope : HPolyhedron
    return T(-F, g)
end

"""
    isuniversal(X::LazySet{N}, [witness]::Bool=false) where {N<:Real}

Check whether a given convex set is universal, and otherwise optionally compute
a witness.

### Input

- `X`       -- convex set
- `witness` -- (optional, default: `false`) compute a witness if activated

### Output

* If `witness` option is deactivated: `true` iff ``X`` is universal
* If `witness` option is activated:
  * `(true, [])` iff ``X`` is universal
  * `(false, v)` iff ``X`` is not universal and ``v ∉ X``

### Notes

This is a naive fallback implementation.
"""
function isuniversal(X::LazySet{N}, witness::Bool=false) where {N<:Real}
    if isbounded(X)
        result = false
    else
        error("cannot determine universality of the set")
    end

    if result
        return witness ? (true, N[]) : true
    elseif witness
        error("witness production is currently not supported")
    else
        return false
    end
end

"""
    is_interior_point(d::AbstractVector{N}, P::LazySet{N};
                      p=N(Inf), ε=_rtol(N)) where {N<:Real}

Check if the point `d` is contained in the interior of the convex set `P`.

### Input

- `d`  -- point
- `P`  -- set
- `p`  -- (optional; default: `N(Inf)`) norm of the ball used to apply the error
          tolerance
- `ε`  -- (optional; default: `_rtol(N)`) error tolerance of check

### Output

Boolean which indicates if the point `d` is contained in `P`.

### Algorithm

The implementation checks if a `Ballp` of norm `p` with center `d` and radius
`ε` is contained in the set `P`.
This is a numerical check for `d ∈ interior(P)` with error tolerance `ε`.
"""
function is_interior_point(d::AbstractVector{N}, P::LazySet{N};
                           p=N(Inf), ε=_rtol(N)) where {N<:Real}
    return Ballp(p, d, ε) ⊆ P
end

"""
    plot_recipe(X::LazySet{N}, [ε]::N=N(PLOT_PRECISION)) where {N<:Real}

Convert a convex set to a pair `(x, y)` of points for plotting.

### Input

- `X` -- convex set
- `ε` -- (optional, default: `PLOT_PRECISION`) approximation error bound

### Output

A pair `(x, y)` of points that can be plotted.

### Notes

Plotting of unbounded sets is not implemented yet (see
[#576](https://github.com/JuliaReach/LazySets.jl/issues/576)).

### Algorithm

We first assert that `X` is bounded.

One-dimensional sets are converted to an `Interval`.
We do not support three-dimensional or higher-dimensional sets at the moment.

For two-dimensional sets, we first compute a polygonal overapproximation.
The second argument, `ε`, corresponds to the error in Hausdorff distance between
the overapproximating set and `X`.
The default value `PLOT_PRECISION` is chosen such that the unit ball in the
2-norm is approximated with reasonable accuracy.
On the other hand, if you only want to produce a fast box-overapproximation of
`X`, pass `ε=Inf`.
Finally, we use the plot recipe for polygons.
"""
function plot_recipe(X::LazySet{N}, ε::N=N(PLOT_PRECISION)) where {N<:Real}
    @assert dim(X) <= 2 "cannot plot a $(dim(X))-dimensional $(typeof(X))"
    @assert isbounded(X) "cannot plot an unbounded $(typeof(X))"

    if dim(X) == 1
        Y = convert(Interval, X)
    else
        Y = overapproximate(X, ε)
    end
    return plot_recipe(Y, ε)
end

"""
    isoperation(X::LazySet)

Check whether the given `LazySet` is an instance of a set operation or not.

### Input

- `X` -- a `LazySet`

### Output

`true` if `X` is an instance of a set-based operation and `false` otherwise.

### Notes

The fallback implementation returns whether the set type of the input is an
operation or not using `isoperationtype`.

See also [`isoperationtype(X::Type{<:LazySet})`](@ref).

### Examples

```jldoctest
julia> B = BallInf([0.0, 0.0], 1.0);

julia> isoperation(B)
false

julia> isoperation(B ⊕ B)
true
```
"""
function isoperation(X::LazySet)
    return isoperationtype(typeof(X))
end

isoperation(::Type{<:LazySet}) = error("`isoperation` cannot be applied to a set " *
                                       "type; use `isoperationtype` instead")

"""
    isoperationtype(X::Type{<:LazySet})

Check whether the given `LazySet` type is an operation or not.

### Input

- `X` -- subtype of `LazySet`

### Output

`true` if the given set type is a set-based operation and `false` otherwise.

### Notes

The fallback for this function returns an error that `isoperationtype` is not
implemented. Subtypes of `LazySet` should dispatch on this function as required.

See also [`isoperation(X<:LazySet)`](@ref).

### Examples

```jldoctest
julia> isoperationtype(BallInf)
false

julia> isoperationtype(LinearMap)
true
```
"""
function isoperationtype(X::Type{<:LazySet})
    error("`isoperationtype` is not implemented for type $X")
end

isoperationtype(::LazySet) = error("`isoperationtype` cannot be applied to " *
                                   "a set instance; use `isoperation` instead")

"""
    isequivalent(X::LazySet, Y::LazySet)

Return whether two LazySets are equal in the mathematical sense, i.e. equivalent.

### Input

- `X` -- any `LazySet`
- `Y` -- another `LazySet`

### Output

`true` iff `X` is equivalent to `Y`.

## Algorithm

First, the check `X == Y` is performed which returns `true` if and only if the given sets are of the same type,
and have the same values (modulo floating-point tolerance). Otherwise, the double inclusion check `X ⊆ Y && Y ⊆ X` is
used.

### Examples

```jldoctest
julia> X = BallInf([0.1, 0.2], 0.3);

julia> Y = convert(HPolytope, X);

julia> X == Y
false

julia> isequivalent(X, Y)
true
```
"""
function isequivalent(X::LazySet, Y::LazySet)
    if X ≈ Y
        return true
    end
    return X ⊆ Y && Y ⊆ X
end

"""
    isconvextype(X::Type{<:LazySet})

Check whether the given `LazySet` type is convex.

### Input

- `X` -- subtype of `LazySet`

### Output

`true` if the given set type is guaranteed to be convex by using only type
information, and `false` otherwise.

### Notes

Since this operation only acts on types (not on values), it can return false
negatives, i.e. there may be instances where the set is convex, even though the
answer of this function is `false`. The examples below illustrate this point.

### Examples

A ball in the infinity norm is always convex, hence we get:

```jldoctest convex_types
julia> isconvextype(BallInf)
true
```

For instance, the union (`UnionSet`) of two sets may in general be either convex
or not, since convexity cannot be decided by just using type information.
Hence, `isconvextype` returns `false` if `X` is `Type{<:UnionSet}`.

```jldoctest convex_types
julia> isconvextype(UnionSet)
false
```

However, the type parameters from the set operations allow to decide convexity
in some cases, by falling back to the convexity of the type of its arguments.
Consider for instance the lazy intersection. The intersection of two convex sets
is always convex, hence we can get:

```jldoctest convex_types
julia> isconvextype(Intersection{Float64, BallInf{Float64}, BallInf{Float64}})
true
```
"""
isconvextype(X::Type{<:LazySet}) = false

"""
    surface(X::LazySet{N}) where {N}

Compute the surface area of a set.

### Input

- `X` -- set

### Output

A number representing the surface area of `X`.
"""
function surface(X::LazySet{N}) where {N}
    if dim(X) == 2
        return area(X)
    else
        throw(ArgumentError("the surface function is only implemented for " *
                    "two-dimensional sets, but the given set is $(dim(X))-dimensional"))
    end
end

"""
    area(X::LazySet{N}) where {N}

Compute the area of a two-dimensional polytopic set using the Shoelace formula.

### Input

- `X` -- two-dimensional set

### Output

A number representing the area of `X`.

### Notes

This algorithm is applicable to any lazy set `X` such that its list of vertices,
`vertices_list`, can be computed.

### Algorithm

Let `m` be the number of vertices of `X`. The following instances are considered:

- `m = 0, 1, 2`: the output is zero.
- `m = 3`: the triangle case is computed using the Shoelace formula with 3 points.
- `m = 4`: the quadrilateral case is obtained by the factored version of the Shoelace
           formula with 4 points.

Otherwise, the general Shoelace formula is used; for detals see the wikipedia
article [Shoelace formula](https://en.wikipedia.org/wiki/Shoelace_formula).
"""
function area(X::LazySet{N}) where {N}
    @assert dim(X) == 2 "this function only applies to two-dimensional sets, " *
    "but the given set is $(dim(X))-dimensional"

    vlist = vertices_list(X)
    m = length(vlist)

    if m <= 2
        return zero(N)
    end

    if m == 3 # triangle
        res = _area_triangle(vlist)

    elseif m == 4 # quadrilateral
        res = _area_quadrilateral(vlist)

    else # general case
        res = _area_polygon(vlist)
    end

    return res
end

function _area_triangle(v::Vector{VN}) where {N, VN<:AbstractVector{N}}
    A = v[1]
    B = v[2]
    C = v[3]
    res = A[1] * (B[2] - C[2]) + B[1] * (C[2] - A[2]) + C[1] * (A[2] - B[2])
    return abs(res/2)
end

function _area_quadrilateral(v::Vector{VN}) where {N, VN<:AbstractVector{N}}
    A = v[1]
    B = v[2]
    C = v[3]
    D = v[4]
    res = A[1] * (B[2] - D[2]) + B[1] * (C[2] - A[2]) + C[1] * (D[2] - B[2]) + D[1] * (A[2] - C[2])
    return abs(res/2)
end

function _area_polygon(v::Vector{VN}) where {N, VN<:AbstractVector{N}}
    m = length(v)
    @inbounds res = v[m][1] * v[1][2] - v[1][1] * v[m][2]
    for i in 1:m-1
        @inbounds res += v[i][1] * v[i+1][2] - v[i+1][1] * v[i][2]
    end
    return abs(res/2)
end


include("../../examples/matern.jl")
include("../plotting/gnuplot_utils.jl")

const VGRID = range(0.25, 10.0, length=101) # to hit "near-integer" v, which is the hardest.
const XGRID = range(0.0,  30.0, length=201)[2:end]

# First and second derivatives with FD:
fdbesselkdv(v, x) = (besselk(v+1e-6, x) - besselk(v+1e-6, x))/1e-6
fdbesselkdvdv(v, x) = (besselk(v+2e-6, x) - 2*besselk(v+1e-6, x) + besselk(v, x))/1e-12

# First and second derivatives with AD:
adbesselkdv(v, x) = ForwardDiff.derivative(_v->BesselK._besselk(_v, x), v)
adbesselkdvdv(v, x) = ForwardDiff.derivative(_v->adbesselkdv(_v, x), v)

function assemble_matrix(fn, pts, p)
  out = Array{Float64}(undef, length(pts), length(pts))
  Threads.@threads for k in 1:length(pts)
    out[k,k] = p[1]
    @inbounds for j in 1:(k-1)
      out[j,k] = fn(pts[j], pts[k], p)
    end
  end
  Symmetric(out)
end

atolfun(tru, est) = isnan(est) ? NaN : (isinf(tru) ? 0.0 : abs(tru-est))
atolfun(tru_est)  = atolfun(tru_est[1], tru_est[2])
rtolfun(tru, est) = atolfun(tru, est)/abs(tru)
rtolfun(tru_est)  = rtolfun(tru_est[1], tru_est[2])


#####################################################################
# IMPLEMENTACIÓN DEL ALGORITMOS DE INICIALIZACIÓN DE PESOS
# Basado en capítulo 7, algoritmo 6
#   CASO ENTRADA REAL (una dimensión) SALIDA REAL (una dimensión)
#####################################################################
export nn_from_data
"""
nn_from_data(X_train::Vector,Y_train::Vector, n::Int, M=10)::AbstractOneLayerNeuralNetwork
    Devuelve una red neuronal de entrada de una dimensión y
    de salida una dimensión con los pesos ya inicializados
    de acorte a los conjuntos de entrenamiento.
    `n` es el número de neuronas en la capa oculta.
    El tamaño de entrada  y salida de la red neuronal vienen determinados por
    por los propios datos de entranamiento.
    El tamaño de entrada se corresponde con el número de atributos de X_train (su número de columnas)
    y a salida con el número de columnas de `Y_train`.

    M es una constante que depende de la función de activación,
    por lo visto en teoría M=10 funciona para todas las

"""
function nn_from_data(X_train::Vector,Y_train::Vector, n::Int, M=10)::AbstractOneLayerNeuralNetwork
    entry_dimension =  1
    output_dimension = 1

    nodes = []
    index = 1
    tam = 0
    y_values = zeros(n)

    while tam < n && index <= n
        if !(X_train[index] in nodes)
            append!(nodes, X_train[index] )
            tam += 1
            y_values[tam] = Y_train[index]
        end
        index += 1
    end
    ordered_index = sortperm(nodes)
    # Matrices de la red neuronal
    # A = n x 1
    # S = n x 1
    # B = 1 x n
    A = zeros(Float64, n, entry_dimension)
    S = zeros(Float64, n)
    B = zeros(Float64, output_dimension, n)

    # valores iniciales
    x_a = nodes[ordered_index[1]]
    y_a = y_values[ordered_index[1]]
    # Función afín constantemente Y_1
    S[1]= M
    B[1] = y_a

    # Cálculo del resto de neuronas
    for (index,key) in collect(Iterators.zip(2:n, ordered_index[2:n]))
        x_s = nodes[key]
        y_s = y_values[key]

        A[index] = 2M / (x_s - x_a)
        S[index] = M - x_s * A[index]
        B[index] = y_s - y_a

        x_a = x_s
        y_a = y_s

    end
    return FromMatrixNN(S,A,B)
end


# MIT license
# Copyright (c) Microsoft Corporation. All rights reserved.
# See LICENSE in the project root for full license information.

module Transforms

using StaticArrays: SVector, SMatrix
using LinearAlgebra: cross, normalize

# export Vec3,
#     unitX3,
#     unitY3,
#     unitZ3,
#     Vec4,
#     unitX4,
#     unitY4,
#     unitZ4,
#     unitW4,
#     get_orthogonal_vectors,
#     Transform,
#     identitytransform,
#     right,
#     up,
#     forward,
#     # it was imported from OpticSim, not required?? IDK
#     #   origin TODO origin(t::Transform{<:Real}) = Vec3(t[1, 4], t[2, 4], t[3, 4])
#     rotationX,
#     rotationY,
#     rotationZ,
#     translation,
#     rotation,
#     rotationd,
#     translation,
#     scale,
#     local2world,
#     world2local,
#     decomposeRTS,
#     rotmatd,
#     rotmat

"""
`Vec3{T}` provides an immutable vector of fixed length 3 and type `T`.

`Vec3` defines a series of convenience constructors, so you can just type e.g. `Vec3(1, 2, 3)` or `Vec3([1.0, 2.0, 3.0])`.
It also supports comprehensions, and the `zeros()`, `ones()`, `fill()`, `rand()` and `randn()` functions, such as `Vec3(rand(3))`.
"""
Vec3{T} = SVector{3, T}

# empty constructor - initialized with zeros
Vec3(::Type{T} = Float64) where {T <: Real} = zeros(Vec3{T})

"""
returns the unit vector `[1, 0, 0]`
"""
function unitX3(::Type{T} = Float64) where {T <: Real}
    return Vec3{T}(one(T), zero(T), zero(T))
end
"""
returns the unit vector `[0, 1, 0]`
"""
function unitY3(::Type{T} = Float64) where {T <: Real}
    return Vec3{T}(zero(T), one(T), zero(T))
end
"""
returns the unit vector `[0, 0, 1]`
"""
function unitZ3(::Type{T} = Float64) where {T <: Real}
    return Vec3{T}(zero(T), zero(T), one(T))
end

"""
`Vec4{T}` provides an immutable vector of fixed length 4 and type `T`.

`Vec4` defines a series of convenience constructors, so you can just type e.g. `Vec3(1, 2, 3, 4)` or `Vec3([1.0, 2.0, 3.0, 4.0])`.
It also supports comprehensions, and the `zeros()`, `ones()`, `fill()`, `rand()` and `randn()` functions, such as `Vec4(rand(4))`.
"""
Vec4{T} = SVector{4, T}

# empty constructor - initialized with zeros
Vec4(::Type{T} = Float64) where {T <: Real} = zeros(Vec4{T})

"""
    Vec4(v::SVector{3, T}) where {T<:Real} -> Vec4{T}

Accept `SVector` and create a `Vec4` type [v[1], v[2], v[3], 1]
"""
function Vec4(v::SVector{3, T}) where {T <: Real}
    return Vec4{T}(v[1], v[2], v[3], one(T))
end

"""
    Vec4(m::SMatrix{3,N,T} where{N,T<:Real} -> SMatrix{3,N,T})
Input is matrix of 3d points, each column is one point. Returns matrix of 3d points with 1 appended in the last row.
"""
function Vec4(m::SMatrix{3, N, T}) where {N, T <: Real}
    return vcat(m, ones(SMatrix{1, N, T}))::SMatrix{4, N, T}
end
"""
returns the unit vector `[1, 0, 0, 0]`
"""
function unitX4(::Type{T} = Float64) where {T <: Real}
    return Vec4{T}(one(T), zero(T), zero(T), zero(T))
end
"""
returns the unit vector `[0, 1, 0, 0]`
"""
function unitY4(::Type{T} = Float64) where {T <: Real}
    return Vec4{T}(zero(T), one(T), zero(T), zero(T))
end
"""
returns the unit vector `[0, 0, 1, 0]`
"""
function unitZ4(::Type{T} = Float64) where {T <: Real}
    return Vec4{T}(zero(T), zero(T), one(T), zero(T))
end
"""
returns the unit vector `[0, 0, 0, 1]`
"""
function unitW4(::Type{T} = Float64) where {T <: Real}
    return Vec4{T}(zero(T), zero(T), zero(T), one(T))
end

# utility function that given a vector, return 2 orthogonal vectors to that one
function get_orthogonal_vectors(direction::Vec3{T}) where {T <: Real}
    axis1 = normalize(direction)

    dp = dot(unitX3(T), axis1)
    # check the special case where the input vector is parallel to the X axis
    if (dp >= one(T) - eps(T))
        # axis1 = unitX(T);
        axis2 = unitY3(T)
        axis3 = unitZ3(T)
        return (axis2, axis3)
    elseif (dp <= -one(T) + eps(T))
        # axis1 = -unitX(T);
        axis2 = -unitY3(T)
        axis3 = -unitZ3(T)
        return (axis2, axis3)
    end

    axis3 = normalize(cross(axis1, unitX3(T)))
    axis2 = normalize(cross(axis3, axis1))
    return (axis2, axis3)
end

#---------------------------------------
# 3D Transform / Local Frame
#---------------------------------------
"""
    Transform{S<:Real}

Transform encapsulating rotation, translation and scale in 3D space. Translation happens **after** rotation.

```julia
Transform{S}(θ::T, ϕ::T, ψ::T, x::T, y::T, z::T)
Transform(rotation::SMatrix{3,3,S}, translation::SVector{3,S})
Transform(rotation::AbstractArray{S,2}, translation::AbstractArray{S,1})
```
`θ`, `ϕ` and `ψ` in first constructor are in **radians**.
"""
struct Transform{T}
    matrix::SMatrix{4, 4, T, 16}

    """ This is a private internal function. In general don't want to allow users to populate Transform matrices with arbitrary elements. Not to be called by code outside of the Transform module. Don't use Transform{Float64}(...) for example. Instead use Transform(..)"""
    function Transform{T}(
        a11::T,
        a21::T,
        a31::T,
        a41::T,
        a12::T,
        a22::T,
        a32::T,
        a42::T,
        a13::T,
        a23::T,
        a33::T,
        a43::T,
        a14::T,
        a24::T,
        a34::T,
        a44::T,
    ) where {T <: Real}
        return new{T}(
            SMatrix{4, 4, T, 16}(
                a11,
                a21,
                a31,
                a41,
                a12,
                a22,
                a32,
                a42,
                a13,
                a23,
                a33,
                a43,
                a14,
                a24,
                a34,
                a44,
            ),
        )
    end

    Transform{T}(mat::SMatrix{4, 4, T, 16}) where {T <: Real} = new{T}(mat)
end

#functions to make Transform compatible with base matrix API
matrix(a::Transform) = a.matrix

# Base.length(a::Transform) = length(matrix(a))
function Base.getindex(a::Transform, indices::Vararg{Int, N}) where {N}
    return getindex(matrix(a), indices...)
end
# Base.iterate(a::Transform) = iterate(matrix(a))
# Base.iterate(a::Transform{Float64}, b::Tuple{StaticArrays.SOneTo{16}, Int64}) = iterate(matrix(a),b)

Base.collect(a::Transform) = collect(matrix(a))

function Base.:*(transa::Transform{T}, transb::Transform{T}) where {T <: Real}
    return Transform{T}(matrix(transa) * matrix(transb))
end

function Base.:*(
    transform::Transform{T},
    v::SVector{3, S},
) where {T <: Real, S <: Number}
    t = matrix(transform)

    res = t * Vec4(v)
    if (t[4, 4] == one(T))
        return SVector{3, S}(res[1], res[2], res[3])
    else
        return SVector{3, S}(res[1] / res[4], res[2] / res[4], res[3] / res[4])
    end
end

#Transform is not necessarily constrained to be a rigid body transformation so use general invers.
Base.inv(a::Transform{T}) where {T <: Real} = Transform{T}(inv(matrix(a)))

""" The t and m matrices are allowed to be of different element type. This allows transforming a Unitful matrix for example:
```
id = identitytransform()
m = fill(1mm,3,4)
id*m #returns a matrix filled with Unitful quantities. If both matrices had to be the same type this would not work
```
"""
function Base.:*(
    transform::Transform{T},
    m::SMatrix{3, N, S},
) where {N, T <: Real, S <: Number}
    res = MMatrix{3, N, T}(undef)
    t = matrix(transform)

    for outcol in 1:N
        for row in 1:3
            sum = T(0)
            for incol in 1:3
                sum += t[row, incol] * m[incol, outcol]
            end
            #implicit 1 w coordinate value
            sum += t[row, 4]
            res[row, outcol] = sum
        end
        if t[4, 4] != 1
            res[:, outcol] /= t[4, 4]
        end
    end
    return SMatrix{3, N, S}(res)
end

""" The t and m matrices are allowed to be of different element type. This allows transforming a Unitful matrix for example:
WARNING: this doesn't work. The translation component of the transform matrix has to be in Unitful units but the rotation part has to be in unitless units for this to work. Only works if one assumes that the translation part of the transform implicitly has the same units as the Unitful vectors being transformed. Brittle and likely to cause obscure bugs.
```
id = identitytransform()
m = fill(1mm,3,4)
id*m #returns a matrix filled with Unitful quantities. If both matrices had to be the same type this would not work
```
"""
function Base.:*(
    transform::Transform{T},
    m::SMatrix{4, N, S},
) where {N, T <: Real, S <: Number}
    return matrix(transform) * m
end

Base.transpose(a::Transform{T}) where {T <: Real} = Transform{T}(a.matrix')

# END of functions for compatibility with base matrix API

# for compatability ith the "old" RigidBodyTransform
"""
identitytransform([S::Type]) -> Transform{S}

Returns the [`Transform`](@ref) of type `S` (default `Float64`) representing the identity transform.
"""
function identitytransform(::Type{T} = Float64) where {T <: Real}
    return Transform{T}(
        one(T),
        zero(T),
        zero(T),
        zero(T),
        zero(T),
        one(T),
        zero(T),
        zero(T),
        zero(T),
        zero(T),
        one(T),
        zero(T),
        zero(T),
        zero(T),
        zero(T),
        one(T),
    )
end

"""
    Transform([S::Type]) -> Transform{S}

Returns the [`Transform`](@ref) of type `S` (default `Float64`) representing the identity transform.
"""
function Transform(::Type{T} = Float64) where {T <: Real}
    return identitytransform(T)
end

"""
    Transform(colx::Vec3{T}, coly::Vec3{T},colz::Vec3{T}, colw::Vec3{T}, ::Type{T} = Float64) where {T<:Real}

Costruct a transform from the input columns.
"""
function Transform(
    colx::Vec3{T},
    coly::Vec3{T},
    colz::Vec3{T},
    colw::Vec3{T} = zero(Vec3{T}),
) where {T <: Real}
    return Transform{T}(
        vcat(
            hcat(colx, coly, colz, colw),
            SMatrix{1, 4, T}(zero(T), zero(T), zero(T), one(T)),
        ),
    )
end

"""
    Transform(colx::Vec3{T}, coly::Vec3{T},colz::Vec3{T}, colw::Vec3{T}, ::Type{T} = Float64) where {T<:Real}

Costruct a transform from the input columns.
"""
function Transform(
    colx::Vec4{T},
    coly::Vec4{T},
    colz::Vec4{T},
    colw::Vec4{T},
) where {T <: Real}
    return Transform{T}(hcat(colx, coly, colz, colw))
end

"""
    Transform(origin, forward) -> Transform{S}

Returns the [`Transform`](@ref) of type `S` (default `Float64`) representing the local frame with origin and forward direction. the other 2 axes are computed automaticlly.
"""
function Transform(
    origin::Vec3{T},
    forward::Vec3{T} = unitZ3(),
) where {T <: Real}
    forward = normalize(forward)
    right, up = get_orthogonal_vectors(forward)
    return Transform(right, up, forward, origin)
end

function Transform(θ::T, ϕ::T, ψ::T, x::T, y::T, z::T) where {T <: Number}
    return Transform(rotmat(T, θ, ϕ, ψ), Vec3{T}(x, y, z))
end

"""
    Transform(rotation::SMatrix{3,3,T}, translation::SVector{3,T}) where {T<:Real} -> Transform{S}

Returns the [`Transform`](@ref) of type `S` (default `Float64`) created by a rotation matrix and translation vector.
"""
function Transform(
    rotation::SMatrix{3, 3, T},
    translation::SVector{3, T},
) where {T <: Real}
    return Transform{T}(
        rotation[1, 1],
        rotation[2, 1],
        rotation[3, 1],
        zero(T),
        rotation[1, 2],
        rotation[2, 2],
        rotation[3, 2],
        zero(T),
        rotation[1, 3],
        rotation[2, 3],
        rotation[3, 3],
        zero(T),
        translation[1],
        translation[2],
        translation[3],
        one(T),
    )
end

"""
    Transform(rotation::AbstractArray{T,2}, translation::AbstractArray{T,1}) where {T<:Real} -> Transform{S}

Returns the [`Transform`](@ref) of type `S` (default `Float64`) created by a rotation matrix (3x3) and translation vector of length 3.
"""
function Transform(
    rotation::AbstractArray{T, 2},
    translation::AbstractArray{T, 1},
) where {T <: Real}
    @assert size(rotation)[1] == size(rotation)[2] == length(translation) == 3
    return Transform{T}(
        rotation[1, 1],
        rotation[2, 1],
        rotation[3, 1],
        zero(T),
        rotation[1, 2],
        rotation[2, 2],
        rotation[3, 2],
        zero(T),
        rotation[1, 3],
        rotation[2, 3],
        rotation[3, 3],
        zero(T),
        translation[1],
        translation[2],
        translation[3],
        one(T),
    )
end

# define some utility functions

"""
    right(t::Transform{<:Real}) -> Vec3

Assuming t is a 3D rigid transform representing a local left-handed coordinate system, this function will return the first column, representing the "X" axis.
"""
right(t::Transform{<:Real}) = normalize(Vec3(t[1, 1], t[2, 1], t[3, 1]))

"""
    up(t::Transform{<:Real}) -> Vec3

Assuming t is a 3D rigid transform representing a local left-handed coordinate system, this function will return the second column, representing the "Y" axis.
"""
up(t::Transform{<:Real}) = normalize(Vec3(t[1, 2], t[2, 2], t[3, 2]))

"""
    forward(t::Transform{<:Real}) -> Vec3

Assuming t is a 3D rigid transform representing a local left-handed coordinate system, this function will return the third column, representing the "Z" axis.
"""
forward(t::Transform{<:Real}) = normalize(Vec3(t[1, 3], t[2, 3], t[3, 3]))

# """
#     origin(t::Transform{<:Real}) -> Vec3
#
# Assuming t is a 3D rigid transform representing a local left-handed coordinate system, this function will return the fourth column, containing the translation part of the transform in 3D.
# """
# OpticSim.origin(t::Transform{<:Real}) = Vec3(t[1, 4], t[2, 4], t[3, 4])

"""
    rotationX(angle::T) where {T<:Real} -> Transform

Builds a rotation matrix for a rotation around the x-axis.
Parameters:
    The counter-clockwise `angle` in radians.
"""
function rotationX(angle::T) where {T <: Real}
    c = cos(angle)
    s = sin(angle)

    row1 = unitX4()
    row2 = Vec4(zero(T), c, s, zero(T))
    row3 = Vec4(zero(T), -s, c, zero(T))
    row4 = unitW4()

    # transposing because the constructors treat these vectors as columns instead of rows
    return transpose(Transform(row1, row2, row3, row4))
end

"""
    rotationY(angle::T) where {T<:Real} -> Transform

Builds a rotation matrix for a rotation around the y-axis.
Parameters:
    The counter-clockwise `angle` in radians.
"""
function rotationY(angle::T) where {T <: Real}
    c = cos(angle)
    s = sin(angle)

    row1 = Vec4(c, zero(T), -s, zero(T))
    row2 = unitY4()
    row3 = Vec4(s, zero(T), c, zero(T))
    row4 = unitW4()

    # transposing because the constructors treat these vectors as columns instead of rows
    return transpose(Transform(row1, row2, row3, row4))
end

"""
    rotationZ(angle::T) where {T<:Real} -> Transform

Builds a rotation matrix for a rotation around the z-axis.
Parameters:
    The counter-clockwise `angle` in radians.
"""
function rotationZ(angle::T) where {T <: Real}
    c = cos(angle)
    s = sin(angle)

    row1 = Vec4(c, s, zero(T), zero(T))
    row2 = Vec4(-s, c, zero(T), zero(T))
    row3 = unitZ4()
    row4 = unitW4()

    # transposing because the constructors treat these vectors as columns instead of rows
    return transpose(Transform(row1, row2, row3, row4))
end

"""
    rotation(t::Transform{T}) where {T<:Real} -> SMatrix{3,3,T}

returns the rotation part of the transform `t` - a 3x3 matrix.
"""
function rotation(t::Transform{T}) where {T <: Real}
    rot = SMatrix{3, 3, T}(
        t[1, 1],
        t[2, 1],
        t[3, 1],
        t[1, 2],
        t[2, 2],
        t[3, 2],
        t[1, 3],
        t[2, 3],
        t[3, 3],
    )
    return Transform(rot, zero(Vec3{T}))
end

"""
    rotate(a::Transform{T}, vector::Union{Vec3{T}, SVector{3,T}}) where {T<:Real} -> Vec3{T}

apply the rotation part of the transform `a` to the vector `vector` - this operation is usually used to rotate direction vectors.
"""
function rotate(
    a::Transform{T},
    vector::Union{Vec3{T}, SVector{3, T}},
) where {T <: Real}
    return rotation(a) * vector
end

"""
    rotation([S::Type], θ::T, ϕ::T, ψ::T) -> Transform{S}

Returns the [`Transform`](@ref) of type `S` (default `Float64`) representing the rotation by `θ`, `ϕ` and `ψ` around the *x*, *y* and *z* axes respectively **in radians**.
"""
rotation(θ::T, ϕ::T, ψ::T) where {T <: Number} = rotation(Float64, θ, ϕ, ψ)
function rotation(::Type{S}, θ::T, ϕ::T, ψ::T) where {T <: Number, S <: Real}
    return Transform(rotmat(S, θ, ϕ, ψ), zeros(SVector{3, S}))
end
"""
    rotationd([S::Type], θ::T, ϕ::T, ψ::T) -> Transform{S}

Returns the [`Transform`](@ref) of type `S` (default `Float64`) representing the rotation by `θ`, `ϕ` and `ψ` around the *x*, *y* and *z* axes respectively **in degrees**.
"""
rotationd(θ::T, ϕ::T, ψ::T) where {T <: Number} = rotationd(Float64, θ, ϕ, ψ)
function rotationd(::Type{S}, θ::T, ϕ::T, ψ::T) where {T <: Number, S <: Real}
    return Transform(rotmatd(S, θ, ϕ, ψ), zeros(SVector{3, S}))
end
export translation, rotation, rotationd
"""
    translation(x::T, y::T, z::T) where {T<:Real}

Creates a translation transform
"""
function translation(::Type{S}, x::T, y::T, z::T) where {T <: Number, S <: Real}
    return convert(Transform{S}, translation(x, y, z))
end

function translation(x::T, y::T, z::T) where {T <: Real}
    col1 = unitX4(T)
    col2 = unitY4(T)
    col3 = unitZ4(T)
    col4 = Vec4(x, y, z, one(T))

    return Transform(col1, col2, col3, col4)
end

"""
    translation(x::T, y::T, z::T) where {T<:Real}

Creates a translation transform
"""
function translation(t::Vec3{T}) where {T <: Real}
    return translation(t[1], t[2], t[3])
end

"""
    scale(x::T, y::T, z::T) where {T<:Real}

Creates a scaling transform
"""
function scale(x::T, y::T, z::T) where {T <: Real}
    col1 = unitX4(T) * x
    col2 = unitY4(T) * y
    col3 = unitZ4(T) * z
    col4 = unitW4(T)

    return Transform(col1, col2, col3, col4)
end

"""
    scale(s::T) where {T<:Real}

Creates a uniform scaling transform
"""
function scale(s::T) where {T <: Real}
    return scale(s, s, s)
end

"""
    scale(t::Vec3{T}) where {T<:Real}

Creates a scaling transform
"""
function scale(t::Vec3{T}) where {T <: Real}
    return scale(t[1], [2], [3])
end

"""
    local2world(t::Transform{T}) where {T<:Real}

return the transform matrix that takes a point in the local coordinate system to the global one
"""
function local2world(t::Transform{T}) where {T <: Real}
    return t
end

"""
    world2local(t::Transform{T}) where {T<:Real}

return the transform matrix that takes a point in the global coordinate system to the local one
"""
function world2local(t::Transform{T}) where {T <: Real}
    return inv(t)
end

"""
    decomposeRTS(tr::Transform{T}) where {T<:Real}

return a touple containing the rotation matrix, the translation vector and the scale vecto represnting the transform.
"""
function decomposeRTS(tr::Transform{T}) where {T <: Real}
    t = Vec3(tr[1, 4], tr[2, 4], tr[3, 4])
    sx = norm(Vec3(tr[1, 1], tr[2, 1], tr[3, 1]))
    sy = norm(Vec3(tr[1, 2], tr[2, 2], tr[3, 2]))
    sz = norm(Vec3(tr[1, 3], tr[2, 3], tr[3, 3]))
    s = Vec3(sx, sy, sz)
    rot = SMatrix{4, 4, T}(
        tr[1, 1] / sx,
        tr[2, 1] / sx,
        tr[3, 1] / sx,
        0,
        tr[1, 2] / sy,
        tr[2, 2] / sy,
        tr[3, 2] / sy,
        0,
        tr[1, 3] / sz,
        tr[2, 3] / sz,
        tr[3, 3] / sz,
        0,
        0,
        0,
        0,
        1,
    )

    return rot, t, s
end

"""
    rotmatbetween([S::Type], a::SVector{3,T}, b::SVector{3,T}) -> SMatrix{3,3,S}

Returns the rotation matrix of type `S` (default `Float64`) representing the rotation between vetors `a` and `b`, i.e. rotation(a,b) * a = b.
"""
function rotmatbetween(a::SVector{3, T}, b::SVector{3, T}) where {T <: Real}
    return rotmatbetween(Float64, a, b)
end
function rotmatbetween(
    ::Type{S},
    a::SVector{3, T},
    b::SVector{3, T},
) where {T <: Real, S <: Real}
    # TODO: Brian, is there a hidden assumption that a and b are normalized?
    v = cross(a, b)
    c = dot(a, b)
    V = SMatrix{3, 3, S, 9}(0, v[3], -v[2], -v[3], 0, v[1], v[2], -v[1], 0)
    R = I + V + V^2 * one(T) / (one(T) + c)
    return SMatrix{3, 3, S, 9}(R)
end

"""
    rotmatd([S::Type], θ::T, ϕ::T, ψ::T) -> SMatrix{3,3,S}

Returns the rotation matrix of type `S` (default `Float64`) representing the rotation by `θ`, `ϕ` and `ψ` around the *x*, *y* and *z* axes respectively **in degrees**.
"""
function rotmatd(θ::T, ϕ::T, ψ::T) where {T <: Number}
    return rotmat(Float64, deg2rad(θ), deg2rad(ϕ), deg2rad(ψ))
end
function rotmatd(::Type{S}, θ::T, ϕ::T, ψ::T) where {T <: Number, S <: Real}
    return rotmat(S, deg2rad(θ), deg2rad(ϕ), deg2rad(ψ))
end
"""
    rotmat([S::Type], θ::T, ϕ::T, ψ::T) -> SMatrix{3,3,S}

Returns the rotation matrix of type `S` (default `Float64`) representing the rotation by `θ`, `ϕ` and `ψ` around the *x*, *y* and *z* axes respectively **in radians**.
"""
rotmat(θ::T, ϕ::T, ψ::T) where {T <: Number} = rotmat(Float64, θ, ϕ, ψ)
function rotmat(::Type{S}, θ::T, ϕ::T, ψ::T) where {T <: Number, S <: Real}
    sinψ = sin(ψ)
    sinϕ = sin(ϕ)
    sinθ = sin(θ)
    cosψ = cos(ψ)
    cosϕ = cos(ϕ)
    cosθ = cos(θ)
    return SMatrix{3, 3, S, 9}(
        cosψ * cosϕ,
        sinψ * cosϕ,
        -sinϕ,
        cosψ * sinϕ * sinθ - sinψ * cosθ,
        sinψ * sinϕ * sinθ + cosψ * cosθ,
        cosϕ * sinθ,
        cosψ * sinϕ * cosθ + sinψ * sinθ,
        sinψ * sinϕ * cosθ - cosψ * sinθ,
        cosϕ * cosθ,
    )
end

end # module


using Plots

data = readdlm(IOBuffer("""
    1 |           524288 |        1.026098 |     0.000510953
    2 |           524288 |        1.285015 |     0.000408001
    4 |           524288 |        1.286281 |     0.000407600
    8 |           524288 |        1.165687 |     0.000449767
   16 |           524288 |        1.103974 |     0.000474910
   32 |           524288 |        1.205526 |     0.000434904
   64 |           524288 |        1.112961 |     0.000471075
   """), '|')

data[:,4]*=524288/4096 #Correct reporting error

plot([1,64], [data[1,4], data[1,4]*64], c=:lightgrey, label="ideal")
plot!(data[:,1], data[:,4], xlabel="# cores", ylabel="Gflops", c=:blue, label="",
	xlims=(0,64), ylims=(0,0.1))
scatter!(data[:,1], data[:,4], c=:blue, label="")
png("../plots/randomupdate")


using Test
using Smoothers
import Smoothers: hmaSymmetricWeights

@testset "hma" begin

    # Types' promotion
    fx = hma(Float32.(1:10),7);
    @test eltype(fx) == Float32
    fx = hma(Float64.(1:10),7);
    @test eltype(fx) == Float64
    fx = hma(BigFloat.(1:10),7);
    @test eltype(fx) == BigFloat
    fx = hma(1:10,7);
    @test eltype(fx) == Float64

    # weights
    w = hmaSymmetricWeights(8,Float64)
    @test ismissing(w[end])

    w = hmaSymmetricWeights(7,Float64)
    @test sum(w) ≈ 1.0

    w = hmaSymmetricWeights(13,Float64)
    @test sum(w) ≈ 1.0
    # (https://www.mathworks.com/help/econ/seasonal-adjustment-using-snxd7m-seasonal-filters.html)
    @test length(w) == 13
    @test round.(w,digits=3) == [-0.019, -0.028, 0.0, 0.065, 0.147, 0.214, 0.24,
                                  0.214, 0.147, 0.065, 0.0, -0.028, -0.019]
    # 13-term moving average
    x = hma(sin.(1:100), 13)
    @test length(x) == 100
    @test first(x)  ≈ 0.5636212576875559
    @test last(x)   ≈ -0.6658500507547161
    @test x[50]     ≈ -0.043655947941143455

end


module spectralanalysis

using ..states, ..operators, ..operators_sparse

export operatorspectrum, operatorspectrum_hermitian, eigenstates, eigenstates_hermitian, groundstate

"""
Calculate the spectrum of a Hermitian operator.

Arguments
---------

H
    Sparse or dense operator.

Keyword arguments
-----------------

Nmax (optional)
    Number of eigenvalues that should be calculated.
"""
function operatorspectrum_hermitian(H::DenseOperator; Nmax::Union{Int, Void}=nothing)
    h = Hermitian(H.data)
    return Nmax == nothing ? eigvals(h) : eigvals(h, 1:Nmax)
end

operatorspectrum_hermitian(H::SparseOperator; Nmax::Union{Int, Void}=nothing) = real(operatorspectrum(H; Nmax=Nmax))

"""
Calculate the spectrum of a not necessarily Hermitian operator.

If the operator is known to be Hermitian use
:func:`operatorspectrum_hermitian(::DenseOperator)` instead.

Arguments
---------

H
    Sparse or dense operator.

Keyword arguments
-----------------

Nmax (optional)
    Number of eigenvalues that should be calculated.
"""
function operatorspectrum(H::DenseOperator; Nmax::Union{Int, Void}=nothing)
    if ishermitian(H.data)
        return operatorspectrum_hermitian(H; Nmax=Nmax)
    end
    s = eigvals(H.data)
    return Nmax == nothing ? s : s[1:Nmax]
end

function operatorspectrum(H::SparseOperator; Nmax::Union{Int, Void}=nothing)
    if Nmax == nothing
        Nmax = size(H.data, 2) - 2
    end
    d, nconv, niter, nmult, resid = eigs(H.data; nev=Nmax, which=:SR, ritzvec=false)
    return d
end

"""
Calculate the eigenstates of a Hermitian operator.

Arguments
---------

H
    Sparse or dense operator.

Keyword arguments
-----------------

Nmax (optional)
    Number of eigenstates that should be calculated.
"""
function eigenstates_hermitian(H::DenseOperator; Nmax::Union{Int, Void}=nothing)
    h = Hermitian(H.data)
    M = Nmax == nothing ? eigvecs(h) : eigvecs(h, 1:Nmax)
    b = Ket[]
    for k=1:size(M,2)
        push!(b, Ket(H.basis_r, M[:,k]))
    end
    return b
end

"""
Calculate the eigenstates of a not necessarily Hermitian operator.

If the operator is known to be Hermitian use
:func:`eigenstates_hermitian(::DenseOperator)` instead.

Arguments
---------

H
    Sparse or dense operator.

Keyword arguments
-----------------

Nmax (optional)
    Number of eigenstates that should be calculated.
"""
function eigenstates(H::DenseOperator; Nmax::Union{Int, Void}=nothing)
    if ishermitian(H.data)
        return eigenstates_hermitian(H; Nmax=Nmax)
    end
    M = eigvecs(H.data)
    b = Ket[]
    for k=1:size(M,2)
        if Nmax!=nothing && k>Nmax
            break
        end
        push!(b, Ket(H.basis_r, M[:,k]))
    end
    return b
end

eigenstates_hermitian(H::SparseOperator; Nmax::Union{Int, Void}=nothing) = eigenstates(H; Nmax=Nmax)

function eigenstates(H::SparseOperator; Nmax::Union{Int, Void}=nothing)
    if Nmax == nothing
        Nmax = size(H.data, 2) - 2
    end
    d, M, nconv, niter, nmult, resid = eigs(H.data; nev=Nmax, which=:SR, ritzvec=true)
    b = Ket[]
    for k=1:size(M,2)
        push!(b, Ket(H.basis_r, M[:,k]))
    end
    return b
end

"""
Calculate the ground-state of a Hermitian operator.

This is just a shortcut for :func:`eigenstates_hermitian(H, Nmax=1)`

Arguments
---------

H
    Sparse or dense operator.

Keyword arguments
-----------------

Nmax (optional)
    Number of eigenstates that should be calculated.
"""
groundstate(H::Union{DenseOperator, SparseOperator}) = eigenstates_hermitian(H; Nmax=1)[1]

end # module


function ord(a, p)
    orders = factor(p - 1)
    for x in orders
        if pow(a, x)%p == 1
            return x
        end
    end
end


function WeakRemoverModel(A,b,C,d,XLB,XUB,VariableDef,Y_bar1,YR)

    const EPS = 1e-5;

    m,n = size(A);
    nObj = size(C)[1];
    MyModel = Model();
    @variable(MyModel,XLB[i] <= X[i=1:n] <= XUB[i],category = VariableType(VariableDef,i))#
    @variable(MyModel,Z)

    @objective(MyModel,Min, sum(sum(C[j,i]*X[i] for i = 1:n) + d[j] for j =1:nObj) )
    for i = 1:m
         @constraint(MyModel,sum(A[i,j]*X[j] for j = 1:n) >= b[i] )
    end
    for j = 1:nObj
         @constraint(MyModel,sum(C[j,i]*X[i] for i = 1:n) + d[j]  <= Y_bar1[j] + EPS)
    end

    for j = 1:nObj
         @constraint(MyModel,sum(C[j,i]*X[i] for i = 1:n) + d[j] >= YR[j]  )
    end
    return MyModel;

end


function constraint_elimination(p :: Phs)
	# Given a constrained Phs with N state variables
	#     and Nconst constraints lambda:
	#                          Xdot = J GradH + B u + G lambda
	#                          y = B' GradH + D u
	#                          0 = G' GradH
	# this function finds an equivalent Phs with reduced order (N-Nconst)
	# without the constraints lambda. In the linear case, it is equivalent
	# to:
	#                  z1dot = Jnew Qnew z1 + Bnew u
	#                  y = Bnew' Qnew z1 + D u
	#
	# the change of variables is given by: X = M z
	#
	if isdefined(p,:Q)
		if (p.G_D .< eps()) == trues(size(p.G_D))
			nstates = size(p.Q,1)
			nconst = size(p.G,2)
			Gnull = transpose(nullspace(copy.(transpose(p.G))))[:,:]
			M = [Gnull; inv((transpose(p.G)*p.G)[1])*transpose(p.G)]
			Jtilde = M*p.J*transpose(M)
			#Minv = inv(M)
			#Qtilde = Minv'*p.Q*Minv
			Qtilde = transpose(M) \ (p.Q / M)
			J11 = Jtilde[1:nstates-nconst,1:nstates-nconst]
			Q11 = Qtilde[1:nstates-nconst,1:nstates-nconst]
			Q12 = Qtilde[1:nstates-nconst,(end-nconst+1):end]
			Q21 = Qtilde[(end-nconst+1):end,1:nstates-nconst]
			Q22 = Qtilde[(end-nconst+1):end,(end-nconst+1):end]
			#Qn = Q11-Q12* inv(Q22)*Q21
			Qn = Q11-Q12* (Q22\Q21)
			II = Matrix(1I, nstates-nconst, nstates-nconst); # idendity
			TransfMatrix = M \ [II;-(Q22\Q21)]
			newphs = Phs(J11, Gnull * p.B, p.D, Qn)
			newphs.TransfMatrix = p.TransfMatrix*TransfMatrix
			newphs.StatesNames = p.StatesNames

			# damping matrix:
			Rtilde = M*p.R*transpose(M)
			Rtilde11 = Rtilde[1:nstates-nconst,1:nstates-nconst]
			newphs.R = Rtilde11

			return newphs
		elseif (det(p.G_D) > 1e8*eps(norm(p.J)))  # G_D is invertible
		## Constrained systems with non-zero, invertible, direct link
			# Given a constrained Phs with N state variables
			#     and Nconst constraints lambda:
			#                          Xdot = J GradH + B u + G lambda
			#                          y = B' GradH + D u
			#                          0 = G' GradH  + G_D lambda
			# this function finds an equivalent Phs without the constraints lambda.
			# In the linear case, it is equivalent
			# to:
			#                  zdot = Jnew Qnew z + Bnew u
			#                  y = Bnew' Qnew z + Dnew u
			nconst = size(p.G,2)
			println("non-zero, invertible G_D:");
			Gnull = transpose(nullspace(copy.(transpose(p.G))));
			M = [Gnull; inv((transpose(p.G)*p.G)[1])*transpose(p.G)];
			Jtilde = M*p.J*transpose(M);
			#Minv = inv(M);
			Qtilde = transpose(M)\ p.Q /M;
			JD = zeros(size(p.J));
			JD[(end-nconst+1):end,(end-nconst+1):end] = inv(p.G_D);
			return Phs(Jtilde-JD, M*p.B, p.D, Qtilde)
		else
			println("non-zero, non-invertible G_D! don't know how to deal with it yet.");
		end
	else

		println("Nonlinear system. Not working yet.")
	end
end

## TO DO:
## more general case of direct links and constraints
	# Given a constrained Phs with N state variables
	#     and Nconst constraints lambda:
	#                          Xdot = J GradH + B u + G lambda
	#                          y = B' GradH + D u + D_lambda lambda
	#                          0 = G' GradH  + G_D lambda + G_u u
	# this function finds an equivalent Phs without the constraints lambda.
	# In the linear case, it is equivalent
	# to:
	#                  zdot = Jnew Qnew z + Bnew u
	#                  y = Bnew' Qnew z + Dnew u
	#

## TO DO: system inversion:
## invert input/ouput of systems with non-zero, invertible D matrix:


using ChaosTools, PkgBenchmark

@benchgroup "lyapunov_discrete" begin
        ds = Systems.henon()
        @bench "henon" lyapunov($ds, 10000,
        d0 = 1e-9, threshold = 1e-6, Ttr = 100)

        ds = Systems.towel()
        @bench "towel" lyapunov($ds, 10000,
        d0 = 1e-9, threshold = 1e-6, Ttr = 100)

        ds = Systems.coupledstandardmaps(5)
        @bench "CSM" lyapunov($ds, 10000,
        d0 = 1e-9, threshold = 1e-6, Ttr = 100)
    end

@benchgroup "lyapunov_continuous" begin
    ds = Systems.lorenz()
    @bench "lorenz" lyapunov($ds, 1000.0, d0 = 1e-9, threshold = 1e-6)
    @bench "lorenz2" lyapunov($ds, 1000.0, d0 = 1e-9, threshold = 1e-6)

    ds = Systems.duffing(β = -1, ω = 1, f = 0.3)
    @bench "duffing" lyapunov($ds, 1000.0, d0 = 1e-9, threshold = 1e-6)
    @bench "duffing2" lyapunov($ds, 1000.0, d0 = 1e-9, threshold = 1e-6)
end


module CGS
    SpeedOfLight = 2.99792458e10;                           # cm / s
    GravitationalConstant = 6.673e-8;                       # cm^3 / g s^2
    PlancksConstantH = 6.62606896e-27;                      # g cm^2 / s
    PlancksConstantHbar = 1.05457162825e-27;                # g cm^2 / s
    AstronomicalUnit = 1.49597870691e13;                    # cm
    LightYear = 9.46053620707e17;                           # cm
    Parsec = 3.08567758135e18;                              # cm
    GravAccel = 9.80665e2;                                  # cm / s^2
    ElectronVolt = 1.602176487e-12;                         # g cm^2 / s^2
    MassElectron = 9.10938188e-28;                          # g
    MassMuon = 1.88353109e-25;                              # g
    MassProton = 1.67262158e-24;                            # g
    MassNeutron = 1.67492716e-24;                           # g
    Rydberg = 2.17987196968e-11;                            # g cm^2 / s^2
    Boltzmann = 1.3806504e-16;                              # g cm^2 / K s^2
    MolarGas = 8.314472e7;                                  # g cm^2 / K mol s^2
    StandardGasVolume = 2.2710981e4;                        # cm^3 / mol
    Minute = 6e1;                                           # s
    Hour = 3.6e3;                                           # s
    Day = 8.64e4;                                           # s
    Week = 6.048e5;                                         # s
    Inch = 2.54e0;                                          # cm
    Foot = 3.048e1;                                         # cm
    Yard = 9.144e1;                                         # cm
    Mile = 1.609344e5;                                      # cm
    NauticalMile = 1.852e5;                                 # cm
    Fathom = 1.8288e2;                                      # cm
    Mil = 2.54e-3;                                          # cm
    Point = 3.52777777778e-2;                               # cm
    Texpoint = 3.51459803515e-2;                            # cm
    Micron = 1e-4;                                          # cm
    Angstrom = 1e-8;                                        # cm
    Hectare = 1e8;                                          # cm^2
    Acre = 4.04685642241e7;                                 # cm^2
    Barn = 1e-24;                                           # cm^2
    Liter = 1e3;                                            # cm^3
    UsGallon = 3.78541178402e3;                             # cm^3
    Quart = 9.46352946004e2;                                # cm^3
    Pint = 4.73176473002e2;                                 # cm^3
    Cup = 2.36588236501e2;                                  # cm^3
    FluidOunce = 2.95735295626e1;                           # cm^3
    Tablespoon = 1.47867647813e1;                           # cm^3
    Teaspoon = 4.92892159375e0;                             # cm^3
    CanadianGallon = 4.54609e3;                             # cm^3
    UkGallon = 4.546092e3;                                  # cm^3
    MilesPerHour = 4.4704e1;                                # cm / s
    KilometersPerHour = 2.77777777778e1;                    # cm / s
    Knot = 5.14444444444e1;                                 # cm / s
    PoundMass = 4.5359237e2;                                # g
    OunceMass = 2.8349523125e1;                             # g
    Ton = 9.0718474e5;                                      # g
    MetricTon = 1e6;                                        # g
    UkTon = 1.0160469088e6;                                 # g
    TroyOunce = 3.1103475e1;                                # g
    Carat = 2e-1;                                           # g
    UnifiedAtomicMass = 1.660538782e-24;                    # g
    GramForce = 9.80665e2;                                  # cm g / s^2
    PoundForce = 4.44822161526e5;                           # cm g / s^2
    KilopoundForce = 4.44822161526e8;                       # cm g / s^2
    Poundal = 1.38255e4;                                    # cm g / s^2
    Calorie = 4.1868e7;                                     # g cm^2 / s^2
    Btu = 1.05505585262e10;                                 # g cm^2 / s^2
    Therm = 1.05506e15;                                     # g cm^2 / s^2
    Horsepower = 7.457e9;                                   # g cm^2 / s^3
    Bar = 1e6;                                              # g / cm s^2
    StdAtmosphere = 1.01325e6;                              # g / cm s^2
    Torr = 1.33322368421e3;                                 # g / cm s^2
    MeterOfMercury = 1.33322368421e6;                       # g / cm s^2
    InchOfMercury = 3.38638815789e4;                        # g / cm s^2
    InchOfWater = 2.490889e3;                               # g / cm s^2
    Psi = 6.89475729317e4;                                  # g / cm s^2
    Poise = 1e0;                                            # g / cm s
    Stokes = 1e0;                                           # cm^2 / s
    Stilb = 1e0;                                            # cd / cm^2
    Lumen = 1e0;                                            # cd sr
    Lux = 1e-4;                                             # cd sr / cm^2
    Phot = 1e0;                                             # cd sr / cm^2
    Footcandle = 1.076e-3;                                  # cd sr / cm^2
    Lambert = 1e0;                                          # cd sr / cm^2
    Footlambert = 1.07639104e-3;                            # cd sr / cm^2
    Curie = 3.7e10;                                         # 1 / s
    Roentgen = 2.58e-7;                                     # A s / g
    Rad = 1e2;                                              # cm^2 / s^2
    SolarMass = 1.98892e33;                                 # g
    BohrRadius = 5.291772083e-9;                            # cm
    Newton = 1e5;                                           # cm g / s^2
    Dyne = 1e0;                                             # cm g / s^2
    Joule = 1e7;                                            # g cm^2 / s^2
    Erg = 1e0;                                              # g cm^2 / s^2
    StefanBoltzmannConstant = 5.67040047374e-5;             # g / K^4 s^3
    ThomsonCrossSection = 6.65245893699e-25;                # cm^2
end


type Fourier1d <: SuperRes #1D Static model
    freqs :: Vector{Int}
    filter :: Vector{Float64}
    x_max :: Float64
    approx_grid :: Vector{Float64}
    bounds :: Tuple{Vector{Float64},Vector{Float64}}
    function Fourier1d(f_c, x_max, filter = ones(2*f_c+1), n_approx = 10*f_c+1)
        # step to get rid of the unused frequencies
        ind = find(filter)
        freqs = -f_c:f_c
        filter = filter[ind]
        freqs = freqs[ind]
        new(freqs, filter, x_max,
            linspace(0, x_max, n_approx),
            ([0.0], [x_max]))
    end
end

function setBounds(model :: Fourier1d, bounds)
    model.bounds = bounds
    model.approx_grid = linspace(bounds[1][1], bounds[2][1], length(model.approx_grid))
end

function psi(model :: Fourier1d, theta :: Vector{Float64})
    # This function computes the direct problem for a single point theta
    psi_cpx =vec(Complex128[model.filter[k] * exp(-2im * pi * model.freqs[k] / model.x_max* theta[1])                                   for k in eachindex(model.freqs)])
    return [real(psi_cpx); imag(psi_cpx)]
end

function dpsi(model :: Fourier1d, theta :: Vector{Float64})
    # This function computes the gradient of psi
    dx = vec(Complex128[-model.filter[k] * model.freqs[k]/model.x_max* 2im * pi *
                        exp(-2im * pi * model.freqs[k] / model.x_max * theta[1]) for k in eachindex(model.freqs)])
    return reshape([real(dx); imag(dx)], 2*length(model.freqs), 1)
end

function getStartingPoint(model :: Fourier1d, v :: Vector{Float64})
    # This function gets an initial value for the location
    vals = Float64[dot(v,psi(model, [x])) for x = model.approx_grid]
    ind = indmin(vals);
    return [model.approx_grid[ind]]
end

parameterBounds(model :: Fourier1d) = model.bounds # Bounds for the parameters
dim(model :: Fourier1d) = 1

type DynamicFourier1d <: DynamicSuperRes #1D Space-time model
    static :: Fourier1d
    times :: Vector{Float64}
    v_max :: Float64
    grid_v :: Vector{Float64}
    bounds :: Tuple{Vector{Float64},Vector{Float64}}
    DynamicFourier1d(static, v_max, tau, K, num_v = 20) = new(static,
                                                              linspace(-tau*K, tau*K, 2*K+1),
                                                              v_max,
                                                              linspace(-v_max, v_max, num_v),
                                                              ([0; -v_max], [static.x_max; v_max]))
end
function setBounds(model :: DynamicFourier1d, bounds)
    setBounds(model.static, (bounds[1][1], bounds[2][1]))
    model.bounds = bounds
    model.grid_v = linspace(bounds[1][2], bounds[2][2], length(grid_v))
end

function getStartingPoint(model :: DynamicFourier1d, v :: Vector{Float64})
    # This function gets an initial value for the location
    vals = Float64[dot(v,psi(model, [x; vel])) for x = model.static.approx_grid, vel = model.grid_v]
    ind = indmin(vals);
    i,j = ind2sub(vals, ind)
    return [model.static.approx_grid[i]; model.grid_v[j]]
end

parameterBounds(model :: DynamicFourier1d) = model.bounds


using Base.Test
using MathProgBase
using MathProgBase.SolverInterface

function quadprogtest(solver=MathProgBase.defaultQPsolver)
    println("Testing quadprog with solver ", string(typeof(solver)))

    sol = quadprog([0., 0., 0.],[2. 1. 0.; 1. 2. 1.; 0. 1. 2.],[1. 2. 3.; 1. 1. 0.],'>',[4., 1.],-Inf,Inf,solver)
    @test sol.status == :Optimal
    @test_approx_eq_eps sol.objval 130/70 1e-6
    @test_approx_eq_eps norm(sol.sol[1:3] - [0.5714285714285715,0.4285714285714285,0.8571428571428572]) 0.0 1e-6

    let
    m = LinearQuadraticModel(solver)
    loadproblem!(m, [1. 2. 3.; 1. 1. 0.],[-Inf,-Inf,-Inf],[Inf,Inf,Inf],[0.,0.,0.],[4., 1.],[Inf,Inf], :Min)

    setquadobj!(m,diagm([10.0,10.0,10.0]))
    rows = [1, 2, 2, 2, 3, 3, 3]
    cols = [1, 1, 1, 2, 2, 3, 3]
    vals = Float64[2, 0.5, 0.5, 2, 1, 1, 1]
    setquadobj!(m,rows,cols,vals)
    optimize!(m)
    stat = status(m)
    @test stat == :Optimal
    @test_approx_eq_eps getobjval(m) 130/70 1e-6
    @test_approx_eq_eps norm(getsolution(m) - [0.5714285714285715,0.4285714285714285,0.8571428571428572]) 0.0 1e-6
    end

    let
    m = LinearQuadraticModel(solver)
    loadproblem!(m, [-1. 1.; 1. 1.], [0.,0.], [Inf,Inf], [1.,1.], [0.,0.], [Inf,Inf], :Max)
    addquadconstr!(m, [2], [1.], [1], [1], [1.], '<', 2)
    optimize!(m)
    stat = status(m)
    @test stat == :Optimal
    @test_approx_eq_eps getobjval(m) 2.25 1e-6
    @test_approx_eq_eps norm(getsolution(m) - [0.5,1.75]) 0.0 1e-3
    end
    println("Done")
end

function qpdualtest(solver=MathProgBase.defaultQPsolver)
    println("Testing QP duals with solver ", string(typeof(solver)))
    # max x
    # s.t. x^2 <= 2
    m = LinearQuadraticModel(solver)
    loadproblem!(m, Array(Float64,0,1), [-Inf], [Inf], [1.0], Float64[], Float64[], :Max)
    addquadconstr!(m, [], [], [1], [1], [1.0], '<', 2.0)
    optimize!(m)
    stat = status(m)

    @test numlinconstr(m) == 0
    @test numquadconstr(m) == 1
    @test numconstr(m) == 1
    @test stat == :Optimal
    @test_approx_eq_eps getobjval(m) sqrt(2) 1e-6
    @test_approx_eq_eps getsolution(m)[1] sqrt(2) 1e-6
    @test_approx_eq_eps getquadconstrduals(m)[1] 0.5/sqrt(2) 1e-6

    # min -x
    # s.t. x^2 <= 2
    m = LinearQuadraticModel(solver)
    loadproblem!(m, Array(Float64,0,1), [-Inf], [Inf], [-1.0], Float64[], Float64[], :Min)
    addquadconstr!(m, [], [], [1], [1], [1.0], '<', 2.0)
    optimize!(m)
    stat = status(m)

    @test numlinconstr(m) == 0
    @test numquadconstr(m) == 1
    @test numconstr(m) == 1
    @test stat == :Optimal
    @test_approx_eq_eps getobjval(m) -sqrt(2) 1e-6
    @test_approx_eq_eps getsolution(m)[1] sqrt(2) 1e-6
    @test_approx_eq_eps getquadconstrduals(m)[1] -0.5/sqrt(2) 1e-6

    println("Done")
end

function socptest(solver=MathProgBase.defaultQPsolver)
    println("Testing SOCP interface with solver ", string(typeof(solver)))

    # min t
    # s.t. x + y >= 1
    #      x^2 + y^2 <= t^2
    #      t >= 0
    m = LinearQuadraticModel(solver)
    loadproblem!(m, [ 1.0 1.0 0.0 ], [-Inf,-Inf,0.0], [Inf,Inf,Inf], [0.0,0.0,1.0], [1.0],[Inf], :Min)
    addquadconstr!(m, [], [], [1,2,3], [1,2,3], [1.0,1.0,-1.0],'<',0.0)
    optimize!(m)
    stat = status(m)

    @test stat == :Optimal
    @test_approx_eq_eps getobjval(m) sqrt(1/2) 1e-6
    @test_approx_eq_eps norm(getsolution(m) - [0.5,0.5,sqrt(1/2)]) 0.0 1e-3
    println("Done")

end