
##==============================================================================
## OLD Victory Park Example code, don't delete until replacements are coded
##==============================================================================



# function projNewLandmPoints(vps::Graphs.ExVertex, br::Array{Float64,1}, cov::Array{Float64,2})
#   # TODO -- convert to use Distributions and common projection function
#   Xps = getVal(vps)
#   lmPts = zeros(2,size(Xps,2))
#   for i in 1:size(Xps,2)
#       ent = [cov[1,1]*randn(); cov[2,2]*randn()]
#       init = vec(Xps[1:2,i])+randn(2)
#       lmPts[:,i] = solveLandm(br + ent, vec(Xps[:,i]), init)
#   end
#   return lmPts
# end



# function calcIntersectVols( fgl::G, predLm::BallTreeDensity;
#                             currage=0, maxdeltaage=Inf) where G <: AbstractDFG
#     # TODO upgrade to using MMD test
#     # all landmarks of interest
#     ll = ls(fgl, r"l\d")
#     # output result
#     rr = Dict{String, RemoteRef}()
#     fetchlist = String[]
#     iv = Dict{String, Float64}()
#     for l in ll
#       pvlm = getVariable(fgl,l)
#       # TODO -- can be improved via query in DB case
#       if currage - pvlm.attributes["maxage"] < maxdeltaage
#         p = getBelief(fgl, l)
#         rr[l] = remotecall(uppA(), intersIntgAppxIS, p,predLm)
#         push!(fetchlist, l)
#       else
#         println("calcIntersectVols -- ignoring $(l) because maxdeltaage exceeded")
#         iv[l] = 0
#       end
#     end
#     max = 0
#     maxl = String("")
#     for l in fetchlist #ll
#       # p = getBelief(fgl, l)
#       # tv = intersIntgAppxIS(p,predLm)
#       # iv[l] = tv
#       tv = fetch(rr[l])
#       iv[l] = tv
#       if max < tv  max = tv; maxl = l; end
#     end
#     return iv, maxl
# end

# function maxIvWithoutID(ivs::Dict{String, Float64}, l::T) where {T <: AbstractString}
#   max = 0
#   maxl = String("")
#   for i in ivs
#     if max < i[2] && i[1] != l;  max = i[2]; maxl = i[1]; end
#   end
#   return maxl
# end

# # binary tests to distinguish how to automatically add a landmark to the existing factor graph
# function doAutoEvalTests(fgl::G, ivs::Dict{T, Float64}, maxl::T, lmid::Int, lmindx::Int) where {G <: AbstractDFG, T <: AbstractString}
#   maxAnyval = maxl != String("") ? ivs[maxl] : 0.0
#   # maxid = fgl.IDs[maxl]
#   lmidSugg = lmid != -1 # a landmark ID has been suggested
#   maxAnyExists = maxAnyval > 0.03 # there is notable intersection with previous landm
#   lmIDExists = haskey(fgl.v, lmid) # suggested lmid already in fgl
#   newlmindx = lmindx
#   if lmIDExists
#     lmSuggLbl = String(getVert(fgl,lmid).label) # TODO -- wasteful
#   else
#     newlmindx = lmindx + 1
#     lmSuggLbl = String(string('l',newlmindx))
#   end
#   maxl2 = lmIDExists ? maxIvWithoutID(ivs, lmSuggLbl) : String("")
#   maxl2Exists = lmIDExists ? (maxl2 != "" ? ivs[maxl2] > 0.03 : false) : false # there is notable intersection with previous landm
#   intgLmIDExists = lmIDExists ? ivs[lmSuggLbl] > 0.03 : false

#   return lmidSugg, maxAnyExists, maxl2Exists, maxl2, lmIDExists, intgLmIDExists, lmSuggLbl, newlmindx
# end


# function evalAutoCases!(fgl::G, lmid::Int, ivs::Dict{T, Float64}, maxl::T,
#                         pose::T, lmPts::Array{Float64,2}, br::Array{Float64,1}, cov::Array{Float64,2}, lmindx::Int;
#                         N::Int=100, solvable::Int=1 ) where {G <: AbstractDFG, T <: AbstractString}
#   lmidSugg, maxAnyExists, maxl2Exists, maxl2, lmIDExists, intgLmIDExists, lmSuggLbl, newlmindx = doAutoEvalTests(fgl,ivs,maxl,lmid, lmindx)

#   println("evalAutoCases -- found=$(lmidSugg), $(maxAnyExists), $(maxl2Exists), $(lmIDExists), $(intgLmIDExists)")

#   vlm = Union{}; fbr = Union{};
#   if (!lmidSugg && !maxAnyExists)
#     #new landmark and UniBR constraint
#     v,L,lm = getLastLandm2D(fgl)
#     vlm = newLandm!(fgl, lm, lmPts, cov, N=N,solvable=solvable)
#     fbr = addBRFG!(fgl, pose, lm, br, cov, solvable=solvable)
#   elseif !lmidSugg && maxAnyExists
#     # add UniBR to best match maxl
#     vlm = getVariable(fgl,maxl)
#     fbr = addBRFG!(fgl, pose, maxl, br, cov, solvable=solvable)
#   elseif lmidSugg && !maxl2Exists && !lmIDExists
#     #add new landmark and add UniBR to suggested lmid
#     vlm = newLandm!(fgl, lmSuggLbl, lmPts, cov, N=N, solvable=solvable)
#     fbr = addBRFG!(fgl, pose, lmSuggLbl, br, cov, solvable=solvable)
#   elseif lmidSugg && !maxl2Exists && lmIDExists && intgLmIDExists
#     # doesn't self intesect with existing lmid, add UniBR to lmid
#     vlm = getVariable(fgl, lmid)
#     fbr = addBRFG!(fgl, pose, lmSuggLbl, br, cov, solvable=solvable)
#   elseif lmidSugg && maxl2Exists && !lmIDExists
#     # add new landmark and add MMBR to both maxl and lmid
#     vlm = newLandm!(fgl, lmSuggLbl, lmPts, cov, N=N, solvable=solvable)
#     addMMBRFG!(fgl, pose, [maxl2;lmSuggLbl], br, cov, solvable=solvable)
#   elseif lmidSugg && maxl2Exists && lmIDExists && intgLmIDExists
#     # obvious case, add MMBR to both maxl and lmid. Double intersect might be the same thing
#     println("evalAutoCases! -- obvious case is happening")
#     addMMBRFG!(fgl, pose, [maxl2;lmSuggLbl], br, cov, solvable=solvable)
#     vlm = getVariable(fgl,lmSuggLbl)
#   elseif lmidSugg && maxl2Exists && lmIDExists && !intgLmIDExists
#     # odd case, does not intersect with suggestion, but does with some previous landm
#     # add MMBR
#     @warn "evalAutoCases! -- no self intersect with suggested $(lmSuggLbl) detected"
#     addMMBRFG!(fgl, pose, [maxl;lmSuggLbl], br, cov, solvable=solvable)
#     vlm = getVariable(fgl,lmSuggLbl)
#   elseif lmidSugg && !maxl2Exists && lmIDExists && !intgLmIDExists
#   #   # landm exists but no intersection with existing or suggested lmid
#   #   # may suggest some error
#     @warn "evalAutoCases! -- no intersect with suggested $(lmSuggLbl) or map detected, adding  new landmark MM constraint incase"
#     v,L,lm = getLastLandm2D(fgl)
#     vlm = newLandm!(fgl, lm, lmPts, cov, N=N, solvable=solvable)
#     addMMBRFG!(fgl, pose, [lm; lmSuggLbl], br, cov, solvable=solvable)
#   else
#     error("evalAutoCases! -- unknown case encountered, can reduce to this error to a warning and ignore user request")
#   end

#   return vlm, fbr, newlmindx
# end

# function addAutoLandmBR!(fgl::G,
#                          pose::T,
#                          lmid::Int,
#                          br::Array{Float64,1},
#                          cov::Array{Float64,2},
#                          lmindx::Int;
#                          N::Int=100,
#                          solvable::Int=1  ) where {G <: AbstractDFG, T <: AbstractString}
#   #
#   vps = getVariable(fgl, pose)
#   lmPts = projNewLandmPoints(vps, br, cov)
#   lmkde = kde!(lmPts)
#   currage = parse(Int, pose[2:end])
#   ivs, maxl = calcIntersectVols(fgl, lmkde, currage=currage,maxdeltaage=10)

#   # There are 8 cases of interest
#   vlm, fbr, newlmindx = evalAutoCases!(fgl, lmid, ivs, maxl,pose,lmPts, br,cov,lmindx,N=N,solvable=solvable)

#   return vlm, fbr, newlmindx
# end


# function newLandm!(fg::AbstractDFG, lm::T, wPos::Array{Float64,2}, sig::Array{Float64,2};
#                   N::Int=100, solvable::Int=1, labels::Vector{T}=String[]) where {T <: AbstractString}

#     vert=addVariable!(fg, Symbol(lm), Point2, N=N, solvable=solvable, tags=union(["LANDMARK";], labels))
#     # TODO -- need to confirm this function is updating the correct memory location. v should be pointing into graph
#     # vert=addVariable!(fg, Symbol(lm), wPos, sig, N=N, solvable=solvable, tags=labels)

#     vert.attributes["age"] = 0
#     vert.attributes["maxage"] = 0
#     vert.attributes["numposes"] = 0
#     updateFullVert!(fg, vert)

#     println("newLandm! -- added $(lm)")
#     return vert
# end

# function addBRFG!(fg::G,
#                   pose::T,
#                   lm::T,
#                   br::Array{Float64,1},
#                   cov::Array{Float64,2};
#                   solvable::Int=1  ) where {G <: AbstractDFG, T <: AbstractString}
#   #
#   vps = getVert(fg,pose)
#   vlm = getVert(fg,lm)
#   testlbl = vps.label*vlm.label
#   for nei in getOutNeighbors(fg, vlm)
#     if nei.label == testlbl
#       # TODO -- makes function call brittle
#       @warn "We already have $(testlbl), skipping this constraint"
#       return nothing
#     end
#   end
#   @show keys(vlm.attributes)
#   np = vlm.attributes["numposes"]
#   la = vlm.attributes["age"]
#   nage = parse(Int,pose[2:end])
#   vlm.attributes["numposes"] = np+1
#   vlm.attributes["age"] = ((la*np)+nage)/(np+1)
#   vlm.attributes["maxage"] = nage
#   updateFullVert!(fg, vlm)


#   pbr = Pose2Point2BearingRange(Normal(br[1], cov[1,1]), Normal(br[2],  cov[2,2]))  #{Normal, Normal}
#   @show vps, vlm
#   f = addFactor!(fg, [vps;vlm], pbr, solvable=solvable, graphinit=true ) #[vps;vlm],

#   # only used for max likelihood unimodal tests.
#   u, P = pol2cart(br[[2;1]], diag(cov))
#   infor = inv(P^2)
#   # addLandmMeasRemote(vps.index,vlm.index,u,infor) # for iSAM1 remote solution as reference
#   return f
# end

# function addMMBRFG!(fg::G,
#                     syms::Array{Symbol,1}, br::Array{Float64,1},
#                     cov::Array{Float64,2}; w::Vector{Float64}=Float64[0.5;0.5],
#                     solvable::Int=1) where G <: AbstractDFG
#     #
#     # vps = getVert(fg,pose)
#     # vlm1 = getVert(fg,lm[1])
#     # vlm2 = getVert(fg,lm[2])

#     pbr = Pose2Point2BearingRange(Normal(br[1],cov[1,1]),  Normal(br[2],cov[2,2]))
#     syms = Symbol.([pose;lm...])
#     f = addFactor!(fg, syms, pbr, multihypo=[1.0; w...], solvable=solvable, graphinit=true )
#     return f
# end

# function projNewLandm!(fg::G,
#                        pose::T,
#                        lm::T,
#                        br::Array{Float64,1},
#                        cov::Array{Float64,2};
#                        addfactor=true,
#                        N::Int=100,
#                        solvable::Int=1,
#                        labels::Vector{T}=String[]  ) where {G <: AbstractDFG, T <: AbstractString}
#     #
#     vps = getVariable(fg, pose)

#     lmPts = projNewLandmPoints(vps, br, cov)
#     vlm = newLandm!(fg, lm, lmPts, cov, N=N, solvable=solvable, tags=labels) # cov should not be required here
#     if addfactor
#       fbr = addBRFG!(fg, pose, lm, br, cov, solvable=solvable)
#       return vlm, fbr
#     end
#     return vlm
# end




##==============================================================================
## OLD Victory Park Example code, don't delete until replacements are coded
##==============================================================================



