text:
mutable struct RangeAzimuthElevation
  range::Float64
  azimuth::Float64
  elevation::Union{Nothing,Float64}
end

function convert(::Type{Rotations.Quat}, q::TransformUtils.Quaternion)
  Rotations.Quat(q.s, q.v...)
end
function convert(::Type{Rotations.Quat}, x::SO3)
  q = convert(TransformUtils.Quaternion, x)
  convert(Rotations.Quat, q)
end
function convert(::Type{T}, x::SO3) where {T <: CoordinateTransformations.AffineMap}
  LinearMap( convert(Quat, x) )
end

function convert(::Type{T}, x::SE3) where {T <: CoordinateTransformations.AffineMap}
  Translation(x.t...) ∘ convert(AffineMap{Rotations.Quat{Float64}}, x.R)
end
function convert(::Type{SE3}, x::T) where {T <: CoordinateTransformations.AffineMap{Rotations.Quat{Float64}}}
  SE3(x.translation[1:3], TransformUtils.Quaternion(x.linear.w, [x.linear.x,x.linear.y,x.linear.z]) )
end

function convert(::Type{SE3}, t::Tuple{Symbol, Vector{Float64}})
  if t[1]==:XYZqWXYZ
    return SE3(t[2][1:3],TransformUtils.Quaternion(t[2][4],t[2][5:7]))
  else
    error("Unknown conversion type $(t[1])")
  end
end

function convert(::Type{RangeAzimuthElevation}, val::Tuple{Symbol, Vector{Float64}})
  if val[1] == :rangeazimuth
    return RangeAzimuthElevation(val[2][1],val[2][2],nothing)
  elseif val[1] == :rangeazimuthelevation
    return RangeAzimuthElevation(val[2][1],val[2][2],val[2][3])
  else
    error("Unknown conversion from $(val[1]) to RangeAzimuthElevation.")
  end
end

# should be deprecated or indicated more clearly
lsrBR(a) = [a[2,:];a[1,:]]';

function veePose3(s::SE3)
  TransformUtils.veeEuler(s)
end
function veePose(s::SE3)
  TransformUtils.veeEuler(s)
end

function \(s::SE3, wTr::CTs.Translation)
  bTr = s.R.R'*(wTr.v-s.t)
  Dtr = bTr
  range = norm(Dtr)
  azi = atan(Dtr[2], Dtr[1])
  elev = atan(Dtr[3], Dtr[1])
  RangeAzimuthElevation(range, azi, elev)
end

"""
    $SIGNATURES

Method to compare current and predicted estimate on a variable, developed for testing a new factor before adding to the factor graph.

Notes
- `fct` does not have to be in the factor graph -- likely used to test beforehand.
- function is useful for detecting if `multihypo` should be used.
- `approxConv` will project the full belief estimate through some factor but must already be in factor graph.

Example

```julia
# fg already exists containing :x7 and :l3
pp = Pose2Point2BearingRange(Normal(0,0.1),Normal(10,1.0))
# possible new measurement from :x7 to :l3
curr, pred = predictVariableByFactor(fg, :l3, pp, [:x7; :l3])
# example of naive user defined test on fit score
fitscore = minkld(curr, pred)
# `multihypo` can be used as option between existing or new variables
```

Related

approxConv
"""
function predictVariableByFactor(dfg::AbstractDFG,
                                 targetsym::Symbol,
                                 fct::Pose2Point2BearingRange,
                                 prevars::Vector{Symbol}  )
  #
  @assert targetsym in prevars
  curr = getKDE(dfg, targetsym)
  tfg = initfg()
  for var in prevars
    varnode = getVariable(dfg, var)
    addVariable!(tfg, var, getSofttype(varnode))
    if var != targetsym
      @assert isInitialized(varnode)
      manualinit!(tfg,var,getKDE(varnode))
    end
  end
  addFactor!(tfg, prevars, fct, autoinit=false)
  fctsym = ls(tfg, targetsym)

  pts, infd = predictbelief(tfg, targetsym, fctsym)
  pred = manikde!(pts, getSofttype(getVariable(dfg, targetsym)))
  # return current and predicted beliefs
  return curr, pred
end

"""
    $(SIGNATURES)

Calculate the cartesian distance between two vertices in the graph using their symbol name, and by maximum belief point.
"""
function getRangeKDEMax2D(fgl::AbstractDFG, vsym1::Symbol, vsym2::Symbol)
  x1 = getKDEMax(getVertKDE(fgl, vsym1))
  x2 = getKDEMax(getVertKDE(fgl, vsym2))
  norm(x1[1:2]-x2[1:2])
end

function measureMeanDist(fg::AbstractDFG, a::AbstractString, b::AbstractString)
    #bearrang!(residual::Array{Float64,1}, Z::Array{Float64,1}, X::Array{Float64,1}, L::Array{Float64,1})
    res = zeros(2)
    A = getVal(fg,a)
    B = getVal(fg,b)
    Ax = Statistics.mean(vec(A[1,:]))
    Ay = Statistics.mean(vec(A[2,:]))
    Bx = Statistics.mean(vec(B[1,:]))
    By = Statistics.mean(vec(B[2,:]))
    dx = Bx - Ax
    dy = By - Ay
    b = atan(dy,dx)
    r = sqrt(dx^2 + dy^2)
    return r, b
end

function predictBodyBR(fg::AbstractDFG, a::Symbol, b::Symbol)
  res = zeros(2)
  A = getKDEMean(getKDE(getVariable(fg,a)))
  B = getKDEMean(getKDE(getVariable(fg,b)))
  Ax = A[1] # Statistics.mean(vec(A[1,:]))
  Ay = A[2] # Statistics.mean(vec(A[2,:]))
  Ath = getKDEMax(getKDE(getVariable(fg,a)))[3]
  Bx = B[1]
  By = B[2]
  wL = SE2([Bx;By;0.0])
  wBb = SE2([Ax;Ay;Ath])
  bL = se2vee((wBb \ Matrix{Float64}(LinearAlgebra.I, 3,3)) * wL)
  dx = bL[1] - 0.0
  dy = bL[2] - 0.0
  b = (atan(dy,dx))
  r = sqrt(dx^2 + dy^2)
  return b, r
end

"""
    $SIGNATURES

Return the last `number::Int` of poses according to `filterLabel::Regex`.

Notes
- Uses FIFO add history of variables to the distribued factor graph object as search index.
"""
function getLastPoses(dfg::AbstractDFG;
                      filterLabel::Regex=r"x\d",
                      number::Int=5)::Vector{Symbol}
  #
  # filter according to pose label
  syms = filter(l->occursin(filterLabel, string(l)), getAddHistory(dfg))

  # return the last segment of syms
  len = length(syms)
  st = number < len ? len-number+1 : 1
  return syms[st:end]
end

"""
    $SIGNATURES

Set old poses and adjacent factors to `solvable::Int=0` (default).

Notes
- `youngest::Int` and `oldest::Int` set the limits of search by count,
  - `oldest` set large enough for solver loop defintely disengage old parts in re-occuring cycle.
- `filterLabel::Regex` sets the template to search for each pose label.
- Poses are assumed to be a thread through time that connects the local exploration variables.
- Initially developed to remove old variables and factors from a solution, in combination with fix-lag marginalization.
  - `getSolverParams(fg).isfixedlag=true`

Related:

getLastPoses
"""
function setSolvableOldPoses!(dfg::AbstractDFG;
                              youngest::Int=50,
                              oldest::Int=200,
                              solvable=0,
                              filterLabel::Regex=r"x\d")
  #
  # collect old variables and factors to disable from next solve
  newPoses = getLastPoses(dfg,filterLabel=filterLabel, number=youngest)
  oldPoses = setdiff(getLastPoses(dfg,filterLabel=filterLabel, number=oldest), newPoses)
  allFcts = (oldPoses .|> x->ls(dfg,x))
  fctAdj = 0 < length(allFcts) ? union(allFcts...) : Symbol[]

  # all together
  fullList = [oldPoses; fctAdj]

  # use solvable=0 to disable variables and factors in the next solve
  map(x->setSolvable!(dfg, x, solvable), fullList)

  return fullList
end

"""
    $(SIGNATURES)

Create a new variable node and insert odometry constraint factor between
which will automatically increment latest pose symbol x<k+1> for new node new node and
constraint factor are returned as a tuple.
"""
function addOdoFG!(fg::G,
                   n::Symbol,
                   DX::Array{Float64,1},
                   cov::Array{Float64,2};
                   N::Int=0,
                   solvable::Int=1,
                   labels::Vector{<:AbstractString}=String[]  ) where G <: AbstractDFG
    #
    prev, X, nextn = getLastPose2D(fg)
    r,c = size(X)
    if N==0
      N = c
    end
    sig = diag(cov)
    XnextInit = zeros(r,c)
    # increases the number of particles based on the number of modes in the measurement Z
    for i in 1:c
        ent = [randn()*sig[1]; randn()*sig[2]; randn()*sig[3]]
        XnextInit[:,i] = addPose2Pose2(X[:,i], DX + ent)
    end

    v = addVariable!(fg, n, Pose2, N=N, solvable=solvable, labels=[labels;"POSE"])
    # v = addVariable!(fg, n, XnextInit, cov, N=N, solvable=solvable, labels=labels)
    pp = Pose2Pose2(MvNormal(DX, cov)) #[prev;v],
    f = addFactor!(fg, [prev;v], pp, solvable=solvable, autoinit=true )
    infor = inv(cov^2)
    # addOdoRemote(prev.index,v.index,DX,infor) # this is for remote factor graph ref parametric solution -- skipped internally by global flag variable
    return v, f
end

function addOdoFG!(fgl::G,
                   Z::Pose3Pose3;
                   N::Int=0,
                   solvable::Int=1,
                   labels::Vector{<:AbstractString}=String[]  ) where {G <: AbstractDFG}
  #
  vprev, X, nextn = getLastPose(fgl)
  vnext = addVariable!(fgl, nextn, Pose3, solvable=solvable, labels=labels)
  fact = addFactor!(fgl, [vprev;vnext], Z, autoinit=true)

  return vnext, fact

  # error("addOdoFG!( , ::Pose3Pose3, ) not currently usable, there were breaking changes. Work in Progress")
  # addOdoFG(fg, n, DX, cov, N=N, solvable=solvable, labels=labels)
end

"""
    $(SIGNATURES)

Create a new variable node and insert odometry constraint factor between
which will automatically increment latest pose symbol x<k+1> for new node new node and
constraint factor are returned as a tuple.

"""
function addOdoFG!(
        fgl::FactorGraph,
        odo::Pose2Pose2;
        N::Int=0,
        solvable::Int=1,
        labels::Vector{<:AbstractString}=String[] ) # where {PP <: RoME.BetweenPoses}
    #
    vprev, X, nextn = getLastPose(fgl)
    if N==0
      N = size(X,2)
    end
    # vnext = addVariable!(fgl, nextn, X⊕odo, ones(1,1), N=N, solvable=solvable, labels=labels)
    vnext = addVariable!(fgl, nextn, Pose2, N=N, solvable=solvable, labels=labels)
    fact = addFactor!(fgl, [vprev;vnext], odo, autoinit=true)

    return vnext, fact
end

# """
#     $SIGNATURES
#
# Initialize an empty `::FactorGraph` object while initializing `sessionname`, `robotname`, and `cloudgraph`.
# """
# function initfg(;sessionname="NA",robotname="",username="",cloudgraph=nothing)
#   # fgl = RoME.initfg(sessionname=sessionname)
#   fgl = IIF.FactorGraph()
#   fgl.sessionname = sessionname
#   fgl.robotname = robotname
#   fgl.username = username
#   fgl.cg = cloudgraph
#   return fgl
# end
# function initfg(;sessionname="NA")
#   fgl = IIF.FactorGraph()
#   fgl.sessionname=sessionname
#   # fgl.robotname = ?
#   # registerCallback!(fgl, RoME.getSample) # RoME.evalPotention
#   return fgl
# end

"""
    $(SIGNATURES)

Initialize a factor graph object as Pose2, Pose3, or neither and returns variable and factor symbols as array.
"""
function initFactorGraph!(fg::G;
                          P0::Union{Array{Float64,2},Nothing}=nothing,
                          init::Union{Vector{Float64},Nothing}=nothing,
                          N::Int=100,
                          lbl::Symbol=:x0,
                          solvable::Int=1,
                          firstPoseType=Pose2,
                          labels::Vector{Symbol}=Symbol[]) where G <: AbstractDFG
  #
  nodesymbols = Symbol[]
  if firstPoseType == Pose2
      init = init!=nothing ? init : zeros(3)
      P0 = P0!=nothing ? P0 : Matrix(Diagonal([0.03;0.03;0.001]))
      # init = vectoarr2(init)
      addVariable!(fg,lbl,Pose2,N=N,autoinit=true,solvable=solvable,labels=labels )
      push!(nodesymbols, lbl)
      # v1 = addVariable!(fg, lbl, init, P0, N=N, solvable=solvable, labels=labels)
      fctVert = addFactor!(fg, [lbl;], PriorPose2(MvNormal(init, P0)), solvable=solvable, labels=labels) #[v1],
      push!(nodesymbols, Symbol(fctVert.label))
  end
  if firstPoseType == Pose3
      init = init!=nothing ? init : zeros(6)
      P0 = P0!=nothing ? P0 : Matrix(Diagonal([0.03;0.03;0.03;0.001;0.001;0.001]))
      addVariable!(fg,lbl,Pose2,N=N,autoinit=true,solvable=solvable,labels=labels )
      push!(nodesymbols, lbl)
      # v1 = addVariable!(fg, lbl, init, P0, N=N, solvable=solvable, labels=labels)
      fctVert = addFactor!(fg, [lbl;], PriorPose3(MvNormal(init, P0)), solvable=solvable, labels=labels) #[v1],
      push!(nodesymbols, Symbol(fctVert.label))
  end
  return nodesymbols
end

function newLandm!(fg::FactorGraph, 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, labels=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, labels=labels)

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

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

function updateLandmAge(vlm::Graphs.ExVertex, pose::T) where {T <: AbstractString}
  error("still working here")
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, autoinit=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, autoinit=true )
    return f
end

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 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 = getVert(fg, pose)

    lmPts = projNewLandmPoints(vps, br, cov)
    vlm = newLandm!(fg, lm, lmPts, cov, N=N, solvable=solvable, labels=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

function calcIntersectVols(fgl::G, predLm::BallTreeDensity;
                           currage=0, maxdeltaage=Inf) where G <: AbstractDFG
    # TODO upgrade to using MMD test
    # all landmarks of interest
    xx,ll = ls(fgl)
    # output result
    rr = Dict{String, RemoteRef}()
    fetchlist = String[]
    iv = Dict{String, Float64}()
    for l in ll
      pvlm = getVert(fgl,l)
      # TODO -- can be improved via query in DB case
      if currage - pvlm.attributes["maxage"] < maxdeltaage
        p = getVertKDE(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 = getVertKDE(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 = getVert(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 = getVert(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 = getVert(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 = getVert(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 = getVert(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 malahanobisBR(measA, preA, cov::Array{Float64,2})
    # measure landmark with noise
    res = measA - preA
    mala2 = Union{}
    #Malahanobis distance
    if false
      lambda = cov \ Matrix{Float64}(LinearAlgebra.I, 2,2)
      mala2 = res' * lambda * res
    else
      mala2 = res' * (cov \ res)
    end

    mala = sqrt(mala2)
    return mala
end

# ------------------------------------
# Transfered from IncrementalInference

function get2DSamples(fg::G; #::Union{Symbol, S};
                      from::Int=0, to::Int=999999999,
                      minnei::Int=0,
                      regexKey::Regex=r"x") where {G <: AbstractDFG, S <: AbstractString}
  #
  X = Array{Float64,1}()
  Y = Array{Float64,1}()

  # if sym = 'l', ignore single measurement landmarks
  allids = DFG.getVariableIds(fg, regexKey)  # fg.IDs
  saids = DFG.sortVarNested(allids)
  for id in saids
    # vertlbl = string(id[1])
    vertlbl = string(id)
    # if vertlbl[1] == sym
      val = parse(Int,split(vertlbl[2:end],'_')[1])
      if from <= val && val <= to
        # if length( getOutNeighbors(fg, vertlbl[2] , needdata=true ) ) >= minnei
        if length( DFG.getNeighbors(fg, id ) ) >= minnei
          # if length(out_neighbors(fg.v[id[2]],fg.g)) >= minnei
          X=[X; vec(getVal(fg,id)[1,:]) ]
          Y=[Y; vec(getVal(fg,id)[2,:]) ]
        end
      end
    # end
  end
  return X,Y
end

# function getAll2D(fg, sym; minnei::Int=0)
#   @warn "getAll2D deprecated, use get2DSamples instead"
#   return get2DSamples(fg, sym, minnei=minnei )
# end

function get2DSampleMeans(fg::G,
                          regexKey::Regex=r"x";
                          from::Int=0, to::Int=9999999999,
                          minnei::Int=0) where G <: AbstractDFG
  #
  X = Array{Float64,1}()
  Y = Array{Float64,1}()
  Th = Array{Float64,1}()
  LB = String[]

  # if sym = 'l', ignore single measurement landmarks
  allids = DFG.getVariableIds(fg, regexKey)  # fg.IDs
  saids = DFG.sortVarNested(allids)
  mask = Array{Bool,1}(undef, length(saids))
  fill!(mask, false)
  count = 0
  for id in saids
    count += 1
    if length( DFG.getNeighbors(fg, id) ) >= minnei
      mask[count] = true
    end
    if from != 0 || to != 9999999999
      vertlbl = string(id)
        # TODO won't work with nested labels
        val = parse(Int,split(vertlbl[2:end],'_')[1])
        if !(from <= val && val <= to)
          mask[count] = false
        end
    end
  end
  # allIDs = sort(allIDs)

  for id in saids[mask]
    X=[X; Statistics.mean( vec( getVal(fg, id )[1,:] ) )]
    Y=[Y; Statistics.mean( vec( getVal(fg, id )[2,:] ) )]
    # crude test for pose TODO probably not going to always work right
    if string(id)[1] == 'x'
      Th=[Th; Statistics.mean( vec( getVal(fg, id )[3,:] ) )]
    end
    push!(LB, string(id))
  end
  return X,Y,Th,LB
end

#draw landmark positions
function getAll2DMeans(fg, sym::Regex)
  return get2DSampleMeans(fg, sym )
end

function getAll2DPoses(fg::G; regexKey=r"x") where G <: AbstractDFG
    return getAll2DSamples(fg, regexKey=regexKey )
end

function get2DPoseSamples(fg::G; from::Int=0, to::Int=999999999, regexKey=r"x") where G <: AbstractDFG
  return get2DSamples(fg, regexKey=regexKey, from=from, to=to )
end

function get2DPoseMeans(fg::G; from::Int=0, to::Int=999999999, regexKey=r"x") where G <: AbstractDFG
  return get2DSampleMeans(fg, regexKey, from=from, to=to )
end

function get2DPoseMax(fgl::G;
					  regexKey::Regex=r"x",
                      from::Int=-99999999999, to::Int=9999999999 ) where G <: AbstractDFG
  #
  # xLB,ll = ls(fgl) # TODO add: from, to, special option 'x'
  xLB = DFG.getVariableIds(fgl, regexKey)
  saids = DFG.sortVarNested(xLB)
  X = Array{Float64,1}()
  Y = Array{Float64,1}()
  Th = Array{Float64,1}()
  LB = String[]
  for slbl in saids
    lbl = string(slbl)
    if from <= parse(Int,split(lbl[2:end],'_')[1]) <=to
      mv = getKDEMax(getVertKDE(fgl,slbl))
      push!(X,mv[1])
      push!(Y,mv[2])
      push!(Th,mv[3])
      push!(LB, string(lbl))
    end
  end
  return X, Y, Th, LB
end

function get2DLandmSamples(fg::G;
                           from::Int=0,
                           to::Int=999999999,
                           minnei::Int=0 ) where G <: AbstractDFG
  #
  return get2DSamples(fg, regexKey=r"l", from=from, to=to, minnei=minnei )
end

function get2DLandmMeans(fg::G;
                         from::Int=0, to::Int=999999999,
                         minnei::Int=0,
                         landmarkRegex::Regex=r"l" ) where G <: AbstractDFG
  #
  return get2DSampleMeans(fg, landmarkRegex, from=from, to=to, minnei=minnei )
end

function removeKeysFromArr(fgl::G,
                           torm::Array{Int,1},
                           lbl::Array{String,1}) where G <: AbstractDFG
  #
  retlbs = String[]
  for i in 1:length(lbl)
    id = parse(Int,split(lbl[i][2:end],'_')[1])
    if something(findfirst(isequal(id), torm), 0) == 0 #findfirst(torm,id) == 0
      push!(retlbs, lbl[i])
    else
      println("removeKeysFromArr -- skipping $(lbl[i]), id=$(id)")
    end
  end
  return retlbs
end
function removeKeysFromArr(fgl::G,
                           torm::Array{Int,1},
                           lbl::Array{Symbol,1} ) where G
  #
  removeKeysFromArr(fgl, torm, string.(lbl))
end

function get2DLandmMax(fgl::G;
                       from::Int=-99999999999,
                       to::Int=9999999999,
                       showmm=false, MM::Dict{Int,T}=Dict{Int,Int}(),
                       regexLandmark::Regex=r"l"  ) where {G <: AbstractDFG, T}
  #
  # xLB,lLB = ls(fgl) # TODO add: from, to, special option 'x'
  lLB = DFG.getVariableIds(fgl, regexLandmark)
  if !showmm lLB = removeKeysFromArr(fgl, collect(keys(MM)), lLB); end
  X = Array{Float64,1}()
  Y = Array{Float64,1}()
  Th = Array{Float64,1}()
  LB = String[]
  for lb in lLB
    @show lb
    lbl = string(lb)
    if from <= parse(Int,split(lbl[2:end],'_')[1]) <=to
      mv = getKDEMax(getVertKDE(fgl, Symbol(lb)))
      push!(X,mv[1])
      push!(Y,mv[2])
      push!(LB, string(lbl))
    end
  end
  return X, Y, Th, LB
end

# convenience function to add DIDSON sonar constraints to graph
function addLinearArrayConstraint(fgl::G,
                                  rangebearing::Union{Tuple{Float64, Float64}, Vector{Float64}},
                                  pose::Symbol,
                                  landm::Symbol ;
                                  rangecov::Float64=3e-4,
                                  bearingcov::Float64=3e-4 ) where G <: AbstractDFG

  #

  cl = LinearRangeBearingElevation((rangebearing[1],rangecov),(rangebearing[2],bearingcov))
  if !haskey(fgl.IDs, landm)
    pts = getVal(fgl, pose) + cl
    N = size(pts,2)
    vl1 = addVariable!(fgl, landm,  pts,  N=N)
    println("Automatically added $(landm) to the factor graph")
  end
  addFactor!(fgl, [getVert(fgl, pose); getVert(fgl, landm)], cl)
  nothing
end

function addSoftEqualityPoint2D(fgl::G,
                                l1::Symbol,
                                l2::Symbol;
                                dist=MvNormal([0.0;0.0],Matrix{Float64}(LinearAlgebra.I, 2,2)),
                                solvable::Int=1  )  where G <: AbstractDFG
  #
  pp = Point2DPoint2D(dist)
  addFactor!(fgl, [l1,l2], pp, solvable=solvable)
end


text:
# Test generalised natural splines (which are the "standard" natural cubic
# splines when k = 4).

using BSplineKit
using Random
using Test

import LinearAlgebra

function test_natural(ord::BSplineOrder)
    B = BSplineBasis(ord, -1:0.1:1)
    k = order(B)
    if isodd(k)
        # `Natural` boundary condition only supported for even-order splines (got k = $k)
        @test_throws ArgumentError RecombinedBSplineBasis(Natural(), B)
        return
    end

    rng = MersenneTwister(42)

    R = @inferred RecombinedBSplineBasis(Natural(), B)
    S = Spline(R, 1 .+ rand(rng, length(R)))  # coefficients in [1, 2]

    M = recombination_matrix(R)
    if k == 2
        # In this case, basis functions are not really recombined, and the
        # resulting basis is identical to the original one.
        @test M == LinearAlgebra.I
    end

    for x ∈ boundaries(R)
        Sx = @inferred S(x)
        @test Sx > 1
        ϵ = 1e-8 * Sx  # threshold
        @test abs((Derivative(1) * S)(x)) > ϵ  # different from zero
        for n = 2:(k ÷ 2)
            Sder = Derivative(n) * S
            @test abs(Sder(x)) < ϵ  # approximately zero
        end
    end

    nothing
end

# Note that the case k = 2 is also supported, even if it's not really useful
# (in this case the recombination is equivalent to the original B-spline basis).
@testset "Natural splines" begin
    @testset "k = $k" for k ∈ (2, 4, 5, 6, 8)
        test_natural(BSplineOrder(k))
    end
end

ext: jl

max_stars_repo_name: jipolanco/BasisSplines

max_issues_repo_name: jipolanco/BasisSplines

max_forks_repo_name: jipolanco/BasisSplines


text:
module timeevolution_schroedinger

export schroedinger, schroedinger_timedependent

using ...bases
using ...operators
using ...states
using ...ode_dopri

"""
Integrate Schroedinger equation with given derivative function.
"""
function integrate_schroedinger{T<:StateVector}(dschroedinger::Function, tspan, psi0::T; fout=nothing, kwargs...)
    as_statevector(x::Vector{Complex128}) = T(psi0.basis, x)
    as_vector(psi::T) = psi.data
    if fout==nothing
        tout = Float64[]
        xout = T[]
        function fout_(t, psi::T)
            push!(tout, t)
            push!(xout, deepcopy(psi))
        end
        f = fout_
    else
        f = fout
    end
    f_(t, x::Vector{Complex128}) = f(t, as_statevector(x))
    dschroedinger_(t, x::Vector{Complex128}, dx::Vector{Complex128}) = dschroedinger(t, as_statevector(x), as_statevector(dx))
    ode(dschroedinger_, float(tspan), as_vector(psi0), f_; kwargs...)
    return fout==nothing ? (tout, xout) : nothing
end

function dschroedinger(psi::Ket, H::Operator, dpsi::Ket)
    operators.gemv!(complex(0,-1.), H, psi, complex(0.), dpsi)
    return dpsi
end

function dschroedinger(psi::Bra, H::Operator, dpsi::Bra)
    operators.gemv!(complex(0,1.), psi, H, complex(0.), dpsi)
    return dpsi
end

function dschroedinger_timedependent{T<:StateVector}(t::Float64, psi0::T, f::Function, dpsi::T)
    H = f(t, psi0)
    _check_input(psi0, H)
    dschroedinger(psi0, H, dpsi)
end

function _check_input(psi::Ket, H::Operator)
    @assert psi.basis==H.basis_l
    @assert multiplicable(H.basis_r, psi.basis)
end

function _check_input(psi::Bra, H::Operator)
    @assert psi.basis==H.basis_r
    @assert multiplicable(psi.basis, H.basis_l)
end

"""
Integrate Schroedinger equation.

Arguments
---------
tspan
    Vector specifying the points of time for which output should be displayed.
psi0
    Initial state vector (can be a bra or a ket).
H
    Operator specifying the Hamiltonian.

Keyword Arguments
-----------------

fout (optional)
    If given this function fout(t, psi) is called every time an output should
    be displayed. To limit copying to a minimum the given state psi
    is further used and therefore must not be changed.
"""
function schroedinger{T<:StateVector}(tspan, psi0::T, H::Operator;
                fout::Union{Function,Void}=nothing,
                kwargs...)
    _check_input(psi0, H)
    dschroedinger_(t, psi::T, dpsi::T) = dschroedinger(psi, H, dpsi)
    integrate_schroedinger(dschroedinger_, tspan, psi0; fout=fout, kwargs...)
end

"""
Integrate time-dependent Schroedinger equation.

Arguments
---------
tspan
    Vector specifying the points of time for which output should be displayed.
psi0
    Initial state vector (can be a bra or a ket).
f
    Function f(t, psi) -> H returning the time and or state dependent Hamiltonian.

Keyword Arguments
-----------------

fout (optional)
    If given this function fout(t, psi) is called every time an output should
    be displayed. To limit copying to a minimum the given state psi
    is further used and therefore must not be changed.
"""
function schroedinger_timedependent{T<:StateVector}(tspan, psi0::T, f::Function;
                fout::Union{Function,Void}=nothing,
                kwargs...)
    dschroedinger_(t, psi::T, dpsi::T) = dschroedinger_timedependent(t, psi, f, dpsi)
    integrate_schroedinger(dschroedinger_, tspan, psi0; fout=fout, kwargs...)
end

end


text:
#Maximum Likelihood Estimation for 16 dimensional systems

using Base.Iterators, LinearAlgebra, Optim, Plotly;
import DelimitedFiles.readdlm;

#Here is the tensor product definition
#CircleTimes = kron;
tp(m1, m2) = partition(flatten(m1 .* m2),2);

n = Int64(16);
#Define the single qubit measurements, R is the state with "-i".
H = Float64[1, 0];
V = Float64[0, 1];
Dia = Float64[1/sqrt(2), 1/sqrt(2)];
A = Float64[1/sqrt(2), -1/sqrt(2)];
R = ComplexF64[1/sqrt(2), -im/sqrt(2)];
L = ComplexF64[1/sqrt(2), im/sqrt(2)];
statevars = [H, V, Dia, A, R, L];

HadamardMatrix_2 = Float64[1/sqrt(2) 1/sqrt(2); 1/sqrt(2) -1/sqrt(2)];
PauliMatrix_1 = Float64[0 1; 1 0];

Trafo = kron(HadamardMatrix_2, Matrix{Float64}(I, 2,2), Matrix{Float64}(I, 2,2), PauliMatrix_1 * HadamardMatrix_2);
NDimensions = 16;

#Here are the specific states and data measured in the tomography experiment
D2Array = readdlm("D:\\Binto\\ddd\\Julia\\Tomography_Mathematica_to_Julia\\measurement_bases_full_edit_measurement_7.txt", skipstart=2)[1:end,2:17];
Data = collect(flatten(D2Array));
SigmaData = sqrt.(Data);

function BadnessPolynomial!(t)
    pM = [t[1] 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
        t[17] + im*t[18] t[2] 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
        t[47] + im*t[48] t[19] + im*t[20] t[3] 0 0 0 0 0 0 0 0 0 0 0 0 0;
        t[75] + im*t[76] t[49] + im*t[50] t[21] + im*t[22] t[4] 0 0 0 0 0 0 0 0 0 0 0 0;
        t[101] + im*t[102] t[77] + im*t[78] t[51] + im*t[52] t[23] + im*t[24] t[5] 0 0 0 0 0 0 0 0 0 0 0;
        t[125] + im*t[126] t[103] + im*t[104] t[79] + im*t[80] t[53] + im*t[54] t[25] + im*t[26] t[6] 0 0 0 0 0 0 0 0 0 0;
        t[147] + im*t[148] t[127] + im*t[128] t[105] + im*t[106] t[81] + im*t[82] t[55] + im*t[56] t[27] + im*t[28] t[7] 0 0 0 0 0 0 0 0 0;
        t[167] + im*t[168] t[149] + im*t[150] t[129] + im*t[130] t[107] + im*t[108] t[83] + im*t[84] t[57] + im*t[58] t[29] + im*t[30] t[8] 0 0 0 0 0 0 0 0;
        t[185] + im*t[186] t[169] + im*t[170] t[151] + im*t[152] t[131] + im*t[132] t[109] + im*t[110] t[85] + im*t[86] t[59] + im*t[60] t[31] + im*t[32] t[9] 0 0 0 0 0 0 0;
        t[201] + im*t[202] t[187] + im*t[188] t[171] + im*t[172] t[153] + im*t[154] t[133] + im*t[134] t[111] + im*t[112] t[87] + im*t[88] t[61] + im*t[62] t[33] + im*t[34] t[10] 0 0 0 0 0 0;
        t[215] + im*t[216] t[203] + im*t[204] t[189] + im*t[190] t[173] + im*t[174] t[155] + im*t[156] t[135] + im*t[136] t[113] + im*t[114] t[89] + im*t[90] t[63] + im*t[64] t[35] + im*t[36] t[11] 0 0 0 0 0;
        t[227] + im*t[228] t[217] + im*t[218] t[205] + im*t[206] t[191] + im*t[192] t[175] + im*t[176] t[157] + im*t[158] t[137] + im*t[138] t[115] + im*t[116] t[91] + im*t[92] t[65] + im*t[66] t[37] + im*t[38] t[12] 0 0 0 0;
        t[237] + im*t[238] t[229] + im*t[230] t[219] + im*t[220] t[207] + im*t[208] t[193] + im*t[194] t[177] + im*t[178] t[159] + im*t[160] t[139] + im*t[140] t[117] + im*t[118] t[93] + im*t[94] t[67] + im*t[68] t[39] + im*t[40] t[13] 0 0 0;
        t[245] + im*t[246] t[239] + im*t[240] t[231] + im*t[232] t[221] + im*t[222] t[209] + im*t[210] t[195] + im*t[196] t[179] + im*t[180] t[161] + im*t[162] t[141] + im*t[142] t[119] + im*t[120] t[95] + im*t[96] t[69] + im*t[70] t[41] + im*t[42] t[14] 0 0;
        t[251] + im*t[252] t[247] + im*t[248] t[241] + im*t[242] t[233] + im*t[234] t[223] + im*t[224] t[211] + im*t[212] t[197] + im*t[198] t[181] + im*t[182] t[163] + im*t[164] t[143] + im*t[144] t[121] + im*t[122] t[97] + im*t[98] t[71] + im*t[72] t[43] + im*t[44] t[15] 0;
        t[255] + im*t[256] t[253] + im*t[254] t[249] + im*t[250] t[243] + im*t[244] t[235] + im*t[236] t[225] + im*t[226] t[213] + im*t[214] t[199] + im*t[200] t[183] + im*t[184] t[165] + im*t[166] t[145] + im*t[146] t[123] + im*t[124] t[99] + im*t[100] t[73] + im*t[74] t[45] + im*t[46] t[16]];

    pMd = [t[1] t[17] - im*t[18] t[47] - im*t[48] t[75] - im*t[76] t[101] - im*t[102] t[125] - im*t[126] t[147] - im*t[148] t[167] - im*t[168] t[185] - im*t[186] t[201] - im*t[202] t[215] - im*t[216] t[227] - im*t[228] t[237] - im*t[238] t[245] - im*t[246] t[251] - im*t[252] t[255] - im*t[256];
        0 t[2] t[19] - im*t[20] t[49] - im*t[50] t[77] - im*t[78] t[103] - im*t[104] t[127] - im*t[128] t[149] - im*t[150] t[169] - im*t[170] t[187] - im*t[188] t[203] - im*t[204] t[217] - im*t[218] t[229] - im*t[230] t[239] - im*t[240] t[247] - im*t[248] t[253] - im*t[254];
        0 0 t[3] t[21] - im*t[22] t[51] - im*t[52] t[79] - im*t[80] t[105] - im*t[106] t[129] - im*t[130] t[151] - im*t[152] t[171] - im*t[172] t[189] - im*t[190] t[205] - im*t[206] t[219] - im*t[220] t[231] - im*t[232] t[241] - im*t[242] t[249] - im*t[250];
        0 0 0 t[4] t[23] - im*t[24] t[53] - im*t[54] t[81] - im*t[82] t[107] - im*t[108] t[131] - im*t[132] t[153] - im*t[154] t[173] - im*t[174] t[191] - im*t[192] t[207] - im*t[208] t[221] - im*t[222] t[233] - im*t[234] t[243] - im*t[244];
        0 0 0 0 t[5] t[25] - im*t[26] t[55] - im*t[56] t[83] - im*t[84] t[109] - im*t[110] t[133] - im*t[134] t[155] - im*t[156] t[175] - im*t[176] t[193] - im*t[194] t[209] - im*t[210] t[223] - im*t[224] t[235] - im*t[236];
        0 0 0 0 0 t[6] t[27] - im*t[28] t[57] - im*t[58] t[85] - im*t[86] t[111] - im*t[112] t[135] - im*t[136] t[157] - im*t[158] t[177] - im*t[178] t[195] - im*t[196] t[211] - im*t[212] t[225] - im*t[226];
        0 0 0 0 0 0 t[7] t[29] - im*t[30] t[59] - im*t[60] t[87] - im*t[88] t[113] - im*t[114] t[137] - im*t[138] t[159] - im*t[160] t[179] - im*t[180] t[197] - im*t[198] t[213] - im*t[214];
        0 0 0 0 0 0 0 t[8] t[31] - im*t[32] t[61] - im*t[62] t[89] - im*t[90] t[115] - im*t[116] t[139] - im*t[140] t[161] - im*t[162] t[181] - im*t[182] t[199] - im*t[200];
        0 0 0 0 0 0 0 0 t[9] t[33] - im*t[34] t[63] - im*t[64] t[91] - im*t[92] t[117] - im*t[118] t[141] - im*t[142] t[163] - im*t[164] t[183] - im*t[184];
        0 0 0 0 0 0 0 0 0 t[10] t[35] - im*t[36] t[65] - im*t[66] t[93] - im*t[94] t[119] - im*t[120] t[143] - im*t[144] t[165] - im*t[166];
        0 0 0 0 0 0 0 0 0 0 t[11] t[37] - im*t[38] t[67] - im*t[68] t[95] - im*t[96] t[121] - im*t[122] t[145] - im*t[146];
        0 0 0 0 0 0 0 0 0 0 0 t[12] t[39] - im*t[40] t[69] - im*t[70] t[97] - im*t[98] t[123] - im*t[124];
        0 0 0 0 0 0 0 0 0 0 0 0 t[13] t[41] - im*t[42] t[71] - im*t[72] t[99] - im*t[100];
        0 0 0 0 0 0 0 0 0 0 0 0 0 t[14] t[43] - im*t[44] t[73] - im*t[74];
        0 0 0 0 0 0 0 0 0 0 0 0 0 0 t[15] t[45] - im*t[46];
        0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 t[16]];

    global GeneralDM = pMd * pM;
    Prediction(DM, State) = sum(conj(State) .* DM .* State);

    StateMeasured = [kron(i,j,k,l) for i in statevars, j in statevars, k in statevars, l in statevars];

    PredictionPart = [];
    for i = 1:length(StateMeasured)
        push!(PredictionPart, Prediction(GeneralDM, StateMeasured[i]));
    end
    BD = sum((PredictionPart[i] - Data[i])^2 /PredictionPart[i] for i=1:length(StateMeasured));
    return real(BD)
end

lower = fill(-5.0, 256);
upper = fill(5.0, 256);
t0 = fill(0.05, 256); #initial points

#took  92secs for 20000 iterations! reached max iterations!
@time res = optimize(BadnessPolynomial!, t0, NelderMead(), Optim.Options(iterations = 10000))
#took 77secs for 20000 iterations! reached max iterations!
#@time res = optimize(BadnessPolynomial!, t0, SimulatedAnnealing(), Optim.Options(iterations = 20000))
#took 10mins for 20000 iterations! objective increased between iterations!
#@time res = optimize(BadnessPolynomial!, t0, LBFGS(), Optim.Options(iterations = 20000))
#took 15mins! solution in 6 iterations!
#inner_optimizer = GradientDescent()
#@time res = optimize(BadnessPolynomial!, lower, upper, t0, Fminbox(inner_optimizer), Optim.Options(iterations = 10)) #Box constrained minimization
#temp = Optim.minimizer(res);

MaxLikDM = GeneralDM/tr(GeneralDM);
n1 = 0;
n2 = 6;
n3 = 4;
n4 = 4;

IdealDM = [0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 1/2 0.0 0.0 -(1/2) 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 -(1/2) 0.0 0.0 1/2 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0];

IdealDMIm = [0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
    0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0];

Fidelity = tr(MaxLikDM.*IdealDM);
Fidelity2 = tr((MaxLikDM^1/2 .* IdealDM .* MaxLikDM^1/2)^1/2)^2;
Purity = tr(MaxLikDM.*MaxLikDM);
LinearEntropy = n/(n - 1)*(1 - Purity);

#=function plotme3d(dta)
    x = [];
    y = [];
    re_z = [];
    im_z = [];
    for i = 1:size(dta)[1]
        for j = 1:size(dta)[1]
            push!(x, i);
            push!(y, j);
            push!(re_z, real(dta[i,j]));
            push!(im_z, imag(dta[i,j]));
        end
    end
    p1 = plot([scatter3d(x = x, y = y, z = re_z, mode = "markers", marker = attr(size = 5), name = "Real part")])
    p2 = plot([scatter3d(x = x, y = y, z = im_z, mode = "markers", marker = attr(size = 5), name = "Imaginary part")])
    [p1, p2]
end =#

function plotme3d(dta)
    x = [];
    y = [];
    re_z = [];
    im_z = [];
    for i = 1:size(dta)[1]
        for j = 1:size(dta)[1]
            push!(x, i);
            push!(y, j);
            push!(re_z, real(dta[i,j]));
            push!(im_z, imag(dta[i,j]));
        end
    end

    trace1 = scatter3d(x = x, y = y, z = re_z, mode = "markers", marker = attr(size = 5), name = "Real part");
    trace2 = scatter3d(x = x, y = y, z = im_z, mode = "markers", marker = attr(size = 5), name = "Imaginary part");
    layout = Layout(xaxis = attr(tickvals=[1,7,10,16], ticktext = ["HHHH", "HVVH", "VHHV", "VVVV"]),
            yaxis = attr(tickvals=[1,7,10,16], ticktext = ["HHHH", "HVVH", "VHHV", "VVVV"]));
    #plot([trace1, trace2])
    p1 = plot(trace1, layout);
    p2 = plot(trace2, layout);
    [p1, p2]
end

plotme3d(MaxLikDM)
plotme3d(IdealDM)


text:
type EMSEngine{T}
    eventlog::DataFrame
    eventqueue::PriorityQueue{T,Int,Base.Order.ForwardOrdering}
end

function EMSEngine(problem::DispatchProblem)
    ncalls = nrow(problem.emergency_calls)
    eventlog = DataFrame(
        id = 1:ncalls,
        dispatch_from = zeros(Int, ncalls),
        waittime = fill(0.0, ncalls),
        responsetime = fill(Inf, ncalls),
        scenetime = fill(Inf, ncalls),
        conveytime = fill(Inf, ncalls),
        returntime = fill(Inf, ncalls),
        return_to = zeros(Int, ncalls),
        return_type = fill(:station, ncalls),
        hospital = zeros(Int, ncalls),
        ambulance = zeros(Int, ncalls)
    )
    eventqueue = PriorityQueue{Tuple{Symbol,Int,Int,Int},Int,Base.Order.ForwardOrdering}()
    for i in 1:nrow(problem.emergency_calls)
        t = problem.emergency_calls[i, :arrival_seconds]
        enqueue!(eventqueue, (:call, i, t, problem.emergency_calls[i, :neighborhood]), t)
    end
    EMSEngine{Tuple{Symbol,Int,Int,Int}}(eventlog, eventqueue)
end

function call_event!(
        ems::EMSEngine,
        problem::DispatchProblem,
        dispatch::DispatchModel,
        redeploy::RedeployModel,
        id::Int, # the id of the emergency call
        t::Int, # the time of the emergency call
        nbhd::Int; # the neighborhood the call is from
        verbose::Bool = false
    )
    if sum(problem.deployment[problem.coverage[nbhd,:]]) == 0
        @assert false "$id: no ambulance reachable for call at $nbhd"
    elseif sum(problem.available[problem.coverage[nbhd,:]]) > 0
        i = available_for(dispatch, id, problem)
        @assert i > 0 "$id: dispatch from $i to nbhd $nbhd" # assume valid i (enforced by <if> condition)
        update_ambulances!(dispatch, i, -1)
        ems.eventlog[id, :dispatch_from] = i
        @assert problem.available[i] > 0
        problem.available[i] -= 1

        travel_time = ceil(Int, 60*2*problem.emergency_calls[id, Symbol("stn$(i)_min")])
        @assert travel_time >= 0
        ems.eventlog[id, :responsetime] = travel_time / 60 # minutes

        amb = respond_to!(redeploy, i, t)
        ems.eventlog[id, :ambulance] = amb
        enqueue!(ems.eventqueue, (:arrive, id, t + travel_time, amb), t + travel_time)
    else
        println(id, ": call from ", nbhd, " queued behind ", problem.wait_queue[nbhd])
        push!(problem.wait_queue[nbhd], id) # queue the emergency call
    end
end

function arrive_event!(
        ems::EMSEngine,
        problem::DispatchProblem,
        redeploy::RedeployModel,
        id::Int, # the id of the emergency call
        t::Int, # the time of the emergency call
        amb::Int
    )
    arriveatscene!(redeploy, amb, t)
    # time the ambulance spends at the scene
    scene_time = ceil(Int,60*0.4*rand(problem.turnaround)) # 60sec*0.4*mean(40minutes) ~ 15minutes
    ems.eventlog[id, :scenetime] = scene_time / 60 # minutes
    @assert scene_time > 0
    enqueue!(ems.eventqueue, (:convey, id, t + scene_time, amb), t + scene_time)
end

"determine the hospital to convey the patient to (currently it's based on the closest hospital)"
function convey_event!(
        ems::EMSEngine,
        problem::DispatchProblem,
        redeploy::RedeployModel,
        id::Int, # the id of the emergency call
        t::Int, # the time of the emergency call
        amb::Int
    )
    h = let mintime = Inf, minindex = 0
        for h in 1:nrow(problem.hospitals)
            traveltime = problem.emergency_calls[id, Symbol("hosp$(h)_min")]
            if !isna(traveltime) && traveltime < mintime
                @assert traveltime >= 0
                minindex = h; mintime = traveltime
            end
        end
        minindex
    end
    @assert h != 0
    redeploy.hospital[amb] = ems.eventlog[id, :hospital] = h
    conveytime = 60*15 + ceil(Int, 60*problem.emergency_calls[id, Symbol("hosp$(h)_min")]) # ~20minutes
    ems.eventlog[id, :conveytime] = conveytime / 60 # minutes
    @assert conveytime >= 0 conveytime
    conveying!(redeploy, amb, h, t)
    enqueue!(ems.eventqueue, (:return, id, t+conveytime, amb), t+conveytime)
end

function return_event!(
        ems::EMSEngine,
        problem::DispatchProblem,
        redeploy::RedeployModel,
        id::Int,
        t::Int,
        amb::Int
    )
    stn = returning_to!(redeploy, amb, t)
    h = redeploy.hospital[amb]
    returntime = ceil(Int,60*2*problem.hospitals[h, Symbol("stn$(stn)_min")]) # ~ 10minutes
    ems.eventlog[id, :returntime] = returntime / 60 # minutes
    @assert returntime >= 0 returntime
    t_end = t + returntime
    enqueue!(ems.eventqueue, (:done, id, t_end, amb), t_end)
end

function done_event!(
        ems::EMSEngine,
        problem::DispatchProblem,
        dispatch::DispatchModel,
        redeploy::RedeployModel,
        id::Int,
        t::Int,
        amb::Int
    )
    stn = redeploy.assignment[amb]; @assert stn > 0
    if sum(length(wq) for wq in problem.wait_queue[problem.coverage[:,stn]]) > 0
        # people are waiting in a queue
        redirected!(redeploy, amb, t)
        # determine the person who has waited the longest
        minindex = 0; mintime = Inf
        for nbhd in 1:size(problem.coverage,1)
            if problem.coverage[nbhd,stn] && length(problem.wait_queue[nbhd]) > 0
                arrivaltime = problem.emergency_calls[problem.wait_queue[nbhd][1], :arrival_seconds]
                if arrivaltime < mintime
                    mintime = arrivaltime
                    minindex = nbhd
                end
            end
        end
        waittime = t - mintime
        @assert minindex != 0
        @assert waittime >= 0
        @assert 0 <= mintime < Inf

        # respond to the person
        let id = shift!(problem.wait_queue[minindex])
            println(id,": amb ", amb, " redirected from stn ", stn, " to serve ", problem.emergency_calls[id, :neighborhood])
            ems.eventlog[id, :return_to] = id
            ems.eventlog[id, :return_type] = :incident
            ems.eventlog[id, :ambulance] = amb
            ems.eventlog[id, :dispatch_from] = stn
            ems.eventlog[id, :waittime] = waittime / 60 # minutes
            travel_time = ceil(Int,60*problem.emergency_calls[id, Symbol("stn$(stn)_min")])
            ems.eventlog[id, :responsetime] = travel_time / 60 # minutes
            @assert travel_time >= 0
            total_delay = waittime + travel_time; @assert total_delay >= 0
            tarrive = t + total_delay; @assert t + total_delay >= 0 "$t, $total_delay"
            enqueue!(ems.eventqueue, (:arrive, id, tarrive, amb), tarrive)
        end
    else # returned to base location
        ems.eventlog[id, :return_to] = redeploy.assignment[amb]
        @assert ems.eventlog[id, :return_type] == :station
        returned_to!(redeploy, amb, t)
        returned_to!(problem, stn, t)
        update_ambulances!(dispatch, stn, 1)
    end
end

function simulate_events!(
        problem::DispatchProblem,
        dispatch::DispatchModel,
        redeploy::RedeployModel;
        verbose::Bool=false
    )
    ems = EMSEngine(problem)
    # @show problem.available
    # @show redeploy.ambulances
    while !isempty(ems.eventqueue)
        (event, id, t, value) = dequeue!(ems.eventqueue)
        # @show (event, id, t, value)
        @assert t >= 0 # in case of integer overflow (when calls > ambulances)
        if event == :call
            call_event!(ems, problem, dispatch, redeploy, id, t, value, verbose=verbose)
        elseif event == :arrive
            arrive_event!(ems, problem, redeploy, id, t, value)
        elseif event == :convey
            convey_event!(ems, problem, redeploy, id, t, value)
        elseif event == :return
            # @show problem.available
            # @show redeploy.ambulances
            # @show redeploy.assignment
            reassign_ambulances!(ems, problem, redeploy, t)
            # println("obj: ", JuMP.getvalue(redeploy.obj1), ", ", JuMP.getvalue(redeploy.obj2))
            return_event!(ems, problem, redeploy, id, t, value)
        else
            @assert event == :done
            done_event!(ems, problem, dispatch, redeploy, id, t, value)
        end
        for i in eachindex(problem.available)
            @assert problem.available[i] == length(redeploy.ambulances[i]) "$(problem.available) versus $(redeploy.ambulances)" # "$i: $(problem.available[i]), $(length(redeploy.ambulances[i]))"
        end
    end
    # @assert all(problem.available .== problem.deployment)
    @assert all(ems.eventlog[:dispatch_from] .>= 0)
    ems.eventlog
end


text:
import DataFrames, GLM

df = simulate_data_correlation([[0.2, 0.5], [0.4]])

model = GLM.lm(GLM.@formula(y ~ Var1), df)
data = datagrid(model)

data[:y_pred] = GLM.predict(model, data)
GLM.predict(model, data, interval_type=:confint)

using Plots
plot(data[:Var1], data[:y_pred], ribbon=(0.5, 1), fillalpha=.5)


text:
function solve_exp_ode(
    M::MetricManifold,
    x,
    v,
    tspan;
    solver = AutoVern9(Rodas5()),
    backend = :default,
    kwargs...,
)
    n = length(x)
    iv = SVector{n}(1:n)
    ix = SVector{n}(n+1:2n)
    u0 = allocate(x, 2n)
    u0[iv] .= v
    u0[ix] .= x

    function exp_problem(u, p, t)
        M = p[1]
        dx = u[iv]
        x = u[ix]
        ddx = allocate(u, Size(n))
        du = allocate(u)
        Γ = christoffel_symbols_second(M, x; backend = backend)
        @einsum ddx[k] = -Γ[k, i, j] * dx[i] * dx[j]
        du[iv] .= ddx
        du[ix] .= dx
        return Base.convert(typeof(u), du)
    end

    p = (M,)
    prob = ODEProblem(exp_problem, u0, tspan, p)
    sol = solve(prob, solver; kwargs...)
    return sol
end


text:
using Revise
using LarSurf
using LinearAlgebraicRepresentation
Lar = LinearAlgebraicRepresentation
using Plasm, SparseArrays
using Pandas
using Seaborn
using Dates
using Profile
using TimerOutputs
using ExSup

fn = "exp_boundary3.csv"

# compile
sz1 = 10
b3_size = [sz1,sz1,sz1]
LarSurf.set_param(boundary_allow_write_files=true)
LarSurf.set_param(boundary_allow_read_files=true)
LarSurf.set_param(boundary_allow_memory=true)
LarSurf.get_boundary3(b3_size)
println("======")

function save_stats(acquisition, b3_size, t1)
    dt = Dict()
    dt["time"] = t1
    dt["datetime"] = Dates.format(Dates.now(), "yyyy-mm-dd HH:MM:SS")
    dt["jlfile"] = @__FILE__
    dt["allow_read_files"] = LarSurf._param_boundary_allow_read_files
    dt["allow_write_files"] = LarSurf._param_boundary_allow_write_files
    dt["allow_memory"] = LarSurf._param_boundary_allow_memory
    dt["block_size"] = b3_size[1]
    dt["acquisition"] = acquisition
    # println(dt)
    println(t1)
    ExSup.add_to_csv(dt, fn)
end

function run_all(b3_size)
    # LarSurf.set_param(boundary_allow_files=false)
    fn = LarSurf._create_name_for_boundary(b3_size)
    if isfile(fn)
        print("D")
        rm(fn)
    end
    LarSurf.set_param(boundary_allow_read_files=false)
    LarSurf.set_param(boundary_allow_write_files=true)
    LarSurf.set_param(boundary_allow_memory=false)
    t1 = @elapsed LarSurf.get_boundary3(b3_size)
    save_stats("write", b3_size, t1)

    # LarSurf.set_param(boundary_allow_files=true)
    LarSurf.set_param(boundary_allow_read_files=true)
    LarSurf.set_param(boundary_allow_write_files=false)
    LarSurf.set_param(boundary_allow_memory=false)
    t1 = @elapsed LarSurf.get_boundary3(b3_size)
    save_stats("read", b3_size, t1)

    LarSurf.set_param(boundary_allow_read_files=false)
    LarSurf.set_param(boundary_allow_write_files=false)
    LarSurf.set_param(boundary_allow_memory=false)
    t1 = @elapsed LarSurf.get_boundary3(b3_size)
    save_stats("computation", b3_size, t1)

    # LarSurf.set_param(boundary_allow_files=true)
    LarSurf.set_param(boundary_allow_read_files=false)
    LarSurf.set_param(boundary_allow_write_files=false)
    LarSurf.set_param(boundary_allow_memory=true)
    t1 = @elapsed LarSurf.get_boundary3(b3_size)
    save_stats("memory", b3_size, t1)

end

    # to have 3 samples per each size
for i=1:3
    for sz1=5:5:30
        b3_size = [sz1, sz1, sz1]
        run_all(b3_size)
    end
end


text:
# using Colors
# using CoordinateTransformations
# using FileIO
# using GeometryBasics
# using MeshCat, MeshIO, Meshing
# using Rotations
#
function visualize!(vis, model::Particle, q;
	Δt = 0.1, r = 0.25,
	name = "1",
	color = RGBA(1.0, 0.0, 0.0, 1.0))

	default_background!(vis)

    setobject!(vis["particle" * name],
		GeometryBasics.Sphere(GeometryBasics.Point3f0(0),
		convert(Float32, r)),
		MeshPhongMaterial(color = color))

    anim = MeshCat.Animation(convert(Int, floor(1.0 / Δt)))

    for t = 1:length(q)
        MeshCat.atframe(anim, t) do
            settransform!(vis["particle" * name], MeshCat.Translation(q[t][1:3]...))
        end
    end

	settransform!(vis["/Cameras/default"],
	    compose(Translation(-2.5, 7.5, 1.0),LinearMap(RotZ(0.0))))

    MeshCat.setanimation!(vis, anim)
end
#
#

using Colors
using CoordinateTransformations
using FileIO
using GeometryBasics
using MeshCat, MeshIO, Meshing
using Rotations
#
# function visualize!(vis, model::Particle, q;
# 	Δt = 0.1, r = 0.25)
#
# 	default_background!(vis)
#
#     setobject!(vis["particle"],
# 		GeometryBasics.Sphere(GeometryBasics.Point3f0(0),
# 		convert(Float32, r)),
# 		MeshPhongMaterial(color = RGBA(13/255, 152/255, 186/255, 1.0)))
#
#     anim = MeshCat.Animation(convert(Int, floor(1.0 / Δt)))
#
#     for t = 1:length(q)
#         MeshCat.atframe(anim, t) do
#             settransform!(vis["particle"], MeshCat.Translation(q[t][1], q[t][2], q[t][3]))
#         end
#     end
#
#     MeshCat.setanimation!(vis, anim)
# end
#

function plot_lines!(vis::Visualizer, model::Particle, q::AbstractVector;
		r=0.25, size=10, name::Symbol = :particle, col::Bool=true)
	# Point Traj
	com_point = Vector{Point{3,Float64}}()

	for qi in q
		push!(com_point, Point(qi[1], qi[2], qi[3] + r))
	end

	# Set lines
	orange_mat, blue_mat, black_mat = get_line_material(size)
	if col
		setobject!(vis[name]["lines"]["com"], MeshCat.Line(com_point, orange_mat))
	else
		setobject!(vis[name]["lines"]["com"], MeshCat.Line(com_point, black_mat))
	end
	return nothing
end

function build_robot!(vis::Visualizer, model::Particle; name::Symbol = :Particle, r=0.25, α=1.0)
	nc = model.nc
	r = convert(Float32, r)
	body_mat = MeshPhongMaterial(color = RGBA(13/255, 152/255, 186/255, α))

	default_background!(vis)

    setobject!(vis[name][:robot]["particle"], GeometryBasics.Rect(
		-r * Vec(1,1,1.),
		2r * Vec(1,1,1.)),
		body_mat)
	return nothing
end

function set_robot!(vis::Visualizer, model::Particle, q::AbstractVector; name::Symbol=:Particle, r=0.25)
	nc = model.nc
	settransform!(vis[name][:robot]["particle"], Translation(q[1], q[2], q[3]+r))
	return nothing
end

function contact_point(model::Particle, q::AbstractVector; r=0.25)
	return [q[1:3]]
end


text:
using KumiKumiSlope
using Test

using KumiKumiSlope: Point, empty_points

@testset "KumiKumiSlope.jl" begin
    include("parts.jl")
    include("sets.jl")
end


text:
# Testing continuous univariate distributions

using Distributions, Random
using Test

using Calculus: derivative

n_tsamples = 100

# additional distributions that have no direct counterparts in R references
@testset "Testing $(distr)" for distr in [Biweight(),
                                          Biweight(1,3),
                                          Epanechnikov(),
                                          Epanechnikov(1,3),
                                          Triweight(),
                                          Triweight(2),
                                          Triweight(1, 3),
                                          Triweight(1)]

    test_distr(distr, n_tsamples; testquan=false)
end

# Test for non-Float64 input
using ForwardDiff
@test string(logpdf(Normal(0,1),big(1))) == "-1.418938533204672741780329736405617639861397473637783412817151540482765695927251"
@test derivative(t -> logpdf(Normal(1.0, 0.15), t), 2.5) ≈ -66.66666666666667
@test derivative(t -> pdf(Normal(t, 1.0), 0.0), 0.0) == 0.0

@testset "Normal distribution with non-standard (ie not Float64) parameter types" begin
    n32 = Normal(1f0, 0.1f0)
    n64 = Normal(1., 0.1)
    nbig = Normal(big(pi), big(ℯ))

    @test eltype(typeof(n32)) === Float32
    @test eltype(rand(n32)) === Float32
    @test eltype(rand(n32, 4)) === Float32

    @test eltype(typeof(n64)) === Float64
    @test eltype(rand(n64)) === Float64
    @test eltype(rand(n64, 4)) === Float64
end

# Test for numerical problems
@test pdf(Logistic(6,0.01),-2) == 0

@testset "Normal with std=0" begin
    d = Normal(0.5,0.0)
    @test pdf(d, 0.49) == 0.0
    @test pdf(d, 0.5) == Inf
    @test pdf(d, 0.51) == 0.0

    @test cdf(d, 0.49) == 0.0
    @test cdf(d, 0.5) == 1.0
    @test cdf(d, 0.51) == 1.0

    @test ccdf(d, 0.49) == 1.0
    @test ccdf(d, 0.5) == 0.0
    @test ccdf(d, 0.51) == 0.0

    @test quantile(d, 0.0) == -Inf
    @test quantile(d, 0.49) == 0.5
    @test quantile(d, 0.5) == 0.5
    @test quantile(d, 0.51) == 0.5
    @test quantile(d, 1.0) == +Inf

    @test rand(d) == 0.5
    @test rand(MersenneTwister(123), d) == 0.5
end

# Test for parameters beyond those supported in R references
@testset "VonMises with large kappa" begin
    d = VonMises(1.1, 1000)
    @test var(d) ≈ 0.0005001251251957198
    @test entropy(d) ≈ -2.034688918525470
    @test cf(d, 2.5) ≈ -0.921417 + 0.38047im atol=1e-6
    @test pdf(d, 0.5) ≈ 1.758235814051e-75
    @test logpdf(d, 0.5) ≈ -172.1295710466005
    @test cdf(d, 1.0) ≈ 0.000787319 atol=1e-9
end

@testset "NormalInverseGaussian random repeatable and basic metrics" begin
    rng = Random.MersenneTwister(42)
    rng2 = copy(rng)
    µ = 0.0
    α = 1.0
    β = 0.5
    δ = 3.0
    g = sqrt(α^2 - β^2)
    d = NormalInverseGaussian(μ, α, β, δ)
    v1 = rand(rng, d)
    v2 = rand(rng, d)
    v3 = rand(rng2, d)
    @test v1 ≈ v3
    @test v1 ≉ v2

    @test mean(d) ≈ µ + β * δ / g
    @test var(d) ≈ δ * α^2 / g^3
    @test skewness(d) ≈ 3β/(α*sqrt(δ*g))
end


text:
"""
    function round_to_result(
        round::Int64
    )::String

Returns the result corresponding to a round

Arguments
=========
- `round`:
    the round within a tournament.
"""

function round_to_result(
    round::Int64
)::String
    if round == 1
        return "Winner"
    elseif round == 2
        return "Finals"
    elseif round == 4
        return "Semi-Finals"
    elseif round == 8
        return "Quarter-Finals"
    elseif round == 16
        return "Round-16"
    elseif round == 32
        return "Round-32"
    elseif round == 64
        return "Round-64"
    elseif round == 128
        return "Round-128"
    else
        return "Qualified"
    end
end

max_stars_repo_name: Danial-Hussain/Tennis-Ranking

max_issues_repo_name: Danial-Hussain/Tennis-Ranking

max_forks_repo_name: Danial-Hussain/Tennis-Ranking


text:
using Test, Joseki, JSON
import HTTP.IOExtras.bytes

@testset "Responders" begin
    req = sample_request()
    obj = Dict{String, Any}("some string" => "a", "a number" => 3)
    expected = Dict{String, Any}("error" => false, "result" => obj)
    res = Joseki.json_responder(req, obj)
    @test JSON.parse(String(res.body)) == expected

    req = sample_request()
    obj = Dict{String, Any}("some string" => "a", "a number" => 3)
    res = Joseki.simple_json_responder(req, obj)
    @test JSON.parse(String(res.body)) == obj

    req = sample_request()
    expected = Dict{String, Any}("error" => true, "message" => "error goes here")
    res = Joseki.error_responder(req, "error goes here")
    @test JSON.parse(String(res.body)) == expected

    req = sample_request()
    res = Joseki.error_responder(req, DivideError())
    # Don't worry about the particular message
    @test JSON.parse(String(res.body))["error"]

    req = sample_request()
    res = Joseki.critical_error_responder(req, DivideError())
    # Don't worry about the particular message
    @test res.status == 500
end


text:
module FASTX

export
    FASTA,
    FASTQ

include("fasta/fasta.jl")
include("fastq/fastq.jl")

end # module


text:
# This file was generated by the Julia Swagger Code Generator
# Do not modify this file directly. Modify the swagger specification instead.

mutable struct DeploymentExportResult <: SwaggerModel
    template::Any # spec type: Union{ Nothing, Any } # spec name: template

    function DeploymentExportResult(;template=nothing)
        o = new()
        validate_property(DeploymentExportResult, Symbol("template"), template)
        setfield!(o, Symbol("template"), template)
        o
    end
end # type DeploymentExportResult

const _property_map_DeploymentExportResult = Dict{Symbol,Symbol}(Symbol("template")=>Symbol("template"))
const _property_types_DeploymentExportResult = Dict{Symbol,String}(Symbol("template")=>"Any")
Base.propertynames(::Type{ DeploymentExportResult }) = collect(keys(_property_map_DeploymentExportResult))
Swagger.property_type(::Type{ DeploymentExportResult }, name::Symbol) = Union{Nothing,eval(Meta.parse(_property_types_DeploymentExportResult[name]))}
Swagger.field_name(::Type{ DeploymentExportResult }, property_name::Symbol) =  _property_map_DeploymentExportResult[property_name]

function check_required(o::DeploymentExportResult)
    true
end

function validate_property(::Type{ DeploymentExportResult }, name::Symbol, val)
end


text:
"""
`folderlistall(dir)` return the list of folders and all subfolders under `dir`. For its keyword arguments, see `walkdir`.
"""
function folderlistall(dir)
    wd = walkdir(dir)
    allpaths = String[]
    for (root, dirs, files) in wd
        for dir_i in dirs
            path_i = joinpath(root, dir_i)
            push!(allpaths, path_i)
        end
    end
    return allpaths
end

"""
`folderlist(dir; join=true)` return the list of folder but no subfolder under `dir`. This function uses `walkdir`.
"""
function folderlist(dir; join=true)
    wd = walkdir(dir) # you cannot add topdown=false to walkdir; otherwise, popfirst! won't return the first level of target folders/files.
    allpaths = String[]
    (root, dir, file) = popfirst!(wd)
    ans = if join
        joinpath.(root, dir)
    else
        dir
    end
    return ans
end

"""
`folderlist(expr::Regex, dir; join=true)` returns a vector of paths who match `expr`.
"""
function folderlist(expr::Regex, dir; join=true)
    allfolders = folderlist(dir; join = join)
    if join
        desired_ind = occursin.(expr, basename.(allfolders))
    else
        desired_ind = occursin.(expr, allfolders)
    end
    return allfolders[desired_ind]
end


text:
module DynamicsOutputs

using RecursiveArrayTools: ArrayPartition
using DiffEqCallbacks: SavedValues, SavingCallback
using ComponentArrays: ComponentVector

using NQCModels: NQCModels
using NQCDynamics:
    Estimators,
    DynamicsUtils

using ..DynamicsUtils:
    get_positions,
    get_velocities,
    get_quantum_subsystem

using ..RingPolymers: get_centroid

"""
    create_saving_callback(quantities::NTuple{N, Symbol}; saveat=[]) where {N}

Get the `SavingCallback` that will populate `saved_values` with the result obtained
by evaluating the functions provided in `function_names`.
"""
function create_saving_callback(function_names::NTuple{N, Symbol}; saveat=[]) where {N}
    saved_values = SavedValues(Float64, Vector{Any})
    saving_function = OutputSaver(function_names)
    SavingCallback(saving_function, saved_values; saveat=saveat), saved_values
end

struct OutputSaver{N,F}
    function_names::NTuple{N, Symbol}
    output_functions::F
end

"""
    OutputSaver(function_names::NTuple{N, Symbol}) where {N}

Used to obtain the outputs for all functions given in `function_names`.
"""
function OutputSaver(function_names::NTuple{N, Symbol}) where {N}
    output_functions = tuple([getfield(DynamicsOutputs, f) for f in function_names]...)
    OutputSaver(function_names, output_functions)
end

"""
    (output::OutputSaver)(u, t, integrator)

Evaluates every function listed in `output.function_names` and returns all the results.
"""
function (output::OutputSaver)(u, t, integrator)
    out = Any[]
    sizehint!(out, length(output.function_names))
    evaluate_output!(out, u, t, integrator, output.output_functions...)
    return out
end

"""
Used to recursively evaluate every function for the [`OutputSaver`](@ref).

See here for a description of why it is written like this:
https://stackoverflow.com/questions/55840333/type-stability-for-lists-of-closures
"""
function evaluate_output!(out, u, t, integrator, f::F, output_functions...) where {F}
    push!(out, f(u, t, integrator))
    return evaluate_output!(out, u, t, integrator, output_functions...)
end
evaluate_output!(out, u, t, integrator) = out

export force
export velocity
export position
export centroid_position
export potential
export hamiltonian
export kinetic
export u
export quantum_subsystem
export state
export noise
export population
export adiabatic_population
export friction

"Get the force"
force(u, t, integrator) = -copy(integrator.p.calculator.derivative)

"Get the velocity"
velocity(u, t, integrator) = copy(get_velocities(u))

"Get the velocity of the ring polymer centroid"
centroid_velocity(u, t, integrator) = get_centroid(get_velocities(u))

"Get the position"
position(u, t, integrator) = copy(get_positions(u))

"Get the position of the ring polymer centroid"
centroid_position(u, t, integrator) = get_centroid(get_positions(u))

"Evaluate the potential from the model"
potential(u, t, integrator) = NQCModels.potential(integrator.p.calculator.model, get_positions(u))[1]

"Evaluate the classical Hamiltonian"
hamiltonian(u, t, integrator) = DynamicsUtils.classical_hamiltonian(integrator.p, u)

"Evaluate the classical kinetic energy"
kinetic(u, t, integrator) = DynamicsUtils.classical_kinetic_energy(integrator.p, get_velocities(u))

"Get all the dynamics variables. This is the default"
u(u, t, integrator) = copy(u)
u(u::ArrayPartition, t, integrator) = ComponentVector(v=copy(get_velocities(u)), r=copy(get_positions(u)))

"""
Get the quantum subsystem of the dynamics variables.
Requires that `DynamicsUtils.get_quantum_subsystem` is implemented for your chosen method.
"""
quantum_subsystem(u, t, integrator) = copy(get_quantum_subsystem(u))

mapping_position(u, t, integrator) = copy(DynamicsUtils.get_mapping_positions(u))
mapping_momentum(u, t, integrator) = copy(DynamicsUtils.get_mapping_momenta(u))

"""
Get the currently occupied state from the dynamics variables.
Requires that the dynamics variable has a field `state`.
Currently this is for surface hopping methods only.
Use [`population`](@ref) or [`adiabatic_population`](@ref) for most other methods.
"""
state(u, t, integrator) = copy(u.state)

"Get the noise along the stochastic trajectory"

noise(u, t, integrator) = copy(integrator.W.dW) / sqrt(integrator.dt)

"Evaluate the diabatic population"
population(u, t, integrator) = Estimators.diabatic_population(integrator.p, u)

"Evaluate the adiabatic population"
adiabatic_population(u, t, integrator) = Estimators.adiabatic_population(integrator.p, u)

"Evaluate the friction. This is used for MDEF only."
function friction(u, t, integrator)
    integrator.g(integrator.cache.gtmp,get_positions(u),integrator.p,t)
    copy(integrator.cache.gtmp)
end

end # module


text:
using HighLevelTypes, Base.Test

include("types.jl")
include("basic.jl")
include("constructors.jl")


text:
using GeometricEquations: _idae_default_v̄, _ldae_default_v̄

function ode_v(t, x, ẋ, params)
    ẋ[1] = x[2]
    ẋ[2] = 2x[1]
end

function sode_v1(t, x, v, params)
    v[1] = x[2]
end

function sode_v2(t, x, v, params)
    v[2] = 2x[1]
end

function sode_q1(t, x̄, x, params)
    x[1] = x̄[1]
    x[2] = x̄[2]
end

function sode_q2(t, x̄, x, params)
    x[1] = x̄[1]
    x[2] = x̄[2]
end

sode_v = (sode_v1, sode_v2)
sode_q = (sode_q1, sode_q2)

function pode_v(t, q, p, v, params)
    v[1] = p[1]
end

function pode_f(t, q, p, f, params)
    f[1] = 2q[1]
end

function pode_h(t, q, p, params)
    p[1]^2/2 + cos(q[1])
end

pode_eqs = (pode_v, pode_f)

function hode_h(t, q, p, params)
    p[1]^2/2 + cos(q[1])
end

function hode_ω(t, q, p, ω, params)
    ω[1,1] = sin(q[1])
    ω[1,2] = 0
    ω[2,1] = 0
    ω[2,2] = p[1]
end

hode_eqs = (pode_v, pode_f, hode_ω, hode_h)

function iode_ϑ(t, q, v, p, params)
    p[1] = v[1]
end

function iode_f(t, q, v, f, params)
    f[1] = sin(q[1])
end

function iode_u(t, q, λ, u, params)
    u[1] = λ[1]
end

function iode_g(t, q, λ, g, params)
    g[1] = λ[1]
end

function iode_v(t, q, p, v, params)
    v[1] = p[1]
end

function iode_h(t, q, v, params)
    v[1]^2/2 + cos(q[1])
end

iode_eqs = (iode_ϑ, iode_f, iode_g)

function lode_l(t, q, v, params)
    v[1]^2/2 - cos(q[1])
end

function lode_ω(t, q, v, ω, params)
    ω[1,1] = sin(q[1])
    ω[1,2] = 0
    ω[2,1] = 0
    ω[2,2] = v[1]
end

lode_eqs = (iode_ϑ, iode_f, iode_g, lode_ω, lode_l)

function dae_v(t, x, v, params)
    v[1] = x[1]
    v[2] = x[2]
end

function dae_u(t, x, λ, u, params)
    u[1] = +λ[1]
    u[2] = -λ[1]
end

function dae_ū(t, x, λ, u, params)
    u[1] = +λ[1]
    u[2] = -λ[1]
end

function dae_ϕ(t, x, ϕ, params)
    ϕ[1] = x[2] - x[1]
end

function dae_ψ(t, x, v, ψ, params)
    ψ[1] = v[2] - v[1]
end

function pdae_v(t, q, p, v, params)
    v[1] = p[1]
end

function pdae_f(t, q, p, f, params)
    f[1] = q[1]
end

function pdae_p(t, q, v, p, params)
    p[1] = v[1]
end

function pdae_u(t, q, p, λ, u, params)
    u[1] = λ[1]
end

function pdae_g(t, q, p, λ, g, params)
    g[1] = λ[1]
end

function pdae_ϕ(t, q, p, λ, ϕ, params)
    ϕ[1] = p[1] - q[1]
end

function pdae_ψ(t, q, p, λ, μ, ψ, params)
    ψ[1] = μ[1] - λ[1]
end

function pdae_h(t, q, p)
    p[1]^2/2 + q[1]^2/2
end

hdae_ω = hode_ω

idae_ϑ = iode_ϑ
idae_f = iode_f
idae_v = _idae_default_v̄

ldae_l = lode_l
ldae_ω = lode_ω
ldae_v = _ldae_default_v̄

function sde_v(λ, t, q, v)
    v[1] = λ*q[1]
    v[2] = λ*q[2]
end

function sde_B(μ, t, q, u)
    u[1] = μ*q[1]
    u[2] = μ*q[2]
end

function psde_v(t, q, p, v)
    v[1] = p[1]
end

function psde_f(t, q, p, f)
    f[1] = - q[1]
end

function psde_B(t, q, p, B)
    B[1,1] = noise_intensity * p[1]
end

function psde_G(t, q, p, G)
    G[1,1] = - noise_intensity * q[1]
end

function spsde_v(t, q, p, v)
    v[1] = p[1]
end

function spsde_f1(t, q, p, f)
    f[1] = - q[1]
end

function spsde_f2(t, q, p, f)
    f[1] = 0
end

function spsde_B(t, q, p, B)
    B[1,1] = noise_intensity * p[1]
end

function spsde_G1(t, q, p, G)
    G[1,1] = - noise_intensity * q[1]
end

function spsde_G2(t, q, p, G)
    G[1,1] = 0
end


text:
# [[fg:]<col>] [bg:<col>] ([[!]properties], ...)

macro crayon_str(str::String)
    _reset         = ANSIStyle()
    _bold          = ANSIStyle()
    _faint         = ANSIStyle()
    _italics       = ANSIStyle()
    _underline     = ANSIStyle()
    _blink         = ANSIStyle()
    _negative      = ANSIStyle()
    _conceal       = ANSIStyle()
    _strikethrough = ANSIStyle()

    fgcol = ANSIColor()
    bgcol = ANSIColor()

    for word in split(str, " ")
        length(word) == 0 && continue
        token = word
        enabled = true
        parse_state = :style

        if word[1] == '!'
            enabled = false
            token = word[2:end]
            @goto doparse
        end

        if ':' in word
            ws = split(word, ':')
            if length(ws) != 2
                @goto parse_err
            end
            val, token = ws
            if val == "fg"
                parse_state = :fg_color
            elseif val == "bg"
                parse_state = :bg_color
            else
                @goto parse_err
            end
            @goto doparse
            @label parse_err
            throw(ArgumentError("should have the format [fg/bg]:color"))
        end

        @label doparse
        if parse_state == :fg_color || parse_state == :bg_color
            color = _parse_color_string(token)
            if parse_state == :fg_color
                fgcol = color
            else
                bgcol = color
            end
        elseif parse_state == :style
            if token == "reset"
                _reset = ANSIStyle(enabled)
            elseif token == "bold"
                _bold = ANSIStyle(enabled)
            elseif token == "faint"
                _faint = ANSIStyle(enabled)
            elseif token == "italics"
                _italics = ANSIStyle(enabled)
            elseif token == "underline"
                _underline = ANSIStyle(enabled)
            elseif token == "blink"
                _blink = ANSIStyle(enabled)
            elseif token == "negative"
                _negative = ANSIStyle(enabled)
            elseif token == "conceal"
                _conceal = ANSIStyle(enabled)
            elseif token == "strikethrough"
                _strikethrough = ANSIStyle(enabled)
            else
                fgcol = _parse_color_string(token)
            end
        end
    end

    return Crayon(
        fgcol,
        bgcol,
        _reset,
        _bold,
        _faint,
        _italics,
        _underline,
        _blink,
        _negative,
        _conceal,
        _strikethrough,
    )
end

function _parse_color_string(token::AbstractString)
    if length(token) >= 6
        tok_hex = token
        startswith(token, "#") && (tok_hex = token[2:end])
        !startswith(token, "0x") && (tok_hex = "0x" * tok_hex)
        nhex = tryparse(UInt32, tok_hex)
        nhex !== nothing && return _parse_color(nhex)
    end

    nint = tryparse(Int, token)
    nint !== nothing && return _parse_color(nint)
    reg = r"\(([0-9]*),([0-9]*),([0-9]*)\)"
    m = match(reg, token)
    if m !== nothing
        r = m.captures[1]::SubString{String}
        g = m.captures[2]::SubString{String}
        b = m.captures[3]::SubString{String}
        return _parse_color(parse.(Int, (r, g, b)))
    end

    if Symbol(token) in keys(COLORS)
        return _parse_color(Symbol(token))
    end

    throw(ArgumentError("could not parse $token as a color"))
end


text:
function spgreqn(x::Vector{Float64}, p::Vector{Float64}, TR::Float64)
  y = similar(x)
  E = exp(-TR*p[2])  # hoisted expensive, invariant result
  @simd for k in 1:length(x)
     @inbounds y[k] = p[1]*sin(x[k])*(1.0 - E) / (1.0 - E*cos(x[k]))
  end
  y
end

function expConv(A::Vector{Float64}, B::Float64, t::Vector{Float64})
  # Returns f = A ConvolvedWith exp(-B t)
  # Based on Flouri et al. (2016) MRM 76(3), doi: 10.1002/mrm.25991
  f = zeros(length(t))
  for i in 1:length(t)-1
        x = B * ( t[i+1] - t[i] )
        dA = ( A[i+1] - A[i] ) / x
        E = exp(-x)
        E0 = 1 - E
        E1 = x - E0
        f[i+1] = E*f[i] + A[i] * E0 + dA * E1
  end
  f = f / B
end

function toftskety(t::Vector{Float64}, p::Vector{Float64}, Cp::Vector{Float64})
  # Standard Tofts-Kety Model.  Works when t_dce = t_aif only, so you should
  # resample the AIF to match the DCE dynamic spacing before using.
  Ct = zero(t)
  kep = p[2]  # kep = Ktrans / ve
  Ct = p[1] * expConv(Cp,kep,t)
  #   for k in 1:length(Ct)
  #     #Ct[k] = p[1]*trapz(exp(-p[1]*(t[k] - t[1:k])/p[2]) .* Cp[1:k], t[1:k])
  #     # inlined trapz here ... original equations are up here ^
  #     tk = t[k]
  #     @simd for j = 1:k
  #       @inbounds y = exp(kep*(t[j] - tk)) * Cp[j]
  #       @inbounds Ct[k] += ifelse((j == 1 || j == k) && k > 1, 0.5*y, y)
  #     end
  #   end
  #   dtp = (t[2] - t[1]) * p[1]
  #   @simd for k = 1:length(Ct)
  #     @inbounds Ct[k] *= dtp
  #   end
  Ct
end

function extendedtoftskety(t::Vector{Float64}, p::Vector{Float64}, Cp::Vector{Float64})
  # Extended Tofts-Kety Model. Works when t_dce = t_aif only, so you should
  # resample the AIF to match the DCE dynamic spacing before using.
  Ct = toftskety(t, p, Cp)
  @simd for k in 1:length(Ct)
    @inbounds Ct[k] += p[3]*Cp[k]
  end
  Ct
end

function onecompartment(t::Vector{Float64}, p::Vector{Float64}, Cp::Vector{Float64})
  # plasma only model
  Ct = similar(Cp)
  @simd for k in 1:length(Ct)
    @inbounds Ct[k] = p[1]*Cp[k]
  end
  Ct
end

function fitTM(t::Vector{Float64}, Ct::Matrix{Float64}, Cp::Vector{Float64})
  (sT,sX) = size(Ct)
  pkParams = zeros(2,sX)
  resid = zeros(sT,sX)

  M = zeros(sT,2)
  M[:,1] = cumtrapz(t,Cp)

  for i in 1:sX
      M[:,2] = -cumtrapz(t,Ct[:,i])
      pkParams[:,i] = M\Ct[:,i]
      resid[:,i] = (M*pkParams[:,i]-Ct[:,i]) / sqrt(norm(Ct[:,i]))
  end
  r = sum(resid.^2, dims=1) ./ (sT-2)
  (pkParams, r)
end

function fitETM(t::Vector{Float64}, Ct::Matrix{Float64}, Cp::Vector{Float64})
  (sT,sX) = size(Ct)
  pkParams = zeros(3,sX)
  resid = zeros(sT,sX)

  trapzCp = cumtrapz(t,Cp)
  M = zeros(sT,3)
  M[:,1] = trapzCp
  M[:,3] = Cp

  for i in 1:sX
      M[:,2] = -cumtrapz(t,Ct[:,i])
      pkParams[:,i] = M\Ct[:,i]
      resid[:,i] = (M*pkParams[:,i]-Ct[:,i]) / sqrt(norm(Ct[:,i]))
  end
  pkParams[1,:] = pkParams[1,:] - pkParams[2,:] .* pkParams[3,:]
  r = sum(resid.^2, dims=1) ./ (sT-3)
  (pkParams, r)
end