# Заполняет словарь d индексами индивидуальных значений
#=
function indsdict!(d::Dict{T}, cdata::Tuple) where T
    @inbounds for (i, element) in enumerate(zip(cdata...))
        ind = ht_keyindex(d, element)
        if ind > 0
            push!(d.vals[ind], i)
        else
            v = Vector{Int}(undef, 1)
            v[1] = i
            d[element] = v
        end
    end
    d
end
=#
nonunique(v) = [k for (k, v) in StatsBase.countmap(v) if v > 1]

function floatparse(data)
    v = Vector{Float64}(undef, length(data))
    @inbounds for i = 1:length(data)
        if !isa(data[i], AbstractFloat) && !ismissing(data[i])
            if isa(data[i], AbstractString)
                try
                    v[i] = parse(Float64, data[i])
                catch
                    v[i] = NaN
                end
            else
                try
                    v[i] = float(data[i])
                catch
                    v[i] = NaN
                end
            end
        elseif ismissing(data[i])
            v[i] = NaN
        else
            v[i] = data[i]
        end
    end
    identity.(v)
end

function checkvalues(timevals_sp, concvals_sp)
    if !(eltype(timevals_sp) <: Number)
        timevals_sp = identity.(timevals_sp)
        eltype(timevals_sp) <: Number || error("Some time values not a number!")
    end
    if !(eltype(concvals_sp) <: Union{Number, Missing})
        concvals_sp = identity.(concvals_sp)
        if !(eltype(concvals_sp) <: Union{Number, Missing})
            @warn "Some concentration values not a number, try to fix."
            concvals_sp = floatparse(concvals_sp)
        end
    end
    timevals_sp, concvals_sp
end

"""
    pkimport(data, time, conc, sort; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime())

Import PK data from table `data`.

* `time` - time column;
* `conc` - concentration column;
* `sort` - subject sorting columns.

keywords:

* `kelauto` - if `true` auto range settings, if `false` used `kelstart`/`kelend` from `elimrange`;
* `elimrange` - set elimination range settings;
* `dosetime` - set dose and dose time, by default dosetime = 0, dose is `NaN`.

!!! note

    If time column have non-unique values - last pair time-concentration will be used.

"""
function pkimport(data, time, conc, sort; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime())
    if isa(sort, String) sort = [Symbol(sort)] end
    if isa(sort, Symbol) sort = [sort] end

    Tables.istable(data) || error("Data not a table!")

    cols   = Tables.columns(data)
    cdata  = Tuple(Tables.getcolumn(cols, y) for y in sort)
    d      = Dict{Tuple{eltype.(cdata)...}, Vector{Int}}()
    indsdict!(d, cdata)

    timec = Tables.getcolumn(data, time)
    concc = Tables.getcolumn(data, conc)

    any(isnanormissing, timec) && error("Some time values is NaN or Missing!")

    sdata = Vector{PKSubject}(undef, length(d))
    i = one(Int)
    @inbounds for (k, v) in d
        timevals = timec[v]
        concvals = concc[v]
        if !allunique(timevals)
            @warn "Not all time values is unique, last observation used! ($k)"
            nuv = nonunique(timevals)
            nuvinds = findall(x -> x == first(nuv), timevals)[1:end-1]
            if length(nuv) > 1
                for cnt = 2:length(nuv)
                    append!(nuvinds, findall(x -> x == nuv[cnt], timevals)[1:end-1])
                end
            end
            sort!(nuvinds)
            deleteat!(timevals, nuvinds)
            deleteat!(concvals, nuvinds)
        end
        sp = sortperm(timevals)
        timevals_sp = timevals[sp]
        concvals_sp = concvals[sp]
        timevals_sp, concvals_sp = checkvalues(timevals_sp, concvals_sp)
        sdata[i] = PKSubject(timevals_sp, concvals_sp, kelauto, elimrange,  dosetime, Dict(sort .=> k))
        i += one(Int)
    end
    return DataSet(identity.(sdata))
end
"""
    pkimport(data, time, conc; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime())

Import PK data from tabular data `data`, `time` - time column, `conc` - concentration column.
"""
function pkimport(data, time, conc; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime())
    timevals_sp, concvals_sp = checkvalues(copy(Tables.getcolumn(data, time)), copy(Tables.getcolumn(data, conc)))
    pkimport(timevals_sp, concvals_sp; kelauto = kelauto,  elimrange = elimrange, dosetime = dosetime)
end
"""
    pkimport(time, conc; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime(), id = Dict{Symbol, Any}())

Import PK data from time vector `time` and concentration vector `conc`.
"""
function pkimport(time, conc; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime(), id = Dict{Symbol, Any}())
    timevals_sp, concvals_sp = checkvalues(copy(time), copy(conc))
    PKSubject(timevals_sp, concvals_sp, kelauto, elimrange,  dosetime, id)
end

"""
    upkimport(data, stime, etime, conc, vol, sort; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime())

Import urine PK data from table `data`.

* `stime` - start time column;
* `etime` - end time column;
* `conc` - concentration column;
* `vol` - volume column;
* `sort` - subject sorting columns.
"""
function upkimport(data, stime, etime, conc, vol, sort; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime())
    if isa(sort, String) sort = [Symbol(sort)] end
    if isa(sort, Symbol) sort = [sort] end

    cols   = Tables.columns(data)
    cdata  = Tuple(Tables.getcolumn(cols, y) for y in sort)
    d      = Dict{Tuple{eltype.(cdata)...}, Vector{Int}}()
    indsdict!(d, cdata)

    tnames = Symbol.(names(data))
    isa(stime, Symbol) || stime in tnames || error("column Start Time ($stime) not found")
    isa(etime, Symbol) || etime in tnames || error("column End Time ($etime) not found")
    isa(conc, Symbol) || conc in tnames || error("column Concentration ($conc) not found")
    isa(vol, Symbol) || vol in tnames || error("column Volume ($vol) not found")

    stimec = Tables.getcolumn(data, stime)
    etimec = Tables.getcolumn(data, etime)
    concc = Tables.getcolumn(data, conc)
    volc = Tables.getcolumn(data, vol)

    any(isnanormissing, stimec) && error("Some Start Time values is NaN or Missing!")
    any(isnanormissing, etimec) && error("Some End Time values is NaN or Missing!")

    sdata = Vector{UPKSubject}(undef, length(d))
    i = one(Int)
    @inbounds for (k, v) in d
        stimevals = stimec[v]
        etimevals = etimec[v]
        concvals  = concc[v]
        volvals   = volc[v]
        #=
        timeranges = collect(zip(stimevals, etimevals))
        sp = sortperm(stimevals)

        timevals_sp = timeranges[sp]
        concvals_sp = concvals[sp]
        volvals_sp  = volvals[sp]

        if length(timevals_sp) > 1
            for c = 2:length(timevals_sp)
                timevals_sp[c][1] == timevals_sp[c-1][2] || error("Start time ($(timevals_sp[c][1])) for observation $c not equal End time ($(timevals_sp[c-1][2])) for observation $(c-1)!")
            end
        end
        sdata[i] = UPKSubject(timevals_sp, concvals_sp, volvals_sp, kelauto, elimrange,  dosetime, Dict(sort .=> k))
        =#
        sdata[i] = upkimport(stimevals, etimevals, concvals, volvals; kelauto = kelauto,  elimrange = elimrange, dosetime = dosetime, id = Dict(sort .=> k))
        i += one(Int)
    end
    return DataSet(identity.(sdata))
end
"""
    upkimport(data, stime, etime, conc, vol; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime())

Import single urine PK data from table `data`.

* `stime` - start time column;
* `etime` - end time column;
* `conc` - concentration column;
* `vol` - volume column.
"""
function upkimport(data, stime, etime, conc, vol; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime())
    upkimport(Tables.getcolumn(data, stime), Tables.getcolumn(data, etime), Tables.getcolumn(data, conc), Tables.getcolumn(data, vol); kelauto = kelauto,  elimrange = elimrange, dosetime = dosetime)
end
"""
    upkimport(stime, etime, conc, vol; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime())

Import urine PK data from time vectors:
* `stime` - start times;
* `etime` - end times;
* `conc` - concentrations;
* `vol` - volumes.
"""
function upkimport(stime, etime, conc, vol; kelauto = true,  elimrange = ElimRange(), dosetime = DoseTime(), id = Dict{Symbol, Any}())
    any(isnanormissing, stime) && error("Some Start Time values is NaN or Missing!")
    any(isnanormissing, etime) && error("Some End Time values is NaN or Missing!")
    timeranges = collect(zip(stime, etime))
    sp = sortperm(stime)
    timevals_sp = timeranges[sp]
    concvals_sp = conc[sp]
    volvals_sp  = vol[sp]
    if length(timevals_sp) > 1
        for c = 2:length(timevals_sp)
            timevals_sp[c][1] == timevals_sp[c-1][2] || error("Start time ($(timevals_sp[c][1])) for observation $c not equal End time ($(timevals_sp[c-1][2])) for observation $(c-1)!")
        end
    end
    UPKSubject(timevals_sp, concvals_sp, volvals_sp, kelauto, elimrange,  dosetime, id)
end

function pdimport(data, time, obs, sort; bl = 0, th = NaN)
    if isa(sort, String) sort = [Symbol(sort)] end
    if isa(sort, Symbol) sort = [sort] end

    Tables.istable(data) || error("Data not a table!")

    cols   = Tables.columns(data)
    cdata  = Tuple(Tables.getcolumn(cols, y) for y in sort)
    d      = Dict{Tuple{eltype.(cdata)...}, Vector{Int}}()
    indsdict!(d, cdata)

    timec = Tables.getcolumn(data, time)
    obsc  = Tables.getcolumn(data, obs)

    any(isnanormissing, timec) && error("Some time values is NaN or Missing!")

    sdata = Vector{PDSubject}(undef, length(d))
    i = one(Int)
    @inbounds for (k, v) in d
        timevals = timec[v]
        obsvals  = obsc[v]

        if !allunique(timevals)
            @warn "Not all time values is unique, last observation used! ($k)"
            nuv = nonunique(timevals)
            nuvinds = findall(x -> x == first(nuv), timevals)[1:end-1]
            if length(nuv) > 1
                for cnt = 2:length(nuv)
                    append!(nuvinds, findall(x -> x == nuv[cnt], timevals)[1:end-1])
                end
            end
            sort!(nuvinds)
            deleteat!(timevals, nuvinds)
            deleteat!(obsvals, nuvinds)
        end
        sp = sortperm(timevals)
        timevals_sp = timevals[sp]
        obsvals_sp = obsvals[sp]
        timevals_sp, obsvals_sp = checkvalues(timevals_sp, obsvals_sp)
        sdata[i] = PDSubject(timevals_sp, obsvals_sp, bl, th, Dict(sort .=> k))
        i += one(Int)
    end
    return DataSet(identity.(sdata))
end


############################################################
## joAbstractDAparallelLinearOperator(s) - overloaded Base functions

# eltype(jo)
eltype(A::joAbstractDAparallelLinearOperator{DDT,RDT}) where {DDT,RDT} = promote_type(DDT,RDT)

# deltype(jo)
deltype(A::joAbstractDAparallelLinearOperator{DDT,RDT}) where {DDT,RDT} = DDT

# reltype(jo)
reltype(A::joAbstractDAparallelLinearOperator{DDT,RDT}) where {DDT,RDT} = RDT

# show(jo)
function show(A::joAbstractDAparallelLinearOperator)
    println("Type: $(typeof(A).name)")
    println("Name: $(A.name)")
    println("Size: $(size(A))")
    println(" NVC: $(A.nvc)")
    println(" DDT: $(deltype(A))")
    println(" RDT: $(reltype(A))")
    return nothing
end

# display(jo)
display(A::joAbstractDAparallelLinearOperator) = println((typeof(A),((A.m,A.n),A.nvc),A.name))

# size(jo)
size(A::joAbstractDAparallelLinearOperator) = A.m,A.n

# size(jo,1/2)
function size(A::joAbstractDAparallelLinearOperator,ind::Integer)
    if ind==1
		return A.m
	elseif ind==2
		return A.n
	elseif ind==3
		return A.nvc
	else
		throw(joAbstractDAparallelLinearOperatorException("invalid index"))
	end
end

# length(jo)
length(A::joAbstractDAparallelLinearOperator) = A.m*A.n

# jo_full(jo)

# norm(jo)

# real(jo)

# imag(jo)

# conj(jo)
conj(A::joDALinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDALinearOperator{DDT,RDT,N}("conj("*A.name*")",A.m,A.n,A.nvc,
        get(A.fop_C), A.fop_A, A.fop_T, A.fop,
        A.iop_C, A.iop_A, A.iop_T, A.iop,
        A.PAs_in, A.PAs_out, A.fclean, A.rclean
        )
conj(A::joDAdistributedLinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDAdistributedLinearOperator{DDT,RDT,N}("conj("*A.name*")",A.m,A.n,A.nvc,
        get(A.fop_C), A.fop_A, A.fop_T, A.fop,
        A.iop_C, A.iop_A, A.iop_T, A.iop,
        A.PAs_in, A.PAs_out, A.fclean, A.rclean
        )
conj(A::joDAdistributingLinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDAdistributingLinearOperator{DDT,RDT,N}("conj("*A.name*")",A.m,A.n,A.nvc,
        get(A.fop_C), A.fop_A, A.fop_T, A.fop,
        A.iop_C, A.iop_A, A.iop_T, A.iop,
        A.PAs_out, A.gclean
        )
conj(A::joDAgatheringLinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDAgatheringLinearOperator{DDT,RDT,N}("conj("*A.name*")",A.m,A.n,A.nvc,
        get(A.fop_C), A.fop_A, A.fop_T, A.fop,
        A.iop_C, A.iop_A, A.iop_T, A.iop,
        A.PAs_in, A.gclean
        )

# transpose(jo)
transpose(A::joDALinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDALinearOperator{RDT,DDT,N}("transpose("*A.name*")",A.n,A.m,A.nvc,
        get(A.fop_T), A.fop, A.fop_C, A.fop_A,
        A.iop_T, A.iop, A.iop_C, A.iop_A,
        A.PAs_out, A.PAs_in, A.rclean, A.fclean
        )
transpose(A::joDAdistributedLinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDAdistributedLinearOperator{RDT,DDT,N}("transpose("*A.name*")",A.n,A.m,A.nvc,
        get(A.fop_T), A.fop, A.fop_C, A.fop_A,
        A.iop_T, A.iop, A.iop_C, A.iop_A,
        A.PAs_out, A.PAs_in, A.rclean, A.fclean
        )
transpose(A::joDAdistributingLinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDAgatheringLinearOperator{RDT,DDT,N}("transpose("*A.name*")",A.n,A.m,A.nvc,
        get(A.fop_T), A.fop, A.fop_C, A.fop_A,
        A.iop_T, A.iop, A.iop_C, A.iop_A,
        A.PAs_out, A.gclean
        )
transpose(A::joDAgatheringLinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDAdistributingLinearOperator{RDT,DDT,N}("transpose("*A.name*")",A.n,A.m,A.nvc,
        get(A.fop_T), A.fop, A.fop_C, A.fop_A,
        A.iop_T, A.iop, A.iop_C, A.iop_A,
        A.PAs_in, A.gclean
        )

# adjoint(jo)
adjoint(A::joDALinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDALinearOperator{RDT,DDT,N}("adjoint("*A.name*")",A.n,A.m,A.nvc,
        get(A.fop_A), A.fop_C, A.fop, A.fop_T,
        A.iop_A, A.iop_C, A.iop, A.iop_T,
        A.PAs_out, A.PAs_in, A.rclean, A.fclean
        )
adjoint(A::joDAdistributedLinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDAdistributedLinearOperator{RDT,DDT,N}("adjoint("*A.name*")",A.n,A.m,A.nvc,
        get(A.fop_A), A.fop_C, A.fop, A.fop_T,
        A.iop_A, A.iop_C, A.iop, A.iop_T,
        A.PAs_out, A.PAs_in, A.rclean, A.fclean
        )
adjoint(A::joDAdistributingLinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDAgatheringLinearOperator{RDT,DDT,N}("adjoint("*A.name*")",A.n,A.m,A.nvc,
        get(A.fop_A), A.fop_C, A.fop, A.fop_T,
        A.iop_A, A.iop_C, A.iop, A.iop_T,
        A.PAs_out, A.gclean
        )
adjoint(A::joDAgatheringLinearOperator{DDT,RDT,N}) where {DDT,RDT,N} =
    joDAdistributingLinearOperator{RDT,DDT,N}("adjoint("*A.name*")",A.n,A.m,A.nvc,
        get(A.fop_A), A.fop_C, A.fop, A.fop_T,
        A.iop_A, A.iop_C, A.iop, A.iop_T,
        A.PAs_in, A.gclean
        )

# isreal(jo)
isreal(A :: joAbstractDAparallelLinearOperator{DDT,RDT}) where {DDT,RDT} = (DDT<:Real && RDT<:Real)

# issymmetric(jo)

# ishermitian(jo)

# getindex(jo,...)

############################################################
## overloaded Base *(...jo...)

# *(jo,jo)
function *(A::joDALinOpsUnion{CDT,ARDT,2},B::joDALinOpsUnion{BDDT,CDT,2}) where {ARDT,BDDT,CDT}
    size(A,2) == size(B,1) || throw(joDALinearOperatorException("*($(A.name),$(B.name)): shape mismatch"))
    A.nvc==B.nvc || throw(joDALinearOperatorException("*($(A.name),$(B.name)): nvc mismatch"))
    isapprox(A.PAs_in,B.PAs_out) || throw(joDALinearOperatorException("*($(A.name),$(B.name)): distributor mismatch"))
    return joDALinearOperator{BDDT,ARDT,2}("($(A.name)*$(B.name))",size(A,1),size(B,2),A.nvc,
               v1->A*(B*v1),
               v2->transpose(B)*(transpose(A)*v2),
               v3->adjoint(B)*(adjoint(A)*v3),
               v4->conj(A)*(conj(B)*v4),
               @joNF, @joNF, @joNF, @joNF,
               B.PAs_in,A.PAs_out,B.fclean,A.rclean)
end
function *(A::joDALinOpsUnion{CDT,ARDT,2},B::joDAdistributeLinOpsUnion{BDDT,CDT,2}) where {ARDT,BDDT,CDT}
    size(A,2) == size(B,1) || throw(joDALinearOperatorException("*($(A.name),$(B.name)): shape mismatch"))
    A.nvc==B.nvc || throw(joDALinearOperatorException("*($(A.name),$(B.name)): nvc mismatch"))
    isapprox(A.PAs_in,B.PAs_out) || throw(joDALinearOperatorException("*($(A.name),$(B.name)): distributor mismatch"))
    return joDAdistributingLinearOperator{BDDT,ARDT,2}("($(A.name)*$(B.name))",size(A,1),size(B,2),A.nvc,
               v1->A*(B*v1),
               v2->transpose(B)*(transpose(A)*v2),
               v3->adjoint(B)*(adjoint(A)*v3),
               v4->conj(A)*(conj(B)*v4),
               @joNF, @joNF, @joNF, @joNF,
               A.PAs_out,B.gclean)
end
function *(A::joDAgatherLinOpsUnion{CDT,ARDT,2},B::joDALinOpsUnion{BDDT,CDT,2}) where {ARDT,BDDT,CDT}
    size(A,2) == size(B,1) || throw(joDAgatheringLinearOperatorException("*($(A.name),$(B.name)): shape mismatch"))
    A.nvc==B.nvc || throw(joDAgatheringLinearOperatorException("*($(A.name),$(B.name)): nvc mismatch"))
    isapprox(A.PAs_in,B.PAs_out) || throw(joDAgatheringLinearOperatorException("*($(A.name),$(B.name)): distributor mismatch"))
    return joDAgatheringLinearOperator{BDDT,ARDT,2}("($(A.name)*$(B.name))",size(A,1),size(B,2),A.nvc,
               v1->A*(B*v1),
               v2->transpose(B)*(transpose(A)*v2),
               v3->adjoint(B)*(adjoint(A)*v3),
               v4->conj(A)*(conj(B)*v4),
               @joNF, @joNF, @joNF, @joNF,
               B.PAs_in,B.fclean)
end
function *(A::joDAdistributeLinOpsUnion{CDT,ARDT,2},B::joDAgatherLinOpsUnion{BDDT,CDT,2}) where {ARDT,BDDT,CDT}
    size(A,2) == size(B,1) || throw(joDALinearOperatorException("*($(A.name),$(B.name)): shape mismatch"))
    A.nvc==B.nvc || throw(joDALinearOperatorException("*($(A.name),$(B.name)): nvc mismatch"))
    return joDALinearOperator{BDDT,ARDT,2}("($(A.name)*$(B.name))",size(A,1),size(B,2),A.nvc,
               v1->A*(B*v1),
               v2->transpose(B)*(transpose(A)*v2),
               v3->adjoint(B)*(adjoint(A)*v3),
               v4->conj(A)*(conj(B)*v4),
               @joNF, @joNF, @joNF, @joNF,
               B.PAs_in,A.PAs_out,B.gclean,A.gclean)
end
function *(A::joDAgatherLinOpsUnion{CDT,ARDT,2},B::joDAdistributeLinOpsUnion{BDDT,CDT,2}) where {ARDT,BDDT,CDT}
    size(A,2) == size(B,1) || throw(joLinearOperatorException("*($(A.name),$(B.name)): shape mismatch"))
    A.nvc==B.nvc || throw(joLinearOperatorException("*($(A.name),$(B.name)): nvc mismatch"))
    isapprox(A.PAs_in,B.PAs_out) || throw(joLinearOperatorException("*($(A.name),$(B.name)): distributor mismatch"))
    return joLinearOperator{BDDT,ARDT}("($(A.name)*$(B.name))",size(A,1),size(B,2),
               v1->A*(B*v1),
               v2->transpose(B)*(transpose(A)*v2),
               v3->adjoint(B)*(adjoint(A)*v3),
               v4->conj(A)*(conj(B)*v4),
               @joNF, @joNF, @joNF, @joNF)
end
# extras with warnings
function *(A::joDAdistribute{CDT,ARDT,2},B::joDAgather{BDDT,CDT,2}) where {ARDT,BDDT,CDT}
    size(A,2) == size(B,1) || throw(joDALinearOperatorException("*($(A.name),$(B.name)): shape mismatch"))
    A.PAs_out.dims[2]==B.PAs_in.dims[2] || throw(joDALinearOperatorException("*($(A.name),$(B.name)): nvc mismatch"))
    @warn "*($(typeof(A)),$(typeof(B))) is a senseless operation. Get rid of it!"
    return joDALinearOperator{BDDT,ARDT,2}("($(A.name)*$(B.name))",size(A,1),size(B,2),A.PAs_out.dims[2],
               v1->v1, v2->v2, v3->v3, v4->v4,
               @joNF, @joNF, @joNF, @joNF,
               B.PAs_in,A.PAs_out,B.gclean,A.gclean)
end
function *(A::joDAgather{CDT,ARDT,2},B::joDAdistribute{BDDT,CDT,2}) where {ARDT,BDDT,CDT}
    size(A,2) == size(B,1) || throw(joLinearOperatorException("*($(A.name),$(B.name)): shape mismatch"))
    A.PAs_in.dims[2]==B.PAs_out.dims[2] || throw(joLinearOperatorException("*($(A.name),$(B.name)): nvc mismatch"))
    isapprox(A.PAs_in,B.PAs_out) || throw(joLinearOperatorException("*($(A.name),$(B.name)): distributor mismatch"))
    @warn "*($(typeof(A)),$(typeof(B))) is a senseless operation. Get rid of it!"
    return joLinearOperator{BDDT,ARDT}("($(A.name)*$(B.name))",size(A,1),size(B,2),
               v1->v1, v2->v2, v3->v3, v4->v4,
               @joNF, @joNF, @joNF, @joNF)
end
# joDA with joAbstractLinearOperator
function *(A::joDAdistributeLinOpsUnion{CDT,ARDT,2},B::joAbstractLinearOperator{BDDT,CDT}) where {ARDT,BDDT,CDT}
    size(A,2) == size(B,1) || throw(joAbstractLinearOperatorException("*($(A.name),$(B.name)): shape mismatch"))
    return joDAdistributingLinearOperator{BDDT,ARDT,2}("($(A.name)*$(B.name))",size(A,1),size(B,2),A.nvc,
               v1->A*(B*v1),
               v2->transpose(B)*(transpose(A)*v2),
               v3->adjoint(B)*(adjoint(A)*v3),
               v4->conj(A)*(conj(B)*v4),
               @joNF, @joNF, @joNF, @joNF,
               A.PAs_out,A.gclean)
end
function *(A::joAbstractLinearOperator{CDT,ARDT},B::joDAgatherLinOpsUnion{BDDT,CDT,2}) where {ARDT,BDDT,CDT}
    size(A,2) == size(B,1) || throw(joAbstractLinearOperatorException("*($(A.name),$(B.name)): shape mismatch"))
    return joDAgatheringLinearOperator{BDDT,ARDT,2}("($(A.name)*$(B.name))",size(A,1),size(B,2),B.nvc,
               v1->A*(B*v1),
               v2->transpose(B)*(transpose(A)*v2),
               v3->adjoint(B)*(adjoint(A)*v3),
               v4->conj(A)*(conj(B)*v4),
               @joNF, @joNF, @joNF, @joNF,
               B.PAs_in,B.gclean)
end

# *(jo,mvec)
function *(A::joDALinearOperator{ADDT,ARDT,2},mv::DArray{mvDT,2}) where {ADDT,ARDT,mvDT<:Number}
    A.n == size(mv,1) || throw(joDALinearOperatorException("shape mismatch"))
    A.nvc == size(mv,2) || throw(joDALinearOperatorException("shape mismatch"))
    jo_check_type_match(ADDT,mvDT,join(["DDT for *(jo,mvec):",A.name,typeof(A),mvDT]," / "))
    isequiv(A.PAs_in,mv) || throw(joDALinearOperatorException("*($(A.name),mv::DArray): input distributor mismatch"))
    MV=A.fop(mv)
    jo_check_type_match(ARDT,eltype(MV),join(["RDT from *(jo,mvec):",A.name,typeof(A),eltype(MV)]," / "))
    return MV
end
function *(A::joDAdistributedLinearOperator{ADDT,ARDT,2},mv::DArray{mvDT,2}) where {ADDT,ARDT,mvDT<:Number}
    A.n == size(mv,1) || throw(joDAdistributedLinearOperatorException("shape mismatch"))
    A.nvc == size(mv,2) || throw(joDAdistributedLinearOperatorException("shape mismatch"))
    jo_check_type_match(ADDT,mvDT,join(["DDT for *(jo,mvec):",A.name,typeof(A),mvDT]," / "))
    isequiv(A.PAs_in,mv) || throw(joDAdistributedLinearOperatorException("*($(A.name),mv::DArray): input distributor mismatch"))
    MV=dalloc(A.PAs_out)
    spmd(joDAutils.jo_x_mv!,A.fop,mv,MV,pids=A.PAs_out.procs)
    jo_check_type_match(ARDT,eltype(MV),join(["RDT from *(jo,mvec):",A.name,typeof(A),eltype(MV)]," / "))
    return MV
end
function *(A::joDAdistributingLinearOperator{ADDT,ARDT,2},mv::LocalMatrix{mvDT}) where {ADDT,ARDT,mvDT<:Number}
    A.n == size(mv,1) || throw(joDAdistributingLinearOperatorException("shape mismatch in A$(size(A))*v$(size(mv))"))
    A.nvc == size(mv,2) || throw(joDAdistributingLinearOperatorException("nvc size mismatch"))
    jo_check_type_match(ADDT,mvDT,join(["DDT for *(jo,mvec):",A.name,typeof(A),mvDT]," / "))
    MV = A.fop(mv)
    jo_check_type_match(ARDT,eltype(MV),join(["RDT from *(jo,mvec):",A.name,typeof(A),eltype(MV)]," / "))
    return MV
end
function *(A::joDAgatheringLinearOperator{ADDT,ARDT,2},mv::DArray{mvDT,2}) where {ADDT,ARDT,mvDT<:Number}
    A.n == size(mv,1) || throw(joDAgatheringLinearOperatorException("shape mismatch in A$(size(A))*v$(size(mv))"))
    A.nvc == size(mv,2) || throw(joDAgatheringLinearOperatorException("nvc size mismatch"))
    jo_check_type_match(ADDT,mvDT,join(["DDT for *(jo,mvec):",A.name,typeof(A),mvDT]," / "))
    isequiv(A.PAs_in,mv) || throw(joDAgatheringLinearOperatorException("*($(A.name),mv::DArray): input distributor mismatch"))
    MV = A.fop(mv)
    jo_check_type_match(ARDT,eltype(MV),join(["RDT from *(jo,mvec):",A.name,typeof(A),eltype(MV)]," / "))
    return MV
end

# *(mvec,jo)

# *(jo,vec)

# *(vec,jo)

# *(num,jo)

# *(jo,num)

############################################################
## overloaded Base \(...jo...)

# \(jo,jo)

# \(jo,mvec)

# \(mvec,jo)

# \(jo,vec)

# \(vec,jo)

# \(num,jo)

# \(jo,num)
#\{ADDT,ARDT}(A::joAbstractDAparallelLinearOperator{ADDT,ARDT},a::Number) = inv(a)*A
#\{ADDT,ARDT}(A::joAbstractDAparallelLinearOperator{ADDT,ARDT},a::joNumber{ADDT,ARDT}) = inv(a)*A

############################################################
## overloaded Base +(...jo...)

# +(jo)
+(A::joAbstractDAparallelLinearOperator) = A

# +(jo,jo)

# +(jo,mvec)

# +(mvec,jo)

# +(jo,vec)

# +(vec,jo)

# +(jo,num)

# +(num,jo)
+(b::Number,A::joAbstractDAparallelLinearOperator{ADDT,ARDT}) where {ADDT,ARDT} = A+b
+(b::joNumber{ADDT,ARDT},A::joAbstractDAparallelLinearOperator{ADDT,ARDT}) where {ADDT,ARDT} = A+b

############################################################
## overloaded Base -(...jo...)

# -(jo)

# -(jo,jo)
-(A::joAbstractDAparallelLinearOperator,B::joAbstractDAparallelLinearOperator) = A+(-B)

# -(jo,mvec)

# -(mvec,jo)

# -(jo,vec)

# -(vec,jo)

# -(jo,num)
-(A::joAbstractDAparallelLinearOperator,b::Number) = A+(-b)
-(A::joAbstractDAparallelLinearOperator,b::joNumber) = A+(-b)

# -(num,jo)
-(b::Number,A::joAbstractDAparallelLinearOperator) = -A+b
-(b::joNumber,A::joAbstractDAparallelLinearOperator) = -A+b

############################################################
## overloaded Base .*(...jo...)

# .*(num,jo)
Base.Broadcast.broadcasted(::typeof(*),a::Number,A::joAbstractDAparallelLinearOperator) = a*A
Base.Broadcast.broadcasted(::typeof(*),a::joNumber,A::joAbstractDAparallelLinearOperator) = a*A

# .*(jo,num)
Base.Broadcast.broadcasted(::typeof(*),A::joAbstractDAparallelLinearOperator,a::Number) = a*A
Base.Broadcast.broadcasted(::typeof(*),A::joAbstractDAparallelLinearOperator,a::joNumber) = a*A

############################################################
## overloaded Base .\(...jo...)

############################################################
## overloaded Base .+(...jo...)

# .+(jo,num)
Base.Broadcast.broadcasted(::typeof(+),A::joAbstractDAparallelLinearOperator,b::Number) = A+b
Base.Broadcast.broadcasted(::typeof(+),A::joAbstractDAparallelLinearOperator,b::joNumber) = A+b

# .+(num,jo)
Base.Broadcast.broadcasted(::typeof(+),b::Number,A::joAbstractDAparallelLinearOperator) = A+b
Base.Broadcast.broadcasted(::typeof(+),b::joNumber,A::joAbstractDAparallelLinearOperator) = A+b

############################################################
## overloaded Base .-(...jo...)

# .-(jo,num)
Base.Broadcast.broadcasted(::typeof(-),A::joAbstractDAparallelLinearOperator,b::Number) = A+(-b)
Base.Broadcast.broadcasted(::typeof(-),A::joAbstractDAparallelLinearOperator,b::joNumber) = A+(-b)

# .-(num,jo)
Base.Broadcast.broadcasted(::typeof(-),b::Number,A::joAbstractDAparallelLinearOperator) = -A+b
Base.Broadcast.broadcasted(::typeof(-),b::joNumber,A::joAbstractDAparallelLinearOperator) = -A+b

############################################################
## overloaded Base block methods

# hcat(...jo...)

# vcat(...jo...)

# hvcat(...jo...)

############################################################
## overloaded LinearAlgebra functions

# mul!(...,jo,...)

# ldiv!(...,jo,...)


export flow, flowdθ, flowdσ, flowdψ, STARTING_σ_ψ, STARTING_log_ogip, STARTING_t

const STARTING_σ_ψ      = 0x1.baddbb87af68ap-2 # = 0.432
const STARTING_log_ogip = 0x1.670bf3d5b282dp-1 # = 0.701
const STARTING_t = 2*0.042/(2016-2003)

# functions in case we have months to expiration
@inline flow(  FF::Type, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer, d1::Integer, Dgt0::Bool, sgn_ext::Bool, τ::Real, geoid::Real, roy::T) where {N,T} = flow(  FF, θ, σ, z, ψ,    d, d1, Dgt0, sgn_ext, geoid, roy)
@inline flowdθ(FF::Type, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, k::Integer, d::Integer, d1::Integer, Dgt0::Bool, sgn_ext::Bool, τ::Real, geoid::Real, roy::T) where {N,T} = flowdθ(FF, θ, σ, z, ψ, k, d, d1, Dgt0, sgn_ext, geoid, roy)
@inline flowdσ(FF::Type, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer, d1::Integer, Dgt0::Bool, sgn_ext::Bool, τ::Real, geoid::Real, roy::T) where {N,T} = flowdσ(FF, θ, σ, z, ψ,    d,                    geoid, roy)
@inline flowdψ(FF::Type, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer,                          sgn_ext::Bool, τ::Real, geoid::Real, roy::T) where {N,T} = flowdψ(FF, θ, σ, z, ψ,    d,           sgn_ext, geoid, roy)
@inline flowdψ(FF::Type, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer, d1::Integer, Dgt0::Bool, sgn_ext::Bool, τ::Real, geoid::Real, roy::T) where {N,T} = flowdψ(FF, θ, σ, z, ψ,    d,           sgn_ext, geoid, roy)

@inline flowdσ(FF::Type, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer,                                                  geoid::Real, roy::T) where {N,T} = flowdσ(FF, θ, σ, z, ψ,    d,                    geoid, roy)

# functions in case we have months to expiration
@inline flow(  FF::Type, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer, geoid::Real, roy::T) where {N,T} = flow(  FF, θ, σ, z, ψ,    d, _d1(wp,i), _Dgt0(wp,i), _sgnext(wp,i), geoid, roy)
@inline flowdθ(FF::Type, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, k::Integer, d::Integer, geoid::Real, roy::T) where {N,T} = flowdθ(FF, θ, σ, z, ψ, k, d, _d1(wp,i), _Dgt0(wp,i), _sgnext(wp,i), geoid, roy)
@inline flowdσ(FF::Type, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer, geoid::Real, roy::T) where {N,T} = flowdσ(FF, θ, σ, z, ψ,    d,                                        geoid, roy)
@inline flowdψ(FF::Type, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer, geoid::Real, roy::T) where {N,T} = flowdψ(FF, θ, σ, z, ψ,    d,                         _sgnext(wp,i), geoid, roy)

# @inline function flowdθ(::Type, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, k::Integer, d::Integer, d1::Integer, Dgt0::Bool, sgn_ext::Bool, geoid::Real, roy::T)::T where {N,T}

# --------------------------- common revenue functions & derivatives  --------------------------------------

@inline    rev_exp_restricted(θ1::T, σ::T, logp::Real,          ψ::Real, Dgt0::Bool, geoid::Real, roy::Real) where {T} = rev_exp(   1, θ1, 1, STARTING_log_ogip, STARTING_σ_ψ,             σ, logp,    ψ, Dgt0, geoid, roy)
@inline drevdσ_exp_restricted(θ1::T, σ::T, logp::Real,          ψ::Real,             geoid::Real, roy::Real) where {T} = drevdσ_exp(1, θ1, 1, STARTING_log_ogip, STARTING_σ_ψ,             σ, logp,    ψ,       geoid, roy)
@inline drevdψ_exp_restricted(θ1::T, σ::T, logp::Real,          ψ::Real,             geoid::Real, roy::Real) where {T} = drevdψ_exp(1, θ1, 1, STARTING_log_ogip, STARTING_σ_ψ,             σ, logp,    ψ,       geoid, roy)

@inline    rev_exp_restricted(θ1::T, σ::T, logp::Real, t::Real, ψ::Real, Dgt0::Bool, geoid::Real, roy::Real) where {T} = rev_exp(   1, θ1, 1, STARTING_log_ogip, STARTING_σ_ψ, STARTING_t, σ, logp, t, ψ, Dgt0, geoid, roy)
@inline drevdσ_exp_restricted(θ1::T, σ::T, logp::Real, t::Real, ψ::Real,             geoid::Real, roy::Real) where {T} = drevdσ_exp(1, θ1, 1, STARTING_log_ogip, STARTING_σ_ψ, STARTING_t, σ, logp, t, ψ,       geoid, roy)
@inline drevdψ_exp_restricted(θ1::T, σ::T, logp::Real, t::Real, ψ::Real,             geoid::Real, roy::Real) where {T} = drevdψ_exp(1, θ1, 1, STARTING_log_ogip, STARTING_σ_ψ, STARTING_t, σ, logp, t, ψ,       geoid, roy)

# chebshev polynomials
# See http://www.aip.de/groups/soe/local/numres/bookcpdf/c5-8.pdf
@inline checkinterval(x::Real,min::Real,max::Real) =  min <= x <= max || throw(DomainError("x = $x must be in [$min,$max]"))
@inline checkinterval(x::Real) = checkinterval(x,-1,1)
@inline cheb0(x::Real) = (checkinterval(x); return one(Float64))
@inline cheb1(x::Real) = (checkinterval(x); return x)
@inline cheb2(x::Real) = (checkinterval(x); return 2*x^2 - 1)
@inline cheb3(x::Real) = (checkinterval(x); return 4*x^3 - 3*x)
@inline cheb4(x::Real) = (checkinterval(x); return 8*(x^4 - x^2) + 1)

# -----------------------------------------
# Flows
# -----------------------------------------

include("flow-payoffs-more.jl")

@inline function flow(::Type{Val{:cheb3_cost_tech}}, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, d::Integer, geoid::Real, roy::T) where {N,T}
# @inline function flow(::Type{Val{:cheb3_cost_tech}}, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, d::Integer, d1::Integer, Dgt0::Bool, sgn_ext::Bool, geoid::Real, roy::T) where {N,T}
    if d == 0
        _sgnext(wp,i) && return θ[11]
        return zero(T)
    end
    logp, logc, t = z
    u = rev_exp(1,θ[1],1,θ[2],θ[3],θ[4],σ, logp, t, ψ, _Dgt0(wp,i), geoid,roy) + (d==1 ? θ[5] : θ[6] )*cheb0(t) + θ[7]*cheb1(t) + θ[8]*cheb2(t) + θ[9]*cheb3(t) + θ[10]*exp(logc)
    d>1 && (u *= d)
    return u::T
end

@inline function flow(::Type{Val{:cheb3_cost_tech_restr}}, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, d::Integer, d1::Integer, Dgt0::Bool, sgn_ext::Bool, geoid::Real, roy::T) where {N,T}
    if d == 0
        sgn_ext && return θ[8]
        return zero(T)
    end
    logp, logc, t = z
    u = rev_exp_restricted(θ[1], σ, logp, t, ψ, Dgt0, geoid, roy) + (d==1 ? θ[2] : θ[3] )*cheb0(t) + θ[4]*cheb1(t) + θ[5]*cheb2(t) + θ[6]*cheb3(t) + θ[7]*exp(logc)
    d>1 && (u *= d)
    return u::T
end

# -----------------------------------------
# number of parms
# -----------------------------------------

function number_of_model_parms(FF::Symbol)::Int
    FF ∈ (:one_restr,)                                                           && return  3
    FF ∈ (:dgt1_restr,:Dgt0_restr,)                                              && return  4
    FF ∈ (:one,:dgt1_ext_restr, :dgt1_d1_restr,:dgt1_cost_restr,:cheb2_restr)   && return  5
    FF ∈ (:dgt1,:Dgt0,:dgt1_cost_Dgt0_restr,:dgt1_pricecost_restr,:cheb3_restr) && return  6
    FF ∈ (:dgt1_ext,:dgt1_d1,:dgt1_cost,:dgt1_pricebreak_restr,:cheb2, :cheb3_dgt1_restr,)  && return  7
    FF ∈ (:dgt1_cost_Dgt0,:dgt1_pricecost,:cheb3,:cheb3_cost_restr,:ttbuild_cost_restr,:cheb3_cost_tech_restr,)  && return  8
    FF ∈ (:dgt1_pricebreak,:cheb3_dgt1,)                                         && return  9
    FF ∈ (:cheb3_cost,:ttbuild_cost,)                                            && return 10
    FF ∈ (:cheb3_cost_tech,)                                                     && return 11
    throw(error("FF = $(FF) not recognized"))
end

# -----------------------------------------
# dθ
# -----------------------------------------

@inline function flowdθ(::Type{Val{:cheb3_cost_tech}}, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, k::Integer, d::Integer, geoid::Real, roy::T) where {N,T}
# @inline function flowdθ(::Type{Val{:cheb3_cost_tech}}, θ::AbstractVector{T}, σ::T,     z::NTuple{N,T}, ψ::T, k::Integer,d::Integer,           d1::Integer, Dgt0::Bool, sgn_ext::Bool, geoid::Real, roy::T)::T where {N,T}
    d == 0 && !_sgnext(wp,i) && return zero(T)
    Dgt0 = _Dgt0(wp,i)
    logp, logc, t = z

    # revenue
    k == 1  && return   d * rev_exp(1,θ[1],1,θ[2],θ[3],θ[4],σ,logp,t,ψ,Dgt0,geoid,roy)
    k == 2  && return   d * rev_exp(1,θ[1],1,θ[2],θ[3],θ[4],σ,logp,t,ψ,Dgt0,geoid,roy) * geoid
    k == 3  && return   d * rev_exp(1,θ[1],1,θ[2],θ[3],θ[4],σ,logp,t,ψ,Dgt0,geoid,roy) * ( Dgt0 ? ψ : ψ*_ρ(σ) + θ[k]*(1-_ρ2(σ)))
    k == 4  && return   d * rev_exp(1,θ[1],1,θ[2],θ[3],θ[4],σ,logp,t,ψ,Dgt0,geoid,roy) * t

    # drilling cost
    k == 5   && return d != 1 ? zero(T) : T(   cheb0(t) )
    k == 6   && return d <= 1 ? zero(T) : T( d*cheb0(t) )
    k == 7   && return d == 0 ? zero(T) : T( d*cheb1(t) )
    k == 8   && return d == 0 ? zero(T) : T( d*cheb2(t) )
    k == 9   && return d == 0 ? zero(T) : T( d*cheb3(t) )
    k == 10  && return d == 0 ? zero(T) : T( d*exp(logc) )

    # extension cost
    k == 11  && return d == 0 && _sgnext(wp,i) ? one(T) : zero(T)

    throw(error("$k out of bounds"))
end

@inline function flowdθ(::Type{Val{:cheb3_cost_tech_restr}}, θ::AbstractVector{T}, σ::T,     z::NTuple{N,T}, ψ::T, k::Integer,d::Integer,           d1::Integer, Dgt0::Bool, sgn_ext::Bool, geoid::Real, roy::T)::T where {N,T}
    d == 0 && !sgn_ext && return zero(T)

    logp, logc, t = z

    # revenue
    k == 1  && return   d * rev_exp_restricted(θ[1], σ, logp, t, ψ, Dgt0, geoid, roy)

    # drilling cost
    k == 2  && return d != 1 ? zero(T) : T(   cheb0(t) )
    k == 3  && return d <= 1 ? zero(T) : T( d*cheb0(t) )
    k == 4  && return d == 0 ? zero(T) : T( d*cheb1(t) )
    k == 5  && return d == 0 ? zero(T) : T( d*cheb2(t) )
    k == 6  && return d == 0 ? zero(T) : T( d*cheb3(t) )
    k == 7  && return d == 0 ? zero(T) : T( d*exp(logc) )
    # extension cost
    k == 8  && return d == 0 && sgn_ext ? one(T) : zero(T)

    throw(error("$k out of bounds"))
end

# -----------------------------------------
# dσ
# -----------------------------------------

@inline function flowdσ(::FF, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, d::Integer, geoid::Real, roy::T)::T where {FF <: Union{ Type{Val{:one}}, Type{Val{:dgt1}}, Type{Val{:Dgt0}}, Type{Val{:dgt1_ext}}, Type{Val{:dgt1_d1}}, Type{Val{:dgt1_cost}}, Type{Val{:dgt1_cost_Dgt0}}, Type{Val{:cheb2}}, Type{Val{:cheb3}}, Type{Val{:cheb3_dgt1}}, Type{Val{:cheb3_cost}}, Type{Val{:ttbuild_cost}} }, N, T}
    d == 0 && return zero(T)
    return d * drevdσ_exp(1,θ[1],1,θ[2],θ[3],σ,z[1],ψ,geoid,roy)
end

@inline function flowdσ(::FF, θ::AbstractVector{T}, σ::T, z::NTuple{N,T},  ψ::T, d::Integer, geoid::Real, roy::T)::T where {FF <: Union{ Type{Val{:one_restr}}, Type{Val{:dgt1_restr}}, Type{Val{:Dgt0_restr}}, Type{Val{:dgt1_ext_restr}}, Type{Val{:dgt1_d1_restr}}, Type{Val{:dgt1_cost_restr}}, Type{Val{:dgt1_cost_Dgt0_restr}}, Type{Val{:cheb2_restr}}, Type{Val{:cheb3_restr}}, Type{Val{:cheb3_dgt1_restr}}, Type{Val{:cheb3_cost_restr}}, Type{Val{:ttbuild_cost_restr}} },N, T}
    d == 0 && return zero(T)
    return d * drevdσ_exp_restricted(θ[1],σ,z[1],ψ,geoid,roy)
end

@inline function flowdσ(::FF, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, d::Integer, geoid::Real, roy::T)::T where {FF <: Union{ Type{Val{:cheb3_cost_tech}} }, N, T}
    d == 0 && return zero(T)
    return d * drevdσ_exp(1,θ[1],1,θ[2],θ[3],θ[4],σ,first(z),last(z),ψ,geoid,roy)
end

@inline function flowdσ(::FF, θ::AbstractVector{T}, σ::T, z::NTuple{N,T},  ψ::T, d::Integer, geoid::Real, roy::T)::T where {FF <: Union{ Type{Val{:cheb3_cost_tech_restr}}  },N, T}
    d == 0 && return zero(T)
    return d * drevdσ_exp_restricted(θ[1],σ,first(z),last(z),ψ,geoid,roy)
end

# -----------------------------------------
# dψ
# -----------------------------------------

@inline function flowdψ(::FF, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, d::Integer, sgn_ext::Bool, geoid::Real, roy::T) where {FF <: Union{ Type{Val{:one}}, Type{Val{:dgt1}}, Type{Val{:Dgt0}}, Type{Val{:dgt1_d1}}, Type{Val{:dgt1_cost}}, Type{Val{:dgt1_cost_Dgt0}}, Type{Val{:cheb2}}, Type{Val{:cheb3}}, Type{Val{:cheb3_dgt1}}, Type{Val{:cheb3_cost}}, Type{Val{:ttbuild_cost}} },N, T}
    d == 0  && return zero(T) # sgn_ext ? θ[10] : zero(T)
    return (d * drevdψ_exp(1,θ[1],1,θ[2],θ[3],σ,z[1],ψ,geoid,roy))::T
end

@inline function flowdψ(::FF, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, d::Integer, sgn_ext::Bool, geoid::Real, roy::T) where {FF <: Union{ Type{Val{:one_restr}}, Type{Val{:dgt1_restr}}, Type{Val{:Dgt0_restr}}, Type{Val{:dgt1_d1_restr}}, Type{Val{:dgt1_cost_restr}}, Type{Val{:dgt1_cost_Dgt0_restr}}, Type{Val{:cheb2_restr}}, Type{Val{:cheb3_restr}}, Type{Val{:cheb3_dgt1_restr}}, Type{Val{:cheb3_cost_restr}}, Type{Val{:ttbuild_cost_restr}} },N, T}
    d == 0  && return zero(T) # sgn_ext ? θ[10] : zero(T)
    return (d * drevdψ_exp_restricted(θ[1],σ,z[1],ψ,geoid,roy))::T
end

# @inline function flow(  FF::Type, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer, geoid::Real, roy::T)
# @inline function flowdθ(FF::Type, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, k::Integer, d::Integer, geoid::Real, roy::T)
# @inline function flowdσ(FF::Type, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer, geoid::Real, roy::T)
# @inline function flowdψ(FF::Type, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T,             d::Integer, geoid::Real, roy::T)

@inline function flowdψ(::FF, wp::AbstractUnitProblem, i::Integer, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, d::Integer, geoid::Real, roy::T) where {FF <: Union{ Type{Val{:cheb3_cost_tech}} }, N, T}
    d == 0  && return zero(T) # _sgnext(wp,i) ? θ[10] : zero(T)
    return (d * drevdψ_exp(1,θ[1],1,θ[2],θ[3],θ[4],σ,first(z),last(z),ψ,geoid,roy))::T
end

@inline function flowdψ(::FF, θ::AbstractVector{T}, σ::T, z::NTuple{N,T}, ψ::T, d::Integer, sgn_ext::Bool, geoid::Real, roy::T) where {FF <: Union{ Type{Val{:cheb3_cost_tech_restr}} }, N, T}
    d == 0  && return zero(T) # sgn_ext ? θ[10] : zero(T)
    return (d * drevdψ_exp_restricted(θ[1],σ,first(z),last(z),ψ,geoid,roy))::T
end


#=

ESSE wrapper

Ignacio Quintero Mächler

t(-_-t)

September 26 2017

=#

"""
    esse(states_file ::String,
         tree_file   ::String,
         envdata_file::String,
         cov_mod     ::NTuple{M,String},
         out_file    ::String,
         h           ::Int64;
         constraints ::NTuple{N,String}  = (" ",),
         mvpars      ::NTuple{O,String}  = ("lambda = beta",),
         niter       ::Int64             = 10_000,
         nthin       ::Int64             = 10,
         nburn       ::Int64             = 200,
         nchains     ::Int64             = 1,
         ntakew      ::Int64             = 100,
         winit       ::Float64             = 2.0,
         scale_y     ::NTuple{2,Bool}    = (true, false),
         algorithm   ::String            = "pruning",
         λpriors     ::Float64           = .1,
         μpriors     ::Float64           = .1,
         gpriors     ::Float64           = .1,
         lpriors     ::Float64           = .1,
         qpriors     ::Float64           = .1,
         βpriors     ::NTuple{2,Float64} = (0.0, 10.0),
         hpriors     ::Float64           = .1,
         optimal_w   ::Float64           = 0.8,
         screen_print::Int64             = 5,
         Eδt         ::Float64           = 1e-3,
         ti          ::Float64           = 0.0,
         ρ           ::Array{Float64,1}  = [1.0]) where {M,N,O}

Wrapper for running a SSE model from file.
"""
function esse(states_file ::String,
              tree_file   ::String,
              envdata_file::String,
              cov_mod     ::NTuple{M,String},
              out_file    ::String,
              h           ::Int64;
              constraints ::NTuple{N,String}  = (" ",),
              mvpars      ::NTuple{O,String}  = ("lambda = beta",),
              niter       ::Int64             = 10_000,
              nthin       ::Int64             = 10,
              nburn       ::Int64             = 200,
              nchains     ::Int64             = 1,
              ntakew      ::Int64             = 100,
              winit       ::Float64             = 2.0,
              scale_y     ::NTuple{2,Bool}    = (true, false),
              algorithm   ::String            = "pruning",
              λpriors     ::Float64           = .1,
              μpriors     ::Float64           = .1,
              gpriors     ::Float64           = .1,
              lpriors     ::Float64           = .1,
              qpriors     ::Float64           = .1,
              βpriors     ::NTuple{2,Float64} = (0.0, 10.0),
              hpriors     ::Float64           = .1,
              optimal_w   ::Float64           = 0.8,
              screen_print::Int64             = 5,
              Eδt         ::Float64           = 1e-3,
              ti          ::Float64           = 0.0,
              ρ           ::Array{Float64,1}  = [1.0]) where {M,N,O}

  # read data
  tv, ed, el, bts, x, y =
    read_data_esse(states_file, tree_file, envdata_file)

  @info "Data for $(length(tv)) species successfully read"

  # scale y
  if scale_y[1]
    # if scale each function separately or together
    if scale_y[2]
      ymin = minimum(y)
      ymax = maximum(y)
      for j in axes(y,2), i in axes(y,1)
        y[i,j] = (y[i,j] - ymin)/(ymax - ymin)
      end
    else
      ymin = minimum(y, dims = 1)
      ymax = maximum(y, dims = 1)
      for j in axes(y,2), i in axes(y,1)
        y[i,j] = (y[i,j] - ymin[j])/(ymax[j] - ymin[j])
      end
    end
  end

  # prepare data
  X, p, fp, trios, ns, ned, pupd, phid, nnps, nps, mvps, nngps, mvhfs,
  dcp, dcfp, pardic, k, h, ny, model, af!, assign_hidfacs!, abts, bts, E0 =
        prepare_data(cov_mod, tv, x, y, ed, el, ρ, h, constraints, mvpars)

  @info "Data successfully prepared"

  ## make likelihood function
  # flow algorithm
  if occursin(r"^[f|F][A-za-z]*", algorithm)

    # prepare likelihood
    Gt, Et, lbts, nets, λevent!, rootll =
      prepare_ll(p, bts, E0, k, h, ny, ns, ned, model, Eδt, ti, abts, af!)

    # make likelihood function
    llf = make_loglik(Gt, Et, X, trios, lbts, bts, ns, ned, nets,
                      λevent!, rootll)
  # prunning algorithm
  elseif occursin(r"^[p|P][A-za-z]*", algorithm)

    # prepare likelihood
    X, int, λevent!, rootll, abts1, abts2 =
      prepare_ll(X, p, E0, ns, k, h, ny, model, abts ,af!)

    # make likelihood function
    llf = make_loglik(X, abts1, abts2, trios, int,
      λevent!, rootll, k, h, ns, ned)

  else
    @error "No matching likelihood for algorithm: $algorithm"
  end

  # create prior function
  lpf = make_lpf(pupd, phid,
    λpriors, μpriors, gpriors, lpriors, qpriors, βpriors, hpriors,
    k, h, ny, model)

  # create posterior functions
  lhf = make_lhf(llf, lpf, assign_hidfacs!, dcp, dcfp)

  # number of parameters
  npars = length(pardic)

  # number of samples
  nlogs = fld(niter,nthin)

  # if parallel
  if nchains > 1
    # where to write in the Shared Array
    cits = [(1+j):(nlogs+j) for j in 0:nlogs:(nchains-1)*nlogs]

    # run slice-sampling in parallel
    R = SharedArray{Float64,2}(nlogs*nchains, npars+2)

    # run parallel loop
    @sync @distributed for ci in Base.OneTo(nchains)
      R[cits[ci],:] =
        slice_sampler(lhf, p, fp, nnps, nps, phid, mvps, nngps, mvhfs, npars,
          niter, nthin, nburn, ntakew, winit, optimal_w, screen_print)
    end

    # write output
    write_ssr(R, pardic, out_file, cits)
  else

    R = slice_sampler(lhf, p, fp, nnps, nps, phid, mvps, nngps, mvhfs, npars,
          niter, nthin, nburn, ntakew, winit, optimal_w, screen_print)

    # write output
    write_ssr(R, pardic, out_file)
  end

  return R
end

"""
    esse(tv          ::Dict{Int64,Array{Float64,1}},
         ed          ::Array{Int64,2},
         el          ::Array{Float64,1},
         x           ::Array{Float64,1},
         y           ::Array{Float64,L},
         cov_mod     ::NTuple{M,String},
         out_file    ::String,
         h           ::Int64;
         constraints ::NTuple{N,String}  = (" ",),
         mvpars      ::NTuple{O,String}  = ("lambda = beta",),
         niter       ::Int64             = 10_000,
         nthin       ::Int64             = 10,
         nburn       ::Int64             = 200,
         nchains     ::Int64             = 1,
         ntakew      ::Int64             = 100,
         winit       ::Float64             = 2.0,
         scale_y     ::NTuple{2,Bool}    = (true, false),
         algorithm   ::String            = "pruning",
         λpriors     ::Float64           = .1,
         μpriors     ::Float64           = .1,
         gpriors     ::Float64           = .1,
         lpriors     ::Float64           = .1,
         qpriors     ::Float64           = .1,
         βpriors     ::NTuple{2,Float64} = (0.0, 10.0),
         hpriors     ::Float64           = .1,
         optimal_w   ::Float64           = 0.8,
         screen_print::Int64             = 5,
         Eδt         ::Float64           = 1e-3,
         ti          ::Float64           = 0.0,
         ρ           ::Array{Float64,1}  = [1.0]) where {L,M,N,O}

Wrapper for running a SSE model from simulations.
"""
function esse(tv          ::Dict{Int64,Array{Float64,1}},
              ed          ::Array{Int64,2},
              el          ::Array{Float64,1},
              x           ::Array{Float64,1},
              y           ::Array{Float64,L},
              cov_mod     ::NTuple{M,String},
              out_file    ::String,
              h           ::Int64;
              constraints ::NTuple{N,String}  = (" ",),
              mvpars      ::NTuple{O,String}  = ("lambda = beta",),
              niter       ::Int64             = 10_000,
              nthin       ::Int64             = 10,
              nburn       ::Int64             = 200,
              nchains     ::Int64             = 1,
              ntakew      ::Int64             = 100,
              winit       ::Float64             = 2.0,
              scale_y     ::NTuple{2,Bool}    = (true, false),
              algorithm   ::String            = "pruning",
              λpriors     ::Float64           = .1,
              μpriors     ::Float64           = .1,
              gpriors     ::Float64           = .1,
              lpriors     ::Float64           = .1,
              qpriors     ::Float64           = .1,
              βpriors     ::NTuple{2,Float64} = (0.0, 10.0),
              hpriors     ::Float64           = .1,
              optimal_w   ::Float64           = 0.8,
              screen_print::Int64             = 5,
              Eδt         ::Float64           = 1e-3,
              ti          ::Float64           = 0.0,
              ρ           ::Array{Float64,1}  = [1.0]) where {L,M,N,O}

  # prepare data
  X, p, fp, trios, ns, ned, pupd, phid, nnps, nps, mvps, nngps, mvhfs,
  dcp, dcfp, pardic, k, h, ny, model, af!, assign_hidfacs!, abts, bts, E0 =
        prepare_data(cov_mod, tv, x, y, ed, el, ρ, h, constraints, mvpars)

  @info "Data successfully prepared"

  ## make likelihood function
  # flow algorithm
  if occursin(r"^[f|F][A-za-z]*", algorithm)

    # prepare likelihood
    Gt, Et, lbts, nets, λevent!, rootll =
      prepare_ll(p, bts, E0, k, h, ny, ns, ned, model, Eδt, ti, abts, af!)

    # make likelihood function
    llf = make_loglik(Gt, Et, X, trios, lbts, bts, ns, ned, nets,
                      λevent!, rootll)
  # pruning algorithm
  elseif occursin(r"^[p|P][A-za-z]*", algorithm)

    # prepare likelihood
    X, int, λevent!, rootll, abts1, abts2 =
      prepare_ll(X, p, E0, ns, k, h, ny, model, abts ,af!)

    # make likelihood function
    llf = make_loglik(X, abts1, abts2, trios, int,
      λevent!, rootll, k, h, ns, ned)

  else
    @error "No matching likelihood for algorithm: $algorithm"
  end

  # create prior function
  lpf = make_lpf(pupd, phid,
    λpriors, μpriors, gpriors, lpriors, qpriors, βpriors, hpriors,
    k, h, ny, model)

  # create posterior functions
  lhf = make_lhf(llf, lpf, assign_hidfacs!, dcp, dcfp)

  # number of parameters
  npars = length(pardic)

  # number of samples
  nlogs = fld(niter,nthin)

  # if parallel
  if nchains > 1
    # where to write in the Shared Array
    cits = [(1+j):(nlogs+j) for j in 0:nlogs:(nchains-1)*nlogs]

    # run slice-sampling in parallel
    R = SharedArray{Float64,2}(nlogs*nchains, npars+2)

    # run parallel loop
    @sync @distributed for ci in Base.OneTo(nchains)
      R[cits[ci],:] =
        slice_sampler(lhf, p, fp, nnps, nps, phid, mvps, nngps, mvhfs, npars,
          niter, nthin, nburn, ntakew, winit, optimal_w, screen_print)
    end

    # write output
    write_ssr(R, pardic, out_file, cits)
  else

    R = slice_sampler(lhf, p, fp, nnps, nps, phid, mvps, nngps, mvhfs, npars,
          niter, nthin, nburn, ntakew, winit, optimal_w, screen_print)

    # write output
    write_ssr(R, pardic, out_file)
  end

  return R
end

"""
    read_data_esse(states_file ::String,
                   tree_file   ::String,
                   envdata_file::String)

Process tree and state and environmental data file to run ESSE.
"""
function read_data_esse(states_file ::String,
                        tree_file   ::String,
                        envdata_file::String)

  # read tree in postorder and assign to objects
  tree, bts = read_tree(tree_file, order = "postorder", branching_times = true)
  ntip = tree.nnod + 1
  ed   = tree.ed
  el   = tree.el
  tlab = tree.tlab

  # assign tip labels to edge numbers
  tip_labels = Dict{String,Integer}()
  ii = 0
  for i in Base.OneTo(size(ed,1))
    if ed[i,2] <= ntip
      ii += 1
      tip_labels[tlab[ii]] = ed[i,2]
    end
  end

  # read states text file
  data = readdlm(states_file)

  if size(data,1) != ntip
    data = readdlm(states_file, '\t', '\r')
  end

  if size(data,1) != ntip
    data = readdlm(states_file, '\t', '\n')
  end

  if size(data,1) != ntip
    error("Data file cannot be made of the right dimensions.\n Make sure the data file has the same number of rows as tips in the tree")
  end

  data_tlab    = convert(Array{String,1}, data[:,1])
  data_states  = convert(Array{Float64,2},  data[:,2:end])

  # create dictionary
  tip_states = Dict(tip_labels[val] => data_states[i,:]
                   for (i,val) = enumerate(data_tlab))

  # process environmental data file
  envdata = readdlm(envdata_file)

  x = envdata[:,1]
  y = envdata[:,2:end]

  return tip_states, ed, el, bts, x, y
end

"""
  write_ssr(R       ::Array{Float64,2},
            pardic  ::Dict{String,Int64},
            out_file::String)

Write the samples from an MC sampler data frame
given a Dictionary of parameters.
"""
function write_ssr(R       ::Array{Float64,2},
                   pardic  ::Dict{String,Int64},
                   out_file::String)

  # column names
  col_nam = ["Iteration", "Posterior"]

  for (k,v) in sort!(collect(pardic), by = x -> x[2])
    push!(col_nam, k)
  end

  R = vcat(reshape(col_nam, 1, lastindex(col_nam)), R)

  writedlm(out_file*".log", R)
end

"""
  write_ssr(R       ::SharedArray{Float64,2},
            pardic  ::Dict{String,Int64},
            out_file::String,
            cits    ::Array{UnitRange{Int64},1})

Write the samples from multiple chains of MC sampler data frame
given a Dictionary of parameters.
"""
function write_ssr(R       ::SharedArray{Float64,2},
                   pardic  ::Dict{String,Int64},
                   out_file::String,
                   cits    ::Array{UnitRange{Int64},1})

  # column names
  col_nam = ["Iteration", "Posterior"]

  for (k,v) in sort!(collect(pardic), by = x -> x[2])
    push!(col_nam, k)
  end

  for ci in Base.OneTo(length(cits))
    ri = vcat(reshape(col_nam, 1, lastindex(col_nam)), R[cits[ci],:])
    writedlm(out_file*"_chain_$ci.log", ri)
  end

end


"""
	ALConstraintSet{T}

An [`AbstractConstraintSet`](@ref) that stores the constraint values as well as Lagrange
multiplier and penalty terms for each constraint.

The cost associated with constraint terms in the augmented Lagrangian can be evaluated for

	cost!(J::Vector, ::ALConstraintSet)

which adds the cost at each time step to the vector `J` of length `N`.

The cost expansion for these terms is evaluated along the trajectory `Z` using

	cost_expansion!(E::Objective, conSet::ALConstraintSet, Z)

which also adds the expansion terms to the terms in `E`.

The penalty and multiplier terms can be updated using

	penalty_update!(::ALConstraintSet)
	dual_update!(::ALConstraintSet)

The current set of active constraint (with tolerance `tol`) can be re-calculated using

	update_active_set!(::ALConstraintSet, ::Val{tol})

The maximum penalty can be queried using `max_penalty(::ALConstraintSet)`, and the
penalties and/or multipliers can be reset using

	reset!(::ALConstraintSet)
	reset_penalties!(::ALConstraintSet)
	reset_duals!(::ALConstraintSet)

# Constructor
	ALConstraintSet(::ConstraintList, ::AbstractModel)
	ALConstraintSet(::Problem)
"""
struct ALConstraintSet{T} <: TO.AbstractConstraintSet
    convals::Vector{ALConVal}
    errvals::Vector{ALConVal}          # TODO: is this needed?
	# ∇c_proj::Vector{<:Vector}        # Jacobians of projected constraints
    # λ::Vector{<:Vector}
    # μ::Vector{<:Vector}
	# active::Vector{<:Vector}
    c_max::Vector{T}
    μ_max::Vector{T}
    μ_maxes::Vector{Vector{T}}
	# params::Vector{TO.ConstraintParams{T}}
	p::Vector{Int}
end

function ALConstraintSet(cons::TO.ConstraintList, model::RD.AbstractModel)
    n,m = cons.n, cons.m
    n̄ = RobotDynamics.state_diff_size(model)
    ncon = length(cons)
    useG = model isa RD.LieGroupModel
	errvals = map(1:ncon) do i
        C,c = TO.gen_convals(n̄, m, cons[i], cons.inds[i])
        ALConVal(n̄, m, cons[i], cons.inds[i], C, c, useG)
    end
    convals = map(errvals) do errval
        ALConVal(n, m, errval)
    end
	errvals = convert(Vector{ALConVal}, errvals)
	convals = convert(Vector{ALConVal}, convals)
	# ∇c_proj = map(1:ncon) do i
	# 	[copy(errvals[i].jac[j,1]) for j in eachindex(cons.inds[i])]
	# end
    # λ = map(1:ncon) do i
	# 	p = length(cons[i])
	# 	convals[i].λ
    #     # [@SVector zeros(p) for j in cons.inds[i]]
    # end
    # μ = map(1:ncon) do i
	# 	p = length(cons[i])
	# 	convals[i].μ
    #     # [@SVector ones(p) for j in cons.inds[i]]
    # end
    # a = map(1:ncon) do i
    #     p = length(cons[i])
    #     [@SVector ones(Bool,p) for j in cons.inds[i]]
	# end
	# if ncon == 0
	# 	∇c_proj = Vector{Float64}[]
	# 	λ = Vector{Float64}[]
	# 	μ = Vector{Float64}[]
	# 	a = Vector{Float64}[]
	# end
    c_max = zeros(ncon)
    μ_max = zeros(ncon)
    μ_maxes = [zeros(length(ind)) for ind in cons.inds]
	# params = [TO.ConstraintParams() for con in cons.constraints]
    # ALConstraintSet(convals, errvals, ∇c_proj, λ, μ, a, c_max, μ_max, μ_maxes, params, copy(cons.p))
    ALConstraintSet(convals, errvals, c_max, μ_max, μ_maxes, copy(cons.p))
end

@inline ALConstraintSet(prob::Problem) = ALConstraintSet(prob.constraints, prob.model)

@inline TO.get_convals(conSet::ALConstraintSet) = conSet.convals
@inline TO.get_errvals(conSet::ALConstraintSet) = conSet.errvals

get_duals(conSet::ALConstraintSet) = [cval.λ for cval in conSet.convals]
function set_duals!(conSet::ALConstraintSet, λ)
	for i = 1:length(conSet)
		set_duals!(conSet.convals[i], λ[i])
	end
end
function set_duals!(cval::ALConVal, λ)
	for i = 1:length(cval.λ)
		cval.λ[i] .= λ[i]
	end
end

for method in (:violation!, :∇violation!, :dual_update!, :penalty_update!, :reset_duals!, :reset_penalties!)
	@eval function $method(conSet::ALConstraintSet)
		for i = 1:length(conSet)
			$method(conSet.convals[i])
		end
	end
end

############################################################################################
#                             Augmented Lagrangian Updates
############################################################################################

# function dual_update!(conSet::ALConstraintSet)
#     for i in eachindex(conSet.λ)
#         dual_update!(conSet.convals[i])
# 	end
# end

function dual_update!(conval::ALConVal)
	c = conval.vals
	λ = conval.λ
	μ = conval.μ
	λ_max = conval.params.λ_max
	cone = TO.sense(conval.con)
	# λ_min = TO.sense(conval.con) == Equality() ? -λ_max : zero(λ_max)
	for i in eachindex(conval.inds)
		λ[i] .= dual_update(cone, SVector(λ[i]), SVector(c[i]), SVector(μ[i]), λ_max)
	end
end

function dual_update(::Equality, λ, c, μ, λmax)
	λbar = λ + μ .* c
	return clamp.(λbar, -λmax, λmax)
end

function dual_update(::Inequality, λ, c, μ, λmax)
 	λbar = λ + μ .* c
	return clamp.(λbar, 0, λmax)  # project onto the dual cone via max(0,x)
end

function dual_update(cone::SecondOrderCone, λ, c, μ, λmax)
	 λbar = λ - μ .* c
	 return TO.projection(cone, λbar)  # project onto the dual cone
end

# function penalty_update!(conSet::ALConstraintSet)
# 	for i in eachindex(conSet.μ)
# 		penalty_update!(conSet.convals[i])
# 	end
# end

function penalty_update!(cval::ALConVal)
	μ = cval.μ
	ϕ = cval.params.ϕ
	μ_max = cval.params.μ_max
	for i = 1:length(μ)
		μ[i] .*= ϕ
		clamp!(μ[i], 0, μ_max)
	end
end

# Active Set
function update_active_set!(conSet::ALConstraintSet, val::Val{tol}=Val(0.0)) where tol
	for i in 1:length(conSet)
		# update_active_set!(conSet.active[i], conSet.λ[i], conSet.convals[i], val)
		update_active_set!(conSet.convals[i], val)
	end
end

function update_active_set!(conval::ALConVal, ::Val{tol}) where tol
	a = conval.active
	λ = conval.λ
	if TO.sense(conval.con) == TO.Inequality()
		for i in eachindex(a)
			a[i] = @. (conval.vals[i] >= -tol) | (λ[i] > zero(tol))
		end
	end
end

"""
	max_penalty(conSet::ALConstraintSet)

Calculate the maximum constrained penalty across all constraints.
"""
function max_penalty(conSet::ALConstraintSet)
	max_penalty!(conSet)
	maximum(conSet.μ_max)
end

function max_penalty!(conSet::ALConstraintSet{T}) where T
    conSet.c_max .*= 0
	for i in 1:length(conSet)
		maxes = conSet.μ_maxes[i]::Vector{T}
        max_penalty!(maxes, conSet.convals[i])
        conSet.μ_max[i] = maximum(maxes)
    end
end

function max_penalty!(μ_max::Vector{<:Real}, cval::ALConVal)
    for i in eachindex(cval.μ)
        μ_max[i] = maximum(cval.μ[i])
    end
    return nothing
end

############################################################################################
#                             Constraint Penalties
############################################################################################
# function projection_jacobians!(conSet::ALConstraintSet)
# 	for i in eachindex(conSet.convals)
# 		projection_jacobians!(conSet.convals[i])
# 	end
# end

# """
# 	constraint_penalty!(conSet::ALConstraintSet)

# Evaluate the penalty of the constraints. For equality constraints, this is the constraint
# itself. For generalized inequalities, this is the projection of the constraint onto the
# appropriate cone.
# """
# function constraint_penalty!(conSet::ALConstraintSet)
# 	for i in eachindex(conSet.convals)
# 		constraint_penalty!(conSet.convals[i], conSet.λ[i])
# 	end
# end

# function constraint_penalty!(conval::TO.ConVal, λ)
# 	for i in eachindex(conval.inds)
# 		conval.vals2[i] = penalty(TO.sense(conval.con), conval.vals[i], λ[i])
# 	end
# end

# """
# 	penalty_jacobian!(conSet::ALConstraintSet)

# Evaluate the Jacobian of the constraint penalty. For equality constraints, this is simply
# the Jacobian of the constraint itself. For generalized inequalities, this is `∇proj*∇c`
# where `∇proj` is the Jacobian of the projection of the constraint onto the corresponding
# cone.
# """
# function penalty_jacobian!(conSet::ALConstraintSet)
# 	for i in eachindex(conSet.errvals)
# 		penalty_jacobian!(conSet.∇c_proj[i], conSet.errvals[i], conSet.λ[i])
# 	end
# end

# function penalty_jacobian!(∇c_proj::Vector, conval::TO.ConVal, λ::Vector)
# 	if size(conval.jac, 2) > 1
# 		throw(ErrorException("Constraint projection not supported for CoupledConstraints"))
# 	end
# 	for i in eachindex(conval.inds)
# 		∇penalty!(TO.sense(conval.con), ∇c_proj[i], conval.jac[i], conval.vals[i], λ[i])
# 	end
# end

############################################################################################
#                                        Cost
############################################################################################

# function TO.cost!(J::Vector{<:Real}, conSet::ALConstraintSet)
# 	for i in eachindex(conSet.convals)
# 		TO.cost!(J, conSet.convals[i], conSet.λ[i], conSet.μ[i], conSet.active[i])
# 	end
# end

# function TO.cost!(J::Vector{<:Real}, conval::ALConVal, λ::Vector{<:StaticVector},
# 		μ::Vector{<:StaticVector}, a::Vector{<:StaticVector})
# 	for (i,k) in enumerate(conval.inds)
# 		c = SVector(conval.vals[i])
# 		Iμ = Diagonal(SVector(μ[i] .* a[i]))
# 		J[k] += λ[i]'c .+ 0.5*c'Iμ*c
# 	end
# end

# function TO.cost_expansion!(E::Objective, conSet::ALConstraintSet, Z::AbstractTrajectory,
# 		init::Bool=false, rezero::Bool=false)
# 	for i in eachindex(conSet.errvals)
# 		TO.cost_expansion!(E, conSet.convals[i], conSet.λ[i], conSet.μ[i], conSet.active[i])
# 	end
# end

# @generated function TO.cost_expansion!(E::QuadraticObjective{n,m}, conval::ALConVal{C}, λ, μ, a) where {n,m,C}
# 	if C <: TO.StateConstraint
# 		expansion = quote
# 			cx = ∇c
# 			E[k].Q .+= cx'Iμ*cx
# 			E[k].q .+= cx'g
# 		end
# 	elseif C <: TO.ControlConstraint
# 		expansion = quote
# 			cu = ∇c
# 			E[k].R .+= cu'Iμ*cu
# 			E[k].r .+= cu'g
# 		end
# 	elseif C<: TO.StageConstraint
# 		ix = SVector{n}(1:n)
# 		iu = SVector{m}(n .+ (1:m))
# 		expansion = quote
# 			cx = ∇c[:,$ix]
# 			cu = ∇c[:,$iu]
# 			E[k].Q .+= cx'Iμ*cx
# 			E[k].q .+= cx'g
# 			E[k].H .+= cu'Iμ*cx
# 			E[k].R .+= cu'Iμ*cu
# 			E[k].r .+= cu'g
# 		end
# 	else
# 		throw(ArgumentError("cost expansion not supported for CoupledConstraints"))
# 	end
# 	quote
# 		for (i,k) in enumerate(conval.inds)
# 			∇c = SMatrix(conval.jac[i])
# 			c = conval.vals[i]
# 			Iμ = Diagonal(a[i] .* μ[i])
# 			g = Iμ*c .+ λ[i]

# 			$expansion
# 		end
# 	end
# end

############################################################################################
#                                  RESET
############################################################################################

function reset!(conSet::ALConstraintSet)
    reset_duals!(conSet)
    reset_penalties!(conSet)
end

# function reset_duals!(conSet::ALConstraintSet)
# 	for i = 1:length(conSet)
# 		reset_duals!(conSet.convals[i])
# 	end
# end

# function reset_penalties!(conSet::ALConstraintSet)
# 	for i = 1:length(conSet)
# 		reset_penalties!(conSet.convals[i])
# 	end
# end

"""
	link_constraints!(set1, set2)

Link any common constraints between `set1` and `set2` by setting elements in `set1` to point
to elements in `set2`
"""
function link_constraints!(set1::ALConstraintSet, set2::ALConstraintSet)
	# Find common constraints
	links = Tuple{Int,Int}[]
	for (i,con1) in enumerate(set1)
		for (j,con2) in enumerate(set2)
			if con1 === con2
				push!(links, (i,j))
			end
		end
	end

	# Link values
	for (i,j) in links
		set1.convals[i] = set2.convals[j]
		set1.errvals[i] = set2.errvals[j]
		# set1.active[i] = set2.active[j]
		# set1.λ[i] = set2.λ[j]
		# set1.μ[i] = set2.μ[j]
	end
	return links
end

function shift_fill!(conSet::ALConstraintSet, n=1)
	for i = 1:length(conSet)
		shift_fill!(conSet.convals[i], n)
	end
end

############################################################################################
#                                    Solver Options
############################################################################################
function reset!(conSet::ALConstraintSet{T}, opts::SolverOptions{T}) where T
    # if !isnan(opts.dual_max)
    #     for params in conSet.params
    #         params.λ_max = opts.dual_max
    #     end
    # end
    # if !isnan(opts.penalty_max)
    #     for params in conSet.params
    #         params.μ_max = opts.penalty_max
    #     end
    # end
    # if !isnan(opts.penalty_initial)
    #     for params in conSet.params
    #         params.μ0 = opts.penalty_initial
    #     end
    # end
    # if !isnan(opts.penalty_scaling)
    #     for params in conSet.params
    #         params.ϕ = opts.penalty_scaling
    #     end
	# end
	for i = 1:length(conSet)
		set_params!(conSet.convals[i], opts)
	end
    if opts.reset_duals
        reset_duals!(conSet)
    end
    if opts.reset_penalties
        reset_penalties!(conSet)
    end
end


#####################################################################
# special.jl
#
# includes non-standard code I needed for papers and whatnot
######################################################################

function norm2(x::LocTuple, y::LocTuple)
	d1 = x[1] - y[1]
	d2 = x[2] - y[2]
	return sqrt(d1*d1 + d2*d2)
end

function sim(j::LocTuple, actions::Vector{Pose}, observations::Vector{Float64}, b::Vector{Matrix{Float64}}, m::SearchDomain, x::Vehicle; show_mean=false, show_cov=false)
	# always start the vehicle in the center
	# NO, don't do that
	#x.x = m.length / 2.0
	#x.y = m.length / 2.0
	theta!(m, j[1], j[2])

	# warn user if jammer is not where it should be
	#if abs(dx) > x.x || abs(dy) > x.y
	#	println("WARNING: jammer outside search domain.")
	#end

	# loop through all observations...
	for (oi,o) in enumerate(observations)
		#update!(f, x, o)
		#plot(m, b[oi], x, show_mean=true, show_cov=true)
		plot(m, b[oi], x, show_mean=show_mean, show_cov=show_cov, obs=o)
		savefig("temp_$(oi).png", format="png")
		hold(false)
		act!(m, x, actions[oi])
	end
end

export sim2
function sim2(j::LocTuple, states::Vector{Pose}, observations::Vector{Float64}, b::Vector{Matrix{Float64}}, m::SearchDomain, x::Vehicle; show_mean=false, show_cov=false, show_path::Bool=true)
	# always start the vehicle in the center
	# NO, don't do that
	#x.x = m.length / 2.0
	#x.y = m.length / 2.0
	theta!(m, j[1], j[2])

	path_x = Float64[]
	path_y = Float64[]

	# loop through all observations...
	for (oi,o) in enumerate(observations)
		if show_path
			#plot path
			plot(path_x, path_y, "k")
		end
		hold(true)
		plot(m, b[oi], x, show_mean=show_mean, show_cov=show_cov, obs=oi)
		#savefig("temp_$(oi).pdf", format="pdf", dpi=300)
		savefig("temp_$(oi).png", format="png")
		hold(false)
		push!(path_x, x.x)
		push!(path_y, x.y)
		x.x = states[oi][1]
		x.y = states[oi][2]
		x.heading = states[oi][3]
		#act!(m, x, actions[oi])
	end
end

# This was for AIAA GNC 2018 paper.
# This probably shouldn't be here.
export eval2
function eval2(j::LocTuple, states::Vector{Pose}, observations::Vector{Float64}, b::Vector{Matrix{Float64}}, m::SearchDomain, x::Vehicle; show_mean=false, show_cov=false, show_path::Bool=true)
	# always start the vehicle in the center
	# NO, don't do that
	#x.x = m.length / 2.0
	#x.y = m.length / 2.0
	theta!(m, j[1], j[2])

    front_cone = Float64[]
    rear_cone = Float64[]
    sides = Float64[]

	# loop through all observations...
	for (oi,o) in enumerate(observations)
        # get true bearing between x
        xt = (x.x,x.y,x.heading)
        rel_bearing = fit_180(xt[3] - true_bearing(xt, m.theta))
        if rel_bearing < 0.0
            rel_bearing = -1.0 * rel_bearing
        end

        cla()
		plot(m, b[oi], x, show_mean=show_mean, show_cov=show_cov, obs=oi)
        #title("o = $o")
        #hold(false)
        if rel_bearing < 60.0
            # expect observation 1
            title("expect 1, got $o")
            push!(front_cone, o)
        elseif rel_bearing < 120.0
            title("expect either, got $o")
            push!(sides, o)
        else
            title("expect 0, got $o")
            push!(rear_cone,o)
        end
        pause(0.15)

        # update vehicle position
		x.x = states[oi][1]
		x.y = states[oi][2]
		x.heading = states[oi][3]
	end
    return front_cone, sides, rear_cone
end

function simulate(m::SearchDomain, uav::SimUnit;
                  video::Bool=true,
                  pause_time=0.3
                 )

    # reset the filter, vehicle, and policy
    # TODO: I think I assume the SimUnit comes in clean and ready to go
    #reset!(uav.f)
    #reset!(m, uav.x)
    #reset!(uav.p)

    # What was the cost to getting this first observation?
    temp_cost = get_cost(uav, m)

    # before doing anything else, we observe
    #  and update filter once
    o = observe(m, uav.x)
    update!(uav, o)

    # This was our first step; steps count number of observations
    step_count = 1

    # plot if need be
    if video
        figure("Simulation")
        plot(m, uav.f, uav.x)
        title("i = $(step_count)")
    end

    while !is_complete(uav.f, uav.tc, step_count)
        # act
        a = action(m, uav, o)
        act!(m, uav.x, a)

        # get cost and update step count
        temp_cost += get_cost(uav, m, a)
        step_count += 1

        # observe and update
        o = observe(m, uav.x)
        update!(uav, o)

        # plot if need be
        if video
            pause(pause_time)
            figure("Simulation")
            cla()
            plot(m, uav.f, uav.x)
            max_b = maximum(uav.f.b)
            title("i = $(step_count), max = $(round(max_b,3))")
        end
    end

    return temp_cost
end


"""
    VF2

An empty concrete type used to dispatch to [`vf2`](@ref) isomorphism functions.
"""
struct VF2 <: IsomorphismAlgorithm end

"""
    VF2State{G, T}

Structure that is internally used by vf2
"""
struct VF2State{G, T}
    g1::G
    g2::G
    core_1::Vector{T}
    core_2::Vector{T}
    in_1::Vector{T}
    in_2::Vector{T}
    out_1::Vector{T}
    out_2::Vector{T}

    function VF2State(g1::G, g2::G) where {G <: AbstractSimpleGraph{T}} where {T <: Integer}
        n1 = nv(g1)
        n2 = nv(g2)
        core_1 = zeros(T, n1)
        core_2 = zeros(T, n2)
        in_1 = zeros(T, n1)
        in_2 = zeros(T, n2)
        out_1 = zeros(T, n1)
        out_2 = zeros(T, n2)

        return new{G, T}(g1, g2, core_1, core_2, in_1, in_2, out_1, out_2)
    end
end

"""
    vf2(callback, g1, g2, problemtype; vertex_relation=nothing, edge_relation=nothing)

Iterate over all isomorphism between the graphs `g1` (or subgraphs thereof) and `g2`.
The problem that is solved depends on the value of `problemtype`:
- IsomorphismProblem(): Only isomorphisms between the whole graph `g1` and `g2` are considered.
- SubGraphIsomorphismProblem(): All isomorphism between subgraphs of `g1` and `g2` are considered.
- InducedSubGraphIsomorphismProblem(): All isomorphism between vertex induced subgraphs of `g1` and `g2` are considered.

Upon finding an isomorphism, the function `callback` is called with a vector `vmap` as an argument.
`vmap` is a vector where `vmap[v] == u` means that vertex `v` in `g2` is mapped to vertex `u` in `g1`.
If the algorithm should look for another isomorphism, then this function should return `true`.

### Optional Arguments
- `vertex_relation`: A binary function that takes a vertex from `g1` and one from `g2`. An
    isomorphism only exists if this function returns `true` for all matched vertices.
- `edge_relation`: A binary function that takes an edge from `g1` and one from `g2`. An
    isomorphism only exists if this function returns `true` for all matched edges.

### References
Luigi P. Cordella, Pasquale Foggia, Carlo Sansone, Mario Vento
“A (Sub)Graph Isomorphism Algorithm for Matching Large Graphs”
"""
function vf2(callback::Function, g1::G, g2::G, problemtype::GraphMorphismProblem;
             vertex_relation::Union{Nothing, Function}=nothing,
             edge_relation::Union{Nothing, Function}=nothing) where {G <: AbstractSimpleGraph}
    if nv(g1) < nv(g2) || (problemtype == IsomorphismProblem() && nv(g1) != nv(g2))
        return
    end

    start_state = VF2State(g1, g2)
    start_depth = 1
    vf2match!(start_state, start_depth, callback, problemtype, vertex_relation, edge_relation)
    return
end

"""
    vf2check_feasibility(u, v, state, problemtype, vertex_relation, edge_relation)

Check whether two vertices of G₁ and G₂ can be matched. Used by [`vf2match!`](@ref).
"""
function vf2check_feasibility(u, v, state::VF2State, problemtype,
                              vertex_relation::Union{Nothing, Function},
                              edge_relation::Union{Nothing, Function})
    @inline function vf2rule_pred(u, v, state::VF2State, problemtype)
        if problemtype != SubGraphIsomorphismProblem()
            @inbounds for u2 in inneighbors(state.g1, u)
                if state.core_1[u2] != 0
                    found = false
                    # TODO can probably be replaced with has_edge for better performance
                    for v2 in inneighbors(state.g2, v)
                        if state.core_1[u2] == v2
                            found = true
                            break
                        end
                    end
                    found || return false
                end
            end
        end
        @inbounds for v2 in inneighbors(state.g2, v)
            if state.core_2[v2] != 0
                found = false
                for u2 in inneighbors(state.g1, u)
                    if state.core_2[v2] == u2
                        found = true
                        break
                    end
                end
                found || return false
            end
        end
        return true
    end

    @inline function vf2rule_succ(u, v, state::VF2State, problemtype)
        if problemtype != SubGraphIsomorphismProblem()
            @inbounds for u2 in outneighbors(state.g1, u)
                if state.core_1[u2] != 0
                    found = false
                    for v2 in outneighbors(state.g2, v)
                        if state.core_1[u2] == v2
                            found = true
                            break
                        end
                    end
                    found || return false
                end
            end
        end
        found = false
        @inbounds for v2 in outneighbors(state.g2, v)
            if state.core_2[v2] != 0
                found = false
                for u2 in outneighbors(state.g1, u)
                    if state.core_2[v2] == u2
                        found = true
                        break
                    end
                end
                found || return false
            end
        end
        return true
    end

    @inline function vf2rule_in(u, v, state::VF2State, problemtype)
        count1 = 0
        count2 = 0
        @inbounds for u2 in outneighbors(state.g1, u)
            if state.in_1[u2] != 0 && state.core_1[u2] == 0
                count1 += 1
            end
        end
        @inbounds for v2 in outneighbors(state.g2, v)
            if state.in_2[v2] != 0 && state.core_2[v2] == 0
                count2 += 1
            end
        end
        if problemtype == IsomorphismProblem()
            count1 == count2 || return false
        else
            count1 >= count2 || return false
        end
        count1 = 0
        count2 = 0
        @inbounds for u2 in inneighbors(state.g1, u)
            if state.in_1[u2] != 0 && state.core_1[u2] == 0
                count1 += 1
            end
        end
        @inbounds for v2 in inneighbors(state.g2, v)
            if state.in_2[v2] != 0 && state.core_2[v2] == 0
                count2 += 1
            end
        end
        problemtype == IsomorphismProblem() && return count1 == count2

        return count1 >= count2
    end

    @inline function vf2rule_out(u, v, state::VF2State, problemtype)
        count1 = 0
        count2 = 0
        @inbounds for u2 in outneighbors(state.g1, u)
            if state.out_1[u2] != 0 && state.core_1[u2] == 0
                count1 += 1
            end
        end
        @inbounds for v2 in outneighbors(state.g2, v)
            if state.out_2[v2] != 0 && state.core_2[v2] == 0
                count2 += 1
            end
        end
        if problemtype == IsomorphismProblem()
            count1 == count2 || return false
        else
            count1 >= count2 || return false
        end

        count1 = 0
        count2 = 0
        @inbounds for u2 in inneighbors(state.g1, u)
            if state.out_1[u2] != 0 && state.core_1[u2] == 0
                count1 += 1
            end
        end
        @inbounds for v2 in inneighbors(state.g2, v)
            if state.out_2[v2] != 0 && state.core_2[v2] == 0
                count2 += 1
            end
        end
        problemtype == IsomorphismProblem() && return count1 == count2

        return count1 >= count2
    end

    @inline function vf2rule_new(u, v, state::VF2State, problemtype)
        problemtype == SubGraphIsomorphismProblem() && return true
        count1 = 0
        count2 = 0
        @inbounds for u2 in inneighbors(state.g1, u)
            if state.in_1[u2] == 0 && state.out_1[u2] == 0
                count1 += 1
            end
        end
        @inbounds for v2 in inneighbors(state.g2, v)
            if state.in_2[v2] == 0 && state.out_2[v2] == 0
                count2 += 1
            end
        end
        if problemtype == IsomorphismProblem()
            count1 == count2 || return false
        else
            count1 >= count2 || return false
        end
        count1 = 0
        count2 = 0
        @inbounds for u2 in outneighbors(state.g1, u)
            if state.in_1[u2] == 0 && state.out_1[u2] == 0
                count1 += 1
            end
        end
        @inbounds for v2 in outneighbors(state.g2, v)
            if state.in_2[v2] == 0 && state.out_2[v2] == 0
                count2 += 1
            end
        end
        problemtype == IsomorphismProblem() && return count1 == count2

        return count1 >= count2
    end

    @inline function vf2rule_self_loops(u, v, state, problemtype)
        u_selflooped = has_edge(state.g1, u, u)
        v_selflooped = has_edge(state.g2, v, v)

        if problemtype == SubGraphIsomorphismProblem()
            return u_selflooped || !v_selflooped
        end
        return u_selflooped == v_selflooped
    end

    syntactic_feasability = vf2rule_pred(u, v, state, problemtype) &&
                            vf2rule_succ(u, v, state, problemtype) &&
                            vf2rule_in(u, v, state, problemtype)   &&
                            vf2rule_out(u, v, state, problemtype)  &&
                            vf2rule_new(u, v, state, problemtype)  &&
                            vf2rule_self_loops(u, v, state, problemtype)
    syntactic_feasability || return false

    if vertex_relation != nothing
        vertex_relation(u, v) || return false
    end
    if edge_relation != nothing
        E1 = edgetype(state.g1)
        E2 = edgetype(state.g2)
        for u2 in outneighbors(state.g1, u)
            state.core_1[u2] == 0 && continue
            v2 = state.core_1[u2]
            edge_relation(E1(u, u2), E2(v, v2)) || return false
        end
        for u2 in inneighbors(state.g1, u)
            state.core_1[u2] == 0 && continue
            v2 = state.core_1[u2]
            edge_relation(E1(u2, u), E2(v2, v)) || return false
        end
    end
    return true
end

"""
    vf2update_state!(state, u, v, depth)

Update state before recursing. Helper function for [`vf2match!`](@ref).
"""
function vf2update_state!(state::VF2State, u, v, depth)
@inbounds begin
     state.core_1[u] = v
     state.core_2[v] = u
     for w in outneighbors(state.g1, u)
         if state.out_1[w] == 0
             state.out_1[w] = depth
         end
     end
     for w in inneighbors(state.g1, u)
         if state.in_1[w] == 0
             state.in_1[w] = depth
         end
     end
     for w in outneighbors(state.g2, v)
         if state.out_2[w] == 0
             state.out_2[w] = depth
         end
     end
     for w in inneighbors(state.g2, v)
         if state.in_2[w] == 0
             state.in_2[w] = depth
         end
     end
end
end

"""
    vf2reset_state!(state, u, v, depth)

Reset state after returning from recursion. Helper function for [`vf2match!`](@ref).
"""
function vf2reset_state!(state::VF2State, u, v, depth)
@inbounds begin
    state.core_1[u] = 0
    state.core_2[v] = 0
    for w in outneighbors(state.g1, u)
        if state.out_1[w] == depth
            state.out_1[w] = 0
        end
    end
    for w in inneighbors(state.g1, u)
        if state.in_1[w] == depth
            state.in_1[w] = 0
        end
    end
    for w in outneighbors(state.g2, v)
        if state.out_2[w] == depth
            state.out_2[w] = 0
        end
    end
    for w in inneighbors(state.g2, v)
        if state.in_2[w] == depth
            state.in_2[w] = 0
        end
    end
end
end

"""
    vf2match!(state, depth, callback, problemtype, vertex_relation, edge_relation)

Perform isomorphic subgraph matching. Called by [`vf2`](@ref).
"""
function vf2match!(state, depth, callback::Function, problemtype::GraphMorphismProblem,
                   vertex_relation, edge_relation)
    n1 = Int(nv(state.g1))
    n2 = Int(nv(state.g2))
    # if all vertices of G₂ are matched we call the callback function. If the
    # algorithm should look for another isomorphism then callback has to return true
    if depth > n2
        keepgoing = callback(state.core_2)
        return keepgoing
    end
    # First we try if there is a pair of unmatched vertices u∈G₁ v∈G₂ that are connected
    # by an edge going out of the set M(s) of already matched vertices
    found_pair = false
    v = 0
    @inbounds for j = 1:n2
        if state.out_2[j] != 0 && state.core_2[j] == 0
            v = j
            break
        end
    end
    if v != 0
        @inbounds for u = 1:n1
            if state.out_1[u] != 0 && state.core_1[u] == 0
                found_pair = true
                if vf2check_feasibility(u, v, state, problemtype, vertex_relation, edge_relation)
                    vf2update_state!(state, u, v, depth)
                    keepgoing = vf2match!(state, depth + 1, callback, problemtype,
                                          vertex_relation, edge_relation)
                    keepgoing || return false
                    vf2reset_state!(state, u, v, depth)
                end
            end
        end
    end
    found_pair && return true
    # If that is not the case we try if there is a pair of unmatched vertices u∈G₁ v∈G₂ that
    # are connected  by an edge coming in from the set M(s) of already matched vertices
    v = 0
    @inbounds for j = 1:n2
        if state.in_2[j] != 0 && state.core_2[j] == 0
            v = j
            break
        end
    end
    if v != 0
        @inbounds for u = 1:n1
            if state.in_1[u] != 0 && state.core_1[u] == 0
                found_pair = true
                if vf2check_feasibility(u, v, state, problemtype, vertex_relation, edge_relation)
                    vf2update_state!(state, u, v, depth)
                    keepgoing = vf2match!(state, depth + 1, callback, problemtype,
                                          vertex_relation, edge_relation)
                    keepgoing || return false
                    vf2reset_state!(state, u, v, depth)
                end
            end
        end
    end
    found_pair && return true
    # If this is also not the case, we try all pairs of vertices u∈G₁ v∈G₂ that are not
    # yet matched
    v = 0
    @inbounds for j = 1:n2
        if state.core_2[j] == 0
            v = j
            break
        end
    end
    if v != 0
        @inbounds for u = 1:n1
            if state.core_1[u] == 0
                if vf2check_feasibility(u, v, state, problemtype, vertex_relation, edge_relation)
                    vf2update_state!(state, u, v, depth)
                    keepgoing = vf2match!(state, depth + 1, callback, problemtype,
                                          vertex_relation, edge_relation)
                    keepgoing || return false
                    vf2reset_state!(state, u, v, depth)
                end
            end
        end
    end
    return true
end

function has_induced_subgraphisomorph(g1::AbstractGraph, g2::AbstractGraph, alg::VF2;
                                 vertex_relation::Union{Nothing, Function}=nothing,
                                 edge_relation::Union{Nothing, Function}=nothing)::Bool
        result = false
        callback(vmap) = (result = true; return false)
        vf2(callback, g1, g2, InducedSubGraphIsomorphismProblem();
                       vertex_relation=vertex_relation, edge_relation=edge_relation)
        return result
end

function has_subgraphisomorph(g1::AbstractGraph, g2::AbstractGraph, alg::VF2;
                                 vertex_relation::Union{Nothing, Function}=nothing,
                                 edge_relation::Union{Nothing, Function}=nothing,)::Bool
    result = false
    callback(vmap) = (result = true; return false)
    vf2(callback, g1, g2, SubGraphIsomorphismProblem();
        vertex_relation=vertex_relation, edge_relation=edge_relation)
    return result
end

function has_isomorph(g1::AbstractGraph, g2::AbstractGraph, alg::VF2;
                         vertex_relation::Union{Nothing, Function}=nothing,
                         edge_relation::Union{Nothing, Function}=nothing)::Bool

    !could_have_isomorph(g1, g2) && return false

    result = false
    callback(vmap) = (result = true; return false)
    vf2(callback, g1, g2, IsomorphismProblem(),
        vertex_relation=vertex_relation,
        edge_relation=edge_relation)
    return result
end

function count_induced_subgraphisomorph(g1::AbstractGraph, g2::AbstractGraph, alg::VF2;
                                   vertex_relation::Union{Nothing, Function}=nothing,
                                   edge_relation::Union{Nothing, Function}=nothing)::Int
    result = 0
    callback(vmap) = (result += 1; return true)
    vf2(callback, g1, g2, InducedSubGraphIsomorphismProblem(),
        vertex_relation=vertex_relation,
        edge_relation=edge_relation)
    return result
end

function count_subgraphisomorph(g1::AbstractGraph, g2::AbstractGraph, alg::VF2;
                                vertex_relation::Union{Nothing, Function}=nothing,
                                edge_relation::Union{Nothing, Function}=nothing)::Int
    result = 0
    callback(vmap) = (result += 1; return true)
    vf2(callback, g1, g2, SubGraphIsomorphismProblem(), vertex_relation=vertex_relation, edge_relation=edge_relation)
    return result
end

function count_isomorph(g1::AbstractGraph, g2::AbstractGraph, alg::VF2;
                        vertex_relation::Union{Nothing, Function}=nothing,
                        edge_relation::Union{Nothing, Function}=nothing)::Int
    !could_have_isomorph(g1, g2) && return 0
    result = 0
    callback(vmap) = (result += 1; return true)
    vf2(callback, g1, g2, IsomorphismProblem(), vertex_relation=vertex_relation, edge_relation=edge_relation)
    return result
end

function all_induced_subgraphisomorph(g1::AbstractGraph, g2::AbstractGraph, alg::VF2;
                                 vertex_relation::Union{Nothing, Function}=nothing,
                                 edge_relation::Union{Nothing, Function}=nothing)::Channel{Vector{Tuple{eltype(g1),eltype(g2)}}}
        make_callback(c) = vmap -> (put!(c, collect(zip(vmap,1:length(vmap)))), return true)
    T = Vector{Tuple{eltype(g1), eltype(g2)}}
    ch::Channel{T} = Channel(ctype=T) do c
        vf2(make_callback(c), g1, g2, InducedSubGraphIsomorphismProblem(),
                       vertex_relation=vertex_relation,
                       edge_relation=edge_relation)
    end
end

function all_subgraphisomorph(g1::AbstractGraph, g2::AbstractGraph, alg::VF2;
                              vertex_relation::Union{Nothing, Function}=nothing,
                              edge_relation::Union{Nothing, Function}=nothing)::Channel{Vector{Tuple{eltype(g1), eltype(g2)}}}

    make_callback(c) = vmap -> (put!(c, collect(zip(vmap,1:length(vmap)))), return true)
    T = Vector{Tuple{eltype(g1), eltype(g2)}}
    ch::Channel{T} = Channel(ctype=T) do c
        vf2(make_callback(c), g1, g2, SubGraphIsomorphismProblem(),
            vertex_relation=vertex_relation,
            edge_relation=edge_relation)
    end
    return ch
end

function all_isomorph(g1::AbstractGraph, g2::AbstractGraph, alg::VF2;
                 vertex_relation::Union{Nothing, Function}=nothing,
                 edge_relation::Union{Nothing, Function}=nothing)::Channel{Vector{Tuple{eltype(g1),eltype(g2)}}}
    T = Vector{Tuple{eltype(g1), eltype(g2)}}
    !could_have_isomorph(g1, g2) && return Channel(_ -> return, ctype=T)
    make_callback(c) = vmap -> (put!(c, collect(zip(vmap,1:length(vmap)))), return true)
    ch::Channel{T} = Channel(ctype=T) do c
        vf2(make_callback(c), g1, g2, IsomorphismProblem(),
            vertex_relation=vertex_relation,
            edge_relation=edge_relation)
    end
    return ch
end