text:
using Distributed
addprocs(2)
# using Revise
using GraphPlot
using Colors
using DistributedFactorGraphs
using RoME
@everywhere using RoME
using RoMEPlotting
Gadfly.set_default_plot_size(35cm,25cm)

function getCoordCartesianFromKDERange(dfg::AbstractDFG, varsym::Symbol; digits::Int=-2, extend::Float64=0.5)
    ax = round.(getKDERange(getKDE(dfg, varsym), extend=extend), digits=digits)
    if size(ax, 1) == 1
      return Coord.Cartesian(xmin=ax[1,1], xmax=ax[1,2])
    elseif size(ax, 1) == 2
      return Coord.Cartesian(xmin=ax[1,1], xmax=ax[1,2],ymin=ax[2,1], ymax=ax[2,2])
    else
      error("only do 1D or 2D")
    end
end
# import KernelDensityEstimate: getCoordCartesianFromKDERange

function addMaptoPlot!(pl)
    # plot the design coordinates
    map_coords = Array{Array{Float64,1},1}[[[0.0, 0.0], [0.0, 330.0], [40.0, 330.0], [40.0, 310.0], [20.0, 310.0], [20.0, 150.0], [130.0, 150.0], [130.0, 140.0], [20.0, 140.0], [20.0, 20.0], [250.0, 20.0], [250.0, 60.0], [260.0, 60.0], [260.0, 20.0], [290.0, 20.0], [290.0, 60.0], [300.0, 60.0], [300.0, 20.0], [430.0, 20.0], [430.0, 110.0], [300.0, 110.0], [300.0, 100.0], [290.0, 100.0], [290.0, 190.0], [280.0, 190.0], [280.0, 200.0], [320.0, 200.0], [320.0, 190.0], [300.0, 190.0], [300.0, 120.0], [430.0, 120.0], [430.0, 190.0], [410.0, 190.0], [410.0, 200.0], [430.0, 200.0], [430.0, 310.0], [390.0, 310.0], [390.0, 330.0], [450.0, 330.0], [450.0, 0.0], [0.0, 0.0]], [[90.0, 310.0], [90.0, 330.0], [170.0, 330.0], [170.0, 310.0], [90.0, 310.0]], [[230.0, 310.0], [340.0, 310.0], [340.0, 330.0], [210.0, 330.0], [210.0, 310.0], [220.0, 310.0], [220.0, 200.0], [210.0, 200.0], [210.0, 190.0], [240.0, 190.0], [240.0, 200.0], [230.0, 200.0], [230.0, 310.0]]]

    for m_coord in map_coords
        Xpp = map(p->p[1] + 150, m_coord)
        Ypp = map(p->-p[2] +330 + 650, m_coord)

        push!(pl.layers, Gadfly.layer(x=Xpp,y=Ypp,Geom.path(), Theme(line_width=1pt))[1])
    end

    # plot the real map variation
    built_coords = Array{Array{Float64,1},1}[[[230.0, 310.0], [340.0, 310.0], [340.0, 330.0], [210.0, 330.0], [210.0, 310.0], [220.0, 310.0], [220.0, 200.0], [210.0, 200.0], [210.0, 190.0], [240.0, 190.0], [240.0, 200.0], [230.0, 200.0], [230.0, 310.0]]]

    for b_coord in built_coords
        Xpp = map(p->p[1] + 150+30, b_coord)
        Ypp = map(p->-p[2] +330 + 650+20, b_coord)

        push!(pl.layers, Gadfly.layer(x=Xpp,y=Ypp,Geom.path(), Theme(line_width=1pt, default_color=colorant"red"))[1])
    end

    return pl
end

## First add the designed landmark positions
function addLandmarkTrueWithPrior!(dfg::AbstractDFG,
                                   key::Symbol,
                                   vals::Vector{<: Real},
                                   lgt_cov::Array{Float64,2}=0.01*Matrix(LinearAlgebra.I, 2,2)  )
  #
  if !exists(dfg, key)
    addVariable!(dfg, key, Point2)
    pp = PriorPoint2(MvNormal(vals, lgt_cov))
    addFactor!(dfg, [key], pp)
  end
  nothing
end

macro doline(skip, operation)
  :($skip ? $operation : nothing)
end

ground = Dict{Symbol, Vector{Float64}}(
  :x0=>[260.0, 720.0, 0.0],
  :x1=>[260.0, 720.0, -pi/2],
  :x2=>[260.0, 720.0, -pi],
  :x3=>[260.0, 720.0, pi/2],
  :x4=>[260.0, 800.0, 0.0],
  :x5=>[310.0, 800.0, 0.0],
  :x6=>[310.0, 800.0, -pi/2],
  :x7=>[310.0, 800.0, -pi],
  :x8=>[310.0, 800.0, pi/2],
)

## Add all variables as design truth (for use as multihypos later)
# Ground truth for landmarks (designed)
landmarks_real = Dict(
    :l1=>[360+30,780+20],
    :l2=>[370+30,780+20],
    :l3=>[370+30,670+20],
    :l4=>[360+30,670+20],
    :l5=>[360+30,790+20],
    :l8=>[190,670],
    :l6=>[170,670],
    :l7=>[170,830],
    :l9=>[280,830],
    :l10=>[440,790],
    :l11=>[430,790],
    :l12=>[400,920])

# map with Dx=-10 on l1-l4 (ground truth from measured, varies from design)
landmarks_design = Dict(
    :l1=>[360,780],
    :l2=>[370,780],
    :l3=>[370,670],
    :l4=>[360,670],
    :l5=>[360,790],
    :l8=>[190,670],
    :l6=>[170,670],
    :l7=>[170,830],
    :l9=>[280,830],
    :l10=>[440,790],
    :l11=>[430,790],
    :l12=>[400,920])

## parameters
autoinit = true
pp_σx = 0.1
pp_σy = 0.1
pp_σψ = 0.01
br_σb = 0.03
# br_σb = 0.001
br_σρ = 0.3
pMeas, pDesg = 0.7, 0.3

SKL = true

## Start the factor graph construction
fg = LightDFG{SolverParams}(params=SolverParams())

## develop the robotic factor graph elements
# x0
x0_prior = [ground[:x0],[0.1,0.1,0.01]] #fixed
# x0_prior = [ground[:x0],[100.,100,1]] #fixed
addVariable!(fg, :x0, Pose2)
addFactor!(fg, [:x0],  PriorPose2( MvNormal(x0_prior[1], Matrix(Diagonal(x0_prior[2].^2))) ), autoinit=autoinit )

    br_calc = calcPosePointBearingRange(ground[:x0], landmarks_real[:l1])
    # x0l1_br = [[deg2rad(27.65), 0.02], [118.53, 0.2]]#(tagψ, σtagψ), Normal(tagrange, σtagrange)
    x0l1_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    @doline SKL addVariable!(fg, :l1, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l1_0, landmarks_design[:l1])
    @doline SKL addFactor!(fg, [:x0, :l1, :l1_0], Pose2Point2BearingRange(Normal(x0l1_br[1]...), Normal(x0l1_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

    br_calc = calcPosePointBearingRange(ground[:x0], landmarks_real[:l2])
    x0l2_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x0l2_br = [[deg2rad(25.56), 0.02], [127.48, 0.2]]
    @doline SKL addVariable!(fg, :l2, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l2_0, landmarks_design[:l2])
    @doline SKL addFactor!(fg, [:x0, :l2, :l2_0], Pose2Point2BearingRange(Normal(x0l2_br[1]...), Normal(x0l2_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

    br_calc = calcPosePointBearingRange(ground[:x0], landmarks_real[:l3])
    x0l3_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x0l3_br = [[deg2rad(-25.56), 0.02], [127.48, 0.2]]
    @doline SKL addVariable!(fg, :l3, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l3_0, landmarks_design[:l3])
    @doline SKL addFactor!(fg, [:x0, :l3, :l3_0], Pose2Point2BearingRange(Normal(x0l3_br[1]...), Normal(x0l3_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

    br_calc = calcPosePointBearingRange(ground[:x0], landmarks_real[:l4])
    x0l4_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x0l4_br = [[deg2rad(-27.65), 0.02], [118.53, 0.2]]
    @doline SKL addVariable!(fg, :l4, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l4_0, landmarks_design[:l4])
    @doline SKL addFactor!(fg, [:x0, :l4, :l4_0], Pose2Point2BearingRange(Normal(x0l4_br[1]...), Normal(x0l4_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

## test for multimodality
# dfgplot(fg)
# drawGraph(fg)
# # ensureAllInitialized!(fg)
getSolverParams(fg).drawtree=true
getSolverParams(fg).showtree=true
# getSolverParams(fg).dbg=true
# getSolverParams(fg).async=false
tree = solveTree!(fg);
pl = drawPosesLandms(fg, drawhist=true, contour=false)
addMaptoPlot!(pl)
pl.coord = Coord.Cartesian(xmin=100, xmax=450, ymin=600, ymax=850)
pl
# pl |> PDF(ENV["HOME"]*"/Downloads/test.pdf")

# plot l1
pl1 = drawLandms(fg, to=1, drawhist=true, contour=false)
addMaptoPlot!(pl1)
pl1.coord = getCoordCartesianFromKDERange(fg, :l1, digits=0)
pl1

# plot l2
pl2 = drawLandms(fg, from=2,to=2, drawhist=true, contour=false)
addMaptoPlot!(pl2)
pl2.coord = getCoordCartesianFromKDERange(fg, :l2, digits=0)
pl2

# plot l3
pl3 = drawLandms(fg, from=3,to=3, drawhist=true, contour=false)
addMaptoPlot!(pl3)
pl3.coord = getCoordCartesianFromKDERange(fg, :l3, digits=0)
pl3

# plot l4
pl4 = drawLandms(fg, from=4,to=4, drawhist=true, contour=false)
addMaptoPlot!(pl4)
pl4.coord = getCoordCartesianFromKDERange(fg, :l4, digits=0)
pl4

pl = vstack(hstack(pl1,pl2), hstack(pl3,pl4))
# pl |> PDF(ENV["HOME"]*"/Downloads/stack.pdf")

# x1
x0x1_pp = [[0,0,deg2rad(-90)], [pp_σx, pp_σy, pp_σψ]]
addVariable!(fg, :x1, Pose2)
addFactor!(fg, [:x0, :x1], Pose2Pose2(MvNormal(x0x1_pp[1], Matrix(Diagonal(x0x1_pp[2].^2)))), autoinit=autoinit)
# ensureAllInitialized!(fg)
# x2
x1x2_pp = [[0,0,deg2rad(-90)], [pp_σx, pp_σy, pp_σψ]]
addVariable!(fg, :x2, Pose2)
addFactor!(fg, [:x1, :x2], Pose2Pose2(MvNormal(x1x2_pp[1], Matrix(Diagonal(x1x2_pp[2].^2)))), autoinit=autoinit)

    br_calc = calcPosePointBearingRange(ground[:x2], landmarks_real[:l5])
    x2l5_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x2l5_br = [[deg2rad(-143.75+180), 0.1], [92.65, 1]]
    @doline SKL addVariable!(fg, :l5, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l5_0, landmarks_design[:l5])
    @doline SKL addFactor!(fg, [:x2, :l5, :l5_0], Pose2Point2BearingRange(Normal(x2l5_br[1]...), Normal(x2l5_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

    br_calc = calcPosePointBearingRange(ground[:x2], landmarks_real[:l6])
    x2l6_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x2l6_br = [[deg2rad(-149.93+180), 0.1], [109.77, 1]]
    @doline SKL addVariable!(fg, :l6, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l6_0, landmarks_design[:l6])
    @doline SKL addFactor!(fg, [:x2, :l6, :l6_0], Pose2Point2BearingRange(Normal(x2l6_br[1]...), Normal(x2l6_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

# ensureAllInitialized!(fg)
# x3:
x2x3_pp = [[0,0,deg2rad(-90)], [pp_σx, pp_σy, pp_σψ]]
addVariable!(fg, :x3, Pose2)
addFactor!(fg, [:x2, :x3], Pose2Pose2(MvNormal(x2x3_pp[1], Matrix(Diagonal(x2x3_pp[2].^2)))), autoinit=autoinit)

    br_calc = calcPosePointBearingRange(ground[:x3], landmarks_real[:l7])
    x3l7_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x3l7_br = [[deg2rad(132.4-90), 0.1], [141.32, 1]]
    @doline SKL addVariable!(fg, :l7, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l7_0, landmarks_design[:l7])
    @doline SKL addFactor!(fg, [:x3, :l7, :l7_0], Pose2Point2BearingRange(Normal(x3l7_br[1]...), Normal(x3l7_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

# ensureAllInitialized!(fg)
# x4 drive + 80
x3x4_pp = [[80,0,deg2rad(-90)], [pp_σx, pp_σy, pp_σψ]]
addVariable!(fg, :x4, Pose2)
addFactor!(fg, [:x3, :x4], Pose2Pose2(MvNormal(x3x4_pp[1], Matrix(Diagonal(x3x4_pp[2].^2)))), autoinit=autoinit)

# ensureAllInitialized!(fg)
# x5 turn -90 -> drive + 50
x4x5_pp = [[50,0,deg2rad(0)], [pp_σx, pp_σy, pp_σψ]]
addVariable!(fg, :x5, Pose2)
addFactor!(fg, [:x4, :x5], Pose2Pose2(MvNormal(x4x5_pp[1], Matrix(Diagonal(x4x5_pp[2].^2)))), autoinit=autoinit)

    br_calc = calcPosePointBearingRange(ground[:x5], landmarks_real[:l8])
    x5l8_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x5l8_br = [[deg2rad(-15.26), 0.1], [57.01, 1]]
    @doline SKL addVariable!(fg, :l8, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l8_0, landmarks_design[:l8])
    @doline SKL addFactor!(fg, [:x5, :l8, :l8_0], Pose2Point2BearingRange(Normal(x5l8_br[1]...), Normal(x5l8_br[2]...)), autoinit=autoinit, multihypo=[1.0;0.5;0.5])

    br_calc = calcPosePointBearingRange(ground[:x5], landmarks_real[:l10])
    x5l10_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x5l10_br = [[deg2rad(-6.84), 0.1], [105.84, 1]]
    @doline SKL addVariable!(fg, :l10, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l10_0, landmarks_design[:l10])
    @doline SKL addFactor!(fg, [:x5, :l10, :l10_0], Pose2Point2BearingRange(Normal(x5l10_br[1]...), Normal(x5l10_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

    br_calc = calcPosePointBearingRange(ground[:x5], landmarks_real[:l11])
    x5l11_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x5l11_br = [[deg2rad(-7.43), 0.1], [113.59, 1]]
    @doline SKL addVariable!(fg, :l11, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l11_0, landmarks_design[:l11])
    @doline SKL addFactor!(fg, [:x5, :l11, :l11_0], Pose2Point2BearingRange(Normal(x5l11_br[1]...), Normal(x5l11_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

#
drawGraph(fg)
# ensureAllInitialized!(fg)
# drawPosesLandms(fg)
# getSolverParams(fg).drawtree=true
# getSolverParams(fg).showtree=true
tree = solveTree!(fg);

pl = drawPosesLandms(fg, drawhist=true, contour=false)
addMaptoPlot!(pl)
pl.coord = Coord.Cartesian(xmin=100, xmax=500, ymin=600, ymax=900)
pl
# pl |> PDF(ENV["HOME"]*"/Downloads/explore1.pdf")

pl5 = drawLandms(fg, from=5,to=5, drawhist=true, contour=false)
addMaptoPlot!(pl5)
pl5.coord = getCoordCartesianFromKDERange(fg, :l5, digits=0)
pl5

# reportFactors(fg, Pose2Pose2)
#

# drawLandms(fg, to=1, drawhist=true, contour=false)

# plot the bimodal wall
plotVariable2D(fg, :l1, refs=[landmarks_design, landmarks_real])
plotVariable2D(fg, :l2, refs=[landmarks_design, landmarks_real])
plotVariable2D(fg, :l3, refs=[landmarks_design, landmarks_real])
plotVariable2D(fg, :l4, refs=[landmarks_design, landmarks_real])
plotVariable2D(fg, :l5, refs=[landmarks_design, landmarks_real])
# plot unimodals
plotVariable2D(fg, :l6, refs=[landmarks_design, landmarks_real])
plotVariable2D(fg, :l7, refs=[landmarks_design, landmarks_real])
plotVariable2D(fg, :l8, refs=[landmarks_design, landmarks_real])

reportFactors(fg, Pose2Pose2)

# plotLocalProduct(fg, :x4, levels=5)

# ensureAllInitialized!(fg)
# x6
x5x6_pp = [[0,0,deg2rad(-90)], [pp_σx, pp_σy, pp_σψ]]
addVariable!(fg, :x6, Pose2)
addFactor!(fg, [:x5, :x6], Pose2Pose2(MvNormal(x5x6_pp[1], Matrix(Diagonal(x5x6_pp[2].^2)))), autoinit=autoinit)

    br_calc = calcPosePointBearingRange(ground[:x6], landmarks_real[:l3])
    x6l3_br  = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x6l3_br = [[deg2rad(-64.29+90), 0.1], [145.07, 1]]
    # addVariable!(fg, :l3, Point2)
    @doline SKL addFactor!(fg, [:x6, :l3, :l3_0], Pose2Point2BearingRange(Normal(x6l3_br[1]...), Normal(x6l3_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

    br_calc = calcPosePointBearingRange(ground[:x6], landmarks_real[:l4])
    x6l4_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x6l4_br = [[deg2rad(-67.83+90), 0.1], [141.01, 1]]
    # addVariable!(fg, :l4, Point2)
    @doline SKL addFactor!(fg, [:x6, :l4, :l4_0], Pose2Point2BearingRange(Normal(x6l4_br[1]...), Normal(x6l4_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

## debug check solve here
# drawGraph(fg)

# ensureAllInitialized!(fg)
# drawPosesLandms(fg)
# getSolverParams(fg).drawtree=true
# getSolverParams(fg).showtree=true
tree = solveTree!(fg);
# tree = solveTree!(fg, tree);
pl = drawPosesLandms(fg, drawhist=true, contour=false)
addMaptoPlot!(pl)
# pl |> PDF(ENV["HOME"]*"/Downloads/explore2.pdf")

# x7
x6x7_pp = [[0,0,deg2rad(-90)], [pp_σx, pp_σy, pp_σψ]]
addVariable!(fg, :x7, Pose2)
addFactor!(fg, [:x6, :x7], Pose2Pose2(MvNormal(x6x7_pp[1], Matrix(Diagonal(x6x7_pp[2].^2)))), autoinit=autoinit)

    br_calc = calcPosePointBearingRange(ground[:x7], landmarks_real[:l7])
    x7l7_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x7l7_br = [[deg2rad(137-180), 0.1], [147.89, 1]]
    pp_x7l7_br = Pose2Point2BearingRange(Normal(x7l7_br[1]...), Normal(x7l7_br[2]...))
    @doline SKL addFactor!(fg, [:x7; :l7; :l7_0], pp_x7l7_br, autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

    br_calc = calcPosePointBearingRange(ground[:x7], landmarks_real[:l9])
    x7l9_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x7l9_br = [[deg2rad(144.46-180), 0.1], [42.04, 1]]
    @doline SKL addVariable!(fg, :l9, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l9_0, landmarks_design[:l9])
    @doline SKL addFactor!(fg, [:x7, :l9, :l9_0], Pose2Point2BearingRange(Normal(x7l9_br[1]...), Normal(x7l9_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

# drawGraph(fg)
# pts = approxConv(fg, :x7l7l7bf1, :l7)
# pts = approxConv(fg, :x7l7l7bf1, :l7b)

# ensureAllInitialized!(fg)
# x8
x7x8_pp = [[0,0,deg2rad(-90)], [pp_σx, pp_σy, pp_σψ]]
addVariable!(fg, :x8, Pose2)
addFactor!(fg, [:x7, :x8], Pose2Pose2(MvNormal(x7x8_pp[1], Matrix(Diagonal(x7x8_pp[2].^2)))), autoinit=autoinit)

    br_calc = calcPosePointBearingRange(ground[:x8], landmarks_real[:l12])
    x8l12_br = [[br_calc[1], br_σb], [br_calc[2], br_σρ]]
    # x8l12_br = [[deg2rad(53.53-90), 0.1], [133.32, 1]]
    @doline SKL addVariable!(fg, :l12, Point2)
    @doline SKL addLandmarkTrueWithPrior!(fg, :l12_0, landmarks_design[:l12])
    @doline SKL addFactor!(fg, [:x8, :l12, :l12_0], Pose2Point2BearingRange(Normal(x8l12_br[1]...), Normal(x8l12_br[2]...)), autoinit=autoinit, multihypo=[1.0;pMeas;pDesg])

# ensureAllInitialized!(fg)

# getSolverParams(fg).async = false
# tree = solveTree!(fg, tree);
tree = solveTree!(fg);

pl = drawPosesLandms(fg, drawhist=true, contour=false)
addMaptoPlot!(pl)
# pl |> PDF(ENV["HOME"]*"/Downloads/mapped.pdf")

pl = drawLandms(fg, drawhist=true, to=5, contour=false)
addMaptoPlot!(pl)
pl.coord = Coord.Cartesian(xmin=100, xmax=450, ymin=600, ymax=850)
pl
# pl |> PDF(ENV["HOME"]*"/Downloads/badwall.pdf")

drawGraph(fg)

# reportFactors(fg, Pose2Pose2);

## =============================================================================
## Deeper look at various other aspects.

# pl = plotVariable2D(fg, :l10)
#
# pl = drawLandms(fg, from=10, to=11, contour=false, drawhist=true)
# addMaptoPlot!(pl)
#
# pl.coord = getCoordCartesianFromKDERange(fg, :l10, digits=0, extend=10.0)
# pl

# ## quick check
#
# pl = drawLandms(fg, from=4, to=4, drawhist=true, contour=false)
# plotKDE(fg, :l4, levels=20)
# plotKDE(fg, :l4_0, levels=20)
# plotKDE(fg, [:l4;:l4_0])

##

#
# ## 3D VIS ==================
#
# using Amphitheatre
#
# fgmax = deepcopy(fg)
# fgmax.robotId = "Max"
#
# fgvis = BasicFactorGraphPose(fg, meanmax=:mean)
# fgvismax = BasicFactorGraphPose(fgmax, meanmax=:max)
#
# visdatasets = AbstractAmphitheatre[fgvis]
# push!(visdatasets, fgvismax)
# vis, vistask = visualize(visdatasets, start_browser=true)
#
# stopAmphiVis!()
# vistask = Amphitheatre.restartVisualize(vis, visdatasets)
#
# ## END VIS 3D

# plotVariable2D(fg, :l9, refs=[landmarks_design, landmarks_real])
# plotVariable2D(fg, :l10, refs=[landmarks_design, landmarks_real])
# plotVariable2D(fg, :l11, refs=[landmarks_design, landmarks_real])
# plotVariable2D(fg, :l12, refs=[landmarks_design, landmarks_real])
#
# plotLocalProduct(fg, :l3, levels=10)
# plotLocalProduct(fg, :l4, levels=10)
#
# # plotLocalProduct(fg, :x6, levels=10)
#
#
#
# syms = sortDFG(ls(fg, r"x"))
# ppes = Base.map((x)->calcVariablePPE(fg,x), syms)
# trivial_poses = Dict(syms .=> ppes)
#
# # syms = sortDFG(ls(fg, r"l\d*_"))
# # ppes = Base.map((x)->calcVariablePPE(fg,x), syms)
# # design_landms = Dict(syms .=> ppes)
# #
# # syms = sortDFG(setdiff(ls(fg, r"l\d*"),ls(fg, r"l\d*_")) )
# # ppes = Base.map((x)->calcVariablePPE(fg,x), syms)
# # design_mapped = Dict(syms .=> ppes)
# #
#
#
#
#
# ##
# p = DFGPlotProps(  (var=colorant"seagreen", fac=colorant"cyan3"),
#                             (var=1.0, fac=0.3),
#                             (var=:box, fac=:elipse),
#                             spring_layout,#spectral_layout
#                             true)
# dfgplot(fg,p)
#
#
#
# ##
#
# # using RoMEPlotting
#
# plotPose(fg, [:x0])
# plotPose(fg, [:x1])
# plotPose(fg, [:x2])
# plotPose(fg, [:x3])
# plotPose(fg, [:x0,:x1,:x2,:x3], levels=1)
#
#
# ##
#
# plotLocalProduct(fg, :x0)
# plotLocalProduct(fg, :x1)
# plotLocalProduct(fg, :x2)
#
# plotLocalProduct(fg, :l7, levels=3)
#
#
# plotLocalProduct(fg, :l1_0)
# plotLocalProduct(fg, :l2_0)
# plotLocalProduct(fg, :l3_0)
# plotLocalProduct(fg, :l4_0)
#
# plotLocalProduct(fg, :l4)
#
# ls(fg, :l7)
#
#
# drawPosesLandms(fg)

# ensureAllInitialized!(fg)
# cu, pr = predictVariableByFactor(fg, :l7, pp_x7l7_br, [:x7;:l7])
# if minkld(cu, pr) < 3
    # addFactor!(fg, [:x7, :l7], pp_x7l7_br, autoinit=autoinit)
# else
#     addVariable!(fg, :l7_1, Point2)
# end

##


text:
module VectorBackedLists

export list, insert_after!, move_before!, prev, sublist, advance

export start, next, done

struct Node{I}
    value::I    # index into list data
    next::I     # index into nodes of the next node
    prev::I     # index into nodes of the previous node
end

struct VectorBackedList{T,S<:AbstractVector{T},I}
    data::S
    nodes::Vector{Node{I}}
    head::I
    tail::I
end

Base.eltype(::VectorBackedList{T}) where {T} = T
start(list::VectorBackedList) = list.nodes[list.head].next
next(list::VectorBackedList, state) = (list.data[list.nodes[state].value], list.nodes[state].next)
done(list::VectorBackedList, state) = state == list.tail
function Base.iterate(list::VectorBackedList, state=start(list))
    done(list, state) && return nothing
    return next(list, state)
end

"""
    advance(iterable, state) -> next_state

Advances the iteration state without producing the associated value. It holds that

    next(iterable, state)[2] == advance(iterable, state)
"""
advance(list::VectorBackedList, state) = list.nodes[state].next

"""
done(iterable) -> state

Produces an iterator state for which `done(iterable,state) == true`. Cf. to the
c++ end() api.
"""
done(list::VectorBackedList) = list.tail
Base.setindex!(list::VectorBackedList{T,S,I}, v::T, state::I) where {T,S,I} = (list.data[list.nodes[state].value] = v)
Base.getindex(list::VectorBackedList{T,S,I}, state::I) where {T,S,I} = list.data[list.nodes[state].value]

Base.IteratorSize(list::VectorBackedList) = Base.SizeUnknown()
# Base.length(sl::VectorBackedList) = (n = 0; for x in sl; n += 1; end; n)

function sublist(ls::VectorBackedList{T,S,I}, b, e) where {T,S,I}
    VectorBackedList{T,S,I}(
        ls.data,
        ls.nodes,
        ls.nodes[b].prev,
        e
    )
end

"""
    prev(list, state) -> item, prevstate

Returns the current item from `list` and sets the state to point to the previous
entry. It hold that

```
_, p = prev(list, s)
_, n = next(list, p)
n == s
```
"""
prev(list, state) = (list.data[list.nodes[state].value], list.nodes[state].prev)

"""
    list(data, [index_type=eltype(eachindex(data))])

Create a list from an indexable container. The list provided a view on the container,
so any mutations realised through calling the list API will be reflected in the
state of the underlying container.

If memory consumption is a priority, the iterator type can be forced to a smaller
sized integer. Be careful: it is possible that the data passed is longer than the
largest value representable for the iterator type. In this case, the list implementation
fails quietly and its behaviour will be undefined.
"""
function list(data, index_type=eltype(eachindex(data)))

    T = eltype(data)
    S = typeof(data)
    I = index_type
    n = length(data)

    nodes = Vector{Node{I}}(undef, n+2)
    nodes[1] = Node{I}(0,2,0)
    for i in 2:n+1; nodes[i] = Node{I}(i-1, i+1, i-1); end
    nodes[end] = Node{I}(0,0,n+1)
    VectorBackedList{T, S, I}(data, nodes, 1, n+2)
end

"""
    move_before(list, item, dest)

Move the node pointed to by iterator `item` in fron of iterator `dest`.
"""
function move_before!(list, I, T)

    @assert I != T
    nodes = list.nodes

    # step 1: remove n
    _, P = prev(list,I);
    _, N = next(list,I);
    p = nodes[P]
    n = nodes[N]
    # @assert P == p.idx
    # @assert N == n.idx

    #nodes[P] = Node(p.value, n.idx, p.prev, p.idx)
    nodes[P] = Node(p.value, N, p.prev)
    # nodes[N] = Node(n.value, n.next, p.idx, n.idx)
    nodes[N] = Node(n.value, n.next, P)

    # step 2: reintroduce n
    _, Q = prev(list, T)
    i = nodes[I]
    t = nodes[T]
    q = nodes[Q]
    # @assert Q == q.idx
    # @assert T == t.idx
    # @assert I == i.idx

    nodes[Q] = Node(q.value, I, q.prev)
    nodes[T] = Node(t.value, t.next, I)
    nodes[I] = Node(i.value, T, Q)
    nothing
end

"""
    insert_after!(list, dest, value)

Insert `value` in `list` after the value pointed to by iterator `dest`.
"""
function insert_after!(list::VectorBackedList, T, v)

    data = list.data
    nodes = list.nodes

    push!(data, v)

    _, N = next(list, T)
    t = nodes[T]
    n = nodes[N]

    I = length(nodes)+1
    push!(nodes, Node(length(data), N, T))

    nodes[T] = Node(t.value, I, t.prev)
    nodes[N] = Node(n.value, n.next, I)
    nothing
end

function insert_before!(list::VectorBackedList, T, v)

    data = list.data
    nodes = list.nodes

    push!(data, v)

    P = list.nodes[T].prev
    t = nodes[T]
    p = nodes[P]

    I = length(nodes)+1
    push!(nodes, Node(length(data), T, P))

    nodes[T] = Node(t.value, t.next, I)
    nodes[P] = Node(p.value, I, p.prev)
    nothing
end

Base.push!(list::VectorBackedList, v) = insert_before!(list, done(list), v)

end # module

ext: jl


text:
using Plots

function rosenbrock(x; a=1, b=100)
    (a-x[1])^2 + b * (x[2] - x[1]^2)^2;
end

# plot

# contour(-2:0.1:2, -1:0.1:3, (x, y)->rosenbrock([x, y]))
heatmap(-2:0.01:2, -1:0.01:3, (x, y)->rosenbrock([x, y]))


text:
abstract type AbstractWorld end

Base.@kwdef mutable struct PrimitiveWorld{T₁ <: Vec3, T₂ <: AbstractVector, T₃ <: Plane} <: AbstractWorld
    xaxis::T₁ = Vec(1.0, 0.0, 0.0)
    yaxis::T₁ = Vec(0.0, 1.0, 0.0)
    zaxis::T₁ = Vec(0.0, 0.0, 1.0)
    #cameras::Vector{T₁} = [DigitalCamera(), DigitalCamera()]
    points::Vector{T₂} = [Point3(rand(-1000.0:1000.0), rand(-1000.0:1000.0), 100.0) for n = 1:5000]
    planes::Vector{T₃} = [Plane(Vec3(0.0, 0.0, 1.0), 100)]
end

function get_xaxis(world::AbstractWorld)
    world.xaxis
end

function set_xaxis!(world::AbstractWorld, xaxis::Vec3)
    world.xaxis = xaxis
end

function get_yaxis(world::AbstractWorld)
    world.yaxis
end

function set_yaxis!(world::AbstractWorld, yaxis::Vec3)
    world.yaxis = yaxis
end

function get_zaxis(world::AbstractWorld)
    world.zaxis
end

function set_zaxis!(world::AbstractWorld, zaxis::Vec3)
    world.zaxis = zaxis
end

# function get_cameras(world::AbstractWorld)
#     world.cameras
# end
#
# function set_cameras!(world::AbstractWorld, cameras::Vector{<: AbstractCamera})
#     world.cameras = cameras
# end

function get_points(world::AbstractWorld)
    world.points
end

function set_points!(world::AbstractWorld, points::Vector{<: AbstractVector})
    world.points = points
end

function get_planes(world::AbstractWorld)
    world.planes
end

function set_planes!(world::AbstractWorld, planes::Vector{<: Plane})
    world.planes = planes
end


text:
module Render
export rendertext, overlay!,
    shape, ellipse, box, squircle, star, ngon, ellipse_area, box_area, squircle_area, star_area, ngon_area,
    GIF, generate, parsecolor, rendertextoutlines,
    colorschemes, torgba, imagemask, outline, padding, dilate, imresize, recolor!, recolor
export issvg, save, load, tobitmap, SVGImageType, svgstring
using Luxor
using Colors
using ColorSchemes
using FileIO
import ImageTransformations.imresize

# because of float error, (randommask(color=Gray(0.3))|>tobitmap)[300,300]|>torgba != Gray(0.3)|>torgba
parsecolor(c) = ARGB{Colors.N0f8}(parse(Colorant, c))
parsecolor(tp::Tuple) = ARGB{Colors.N0f8}(tp...)
parsecolor(gray::Real) = ARGB{Colors.N0f8}(Gray(gray))
parsecolor(sc::Symbol) = parsecolor.(colorschemes[sc].colors)
parsecolor(sc::AbstractArray) = parsecolor.(sc)

issvg(d) = d isa Drawing && d.surfacetype == :svg
const SVGImageType = Drawing
Base.broadcastable(s::SVGImageType) = Ref(s)
Base.size(s::SVGImageType) = (s.height, s.width)
svgstring(d) = String(copy(d.bufferdata))

function loadsvg(svg)
    p = readsvg(svg)
    d = Drawing(p.width, p.height, :svg)
    placeimage(p)
    finish()
    d
end

function load(fn::AbstractString)
    if endswith(fn, r".svg|.SVG")
        loadsvg(fn)
    else
        FileIO.load(fn)
    end
end
function load(file::IO)
    try
        FileIO.load(file)
    catch
        seekstart(file)
        loadsvg(read(file, String))
    end
end
function tobitmap(svg::Drawing)
    Drawing(ceil(svg.width), ceil(svg.height), :image)
    placeimage(svg)
    m = image_as_matrix()
    finish()
    m
end

function boundbox(p::AbstractMatrix, bgcolor; border=0)
    a = c = 1
    b = d = 0
    while a < size(p, 1) && all(p[a,:] .== bgcolor)
        a += 1
    end
    while b < size(p, 1) && all(p[end - b,:] .== bgcolor)
        b += 1
    end
    a = max(1, a - border)
    b = min(size(p, 1), max(size(p, 1) - b + border, a))
    p = @view p[a:b, :]
    while c < size(p, 2) && all(p[:,c] .== bgcolor)
        c += 1
    end
    while d < size(p, 2) && all(p[:, end - d] .== bgcolor)
        d += 1
    end
    # @show a,b,c,d,border,bgcolor
    # @show c, d, p
    c = max(1, c - border)
    d = min(size(p, 2), max(size(p, 2) - d + border, c))
    return a, b, c, d
end

"a, b, c, d are all inclusive"
function clipsvg(m, a, b, c, d)
    m2 = Drawing(d - c + 1, b - a + 1, :svg)
    placeimage(m, Point(-c + 1, -a + 1))
    finish()
    m2
    end
"a, b, c, d are all inclusive"
clipbitmap(m, a, b, c, d) = m[a:b, c:d]

function drawtext(t, size, pos, angle=0, color="black", font="")
    setcolor(parsecolor(color))
    setfont(font, size)
    settext(t, Point(pos...), halign="center", valign="center"; angle=angle)
end

function rendertext(str::AbstractString, size::Real;
        pos=(0, 0), color="black", backgroundcolor=(0, 0, 0, 0), angle=0, font="", border=0, type=:bitmap)
    @assert type in (:svg, :bitmap, :both)
    l = length(str) + 1
    l = ceil(Int, size * l + 2border + 2)
    if type == :bitmap
        Drawing(l, l, :image)
    else
        svg = Drawing(l, l, :svg) # svg is slow
    end
    origin()
    bgcolor = parsecolor(backgroundcolor)
    bgcolor = background(bgcolor)

    drawtext(str, size, pos, angle, color, font)
    if type == :bitmap mat = image_as_matrix() end
    finish()
    if type != :bitmap mat = tobitmap(svg) end
    #     bgcolor = Luxor.ARGB32(bgcolor...) #https://github.com/JuliaGraphics/Luxor.jl/issues/107
    bgcolor = mat[1]
    box = boundbox(mat, bgcolor, border=border)
    mat = clipbitmap(mat, box...)
    if type == :bitmap
        return mat
    elseif type == :svg
        return clipsvg(svg, box...)
    else
        return mat, clipsvg(svg, box...)
    end
end

function rendertextoutlines(str::AbstractString, size::Real; color="black", bgcolor=(0, 0, 0, 0),
        linewidth=3, linecolor="white", font="")
    l = length(str)
    Drawing(ceil(Int, 2l * (size + 2linewidth) + 2), ceil(Int, 2 * (size + 2linewidth) + 2), :image)
    origin()
    bgcolor = parsecolor(bgcolor)
    bgcolor = background(bgcolor)
    # bgcolor = Luxor.ARGB32(bgcolor...)
    setcolor(parsecolor(color))
#     setfont(font, size)
    fontface(font)
    fontsize(size)
    setline(linewidth)
    textoutlines(str, O, :path, halign="center", valign="center")
    fillpreserve()
    setcolor(linecolor)
    strokepath()
    mat = image_as_matrix()
    finish()
    mat = clipbitmap(mat, boundbox(mat, mat[1])...)
end

function torgba(c)
    c = Colors.RGBA{Colors.N0f8}(parsecolor(c))
    rgba = (Colors.red(c), Colors.green(c), Colors.blue(c), Colors.alpha(c))
    reinterpret.(UInt8, rgba)
end
torgba(img::AbstractArray) = torgba.(img)
function _backgroundcolor(img, c=:auto)
    if c == :auto
        return img[1] == img[end] && any(c -> c != img[1], img) ? img[1] : (0, 0, 0, 0)
        else
    return c
    end
end
imagemask(img::AbstractArray{Bool,2}) = img
imagemask(img, istransparent::Function) = .!istransparent.(torgba.(img))
imagemask(img, transparent::AbstractArray{Bool,2}) = .!transparent
function imagemask(img, transparent=:auto)
    if transparent == :auto
        if img[1] == img[end] && any(c -> c != img[1], img)
            transparent = img[1]
        else
            transparent = nothing
        end
    end
    if transparent === nothing
        return trues(size(img))
    end
    img .!= convert(eltype(img), parsecolor(transparent))
end

function dilate(mat, r)
    r == 0 && return mat
    mat2 = copy(mat)
    mat2[1:end - r, :] .|= mat[1 + r:end, :]
    mat2[1 + r:end, : ] .|= mat[1:end - r, :]
    mat2[:, 1:end - r] .|= mat[:, 1 + r:end]
    mat2[:, 1 + r:end] .|= mat[:, 1:end - r]

    mat2[1:end - r, 1:end - r] .|= mat[1 + r:end, 1 + r:end]
    mat2[1 + r:end, 1 + r:end ] .|= mat[1:end - r, 1:end - r]
    mat2[1:end - r, 1 + r:end ] .|= mat[1 + r:end, 1:end - r]
    mat2[1 + r:end, 1:end - r ] .|= mat[1:end - r, 1 + r:end]
    mat2
end

function dilate2(mat, r; smoothness=0.5) # better and slower
    @assert smoothness >= 0
    m = zeros(size(mat) .+ 2)
    m[2:end - 1, 2:end - 1] .= mat
    # 立方 ∫∫ x^2+y^2 dx dy
    s = max(7, 171 * smoothness) # 13*4+7*4+91, smoothness是平滑系数，0-1，越大越圆但边缘越模糊，越小越方但边缘越清晰
    # s < 7 无意义，反而增加溢出风险
    # 权重 1/1 : 1/13 : 1/7
    o = 91 / s # 1 / ((0.5^3-(-0.5)^3) * 2)
    p = 7 / s # 1 / ((1.5^3-0.5^3) * 2)
    q = 13 / s # 1 / ((1.5^3-0.5^3) + (0.5^3-(-0.5)^3))

    for _ in 1:r
        @views m[2:end - 1, 2:end - 1] .= (
            o * m[2:end - 1, 2:end - 1] .+ # 中

            q * m[1:end - 2, 2:end - 1] .+ q * m[3:end, 2:end - 1] .+ # 上下
            q * m[2:end - 1, 1:end - 2] .+ q * m[2:end - 1, 3:end] .+ # 左右

            p * m[1:end - 2, 1:end - 2] .+ p * m[3:end, 3:end] .+ # 主对角
            p * m[1:end - 2, 3:end] .+ p * m[3:end, 1:end - 2] # 副对角
        )
    end
    return min.(1., m[2:end - 1, 2:end - 1])
end
"""
img: a bitmap image
linewidth: 0 <= linewidth
color: line color
transparent: color of the background
smoothness: 0 <= smoothness <= 1
"""
function outline(img; transparent=:auto, color="black", linewidth=2, smoothness=0.5)
    @assert linewidth >= 0
    mask = imagemask(img, transparent)
    r = 2 * linewidth * smoothness
    # @show r
    mask2 = dilate2(mask, max(linewidth, round(r)), smoothness=smoothness)
    c = ARGB(parsecolor(color)) # https://github.com/JuliaGraphics/Colors.jl/issues/500
    bg = convert.(eltype(img), coloralpha.(c, mask2))
    bg = overlay!(copy(img), bg)
    @views bg[mask] .= overlay.(bg[mask], img[mask])
    bg
end

function padding(img::AbstractMatrix, r=maximum(size(img)) ÷ 10; backgroundcolor=:auto)
    color = convert(eltype(img), parsecolor(_backgroundcolor(img, backgroundcolor)))
    r = round.(Int, r)
    bg = fill(color, size(img) .+ 2 .* r)
    overlay!(bg, img, reverse((0, 0) .+ r)...)
end
function padding(img::SVGImageType, r=maximum(size(img)) ÷ 10; backgroundcolor=(0, 0, 0, 0))
    color = parsecolor(backgroundcolor)
    sz = size(img) .+ 2 .* round.(Int, r)
    m2 = Drawing(reverse(sz)..., :svg)
    origin()
    background(color)
    placeimage(img, centered=true)
    finish()
    m2
end
"return the overlapping view of img1 and img2 when img2's top left corner at img1's (x, y)"
function overlappingarea(img1, img2, x=1, y=1)
    h1, w1 = size(img1)
    h2, w2 = size(img2)
    img1v = @view img1[max(1, y):min(h1, y + h2 - 1), max(1, x):min(w1, x + w2 - 1)]
    img2v = @view img2[max(1, -y + 2):min(h2, -y + h1 + 1), max(1, -x + 2):min(w2, -x + w1 + 1)]
    img1v, img2v
end

function overlay(color1::TransparentRGB, color2::TransparentRGB)
#     @show color1, color2
    a2 = Colors.alpha(color2)
    if a2 == 0 return color1 end
    if a2 == 1 return color2 end
    a1 = Colors.alpha(color1) |> Float64
    c1 = [Colors.red(color1), Colors.green(color1), Colors.blue(color1)]
    c2 = [Colors.red(color2), Colors.green(color2), Colors.blue(color2)]
    a = a1 + a2 - a1 * a2
    c = (c1 .* a1 .* (1 - a2) .+ c2 .* a2) ./ ifelse(a > 0, a, 1)
#     @show c, a
    typeof(color1)(min.(1, c)..., min(1, a))
end
"put img2 on img1 at (x, y)"
function overlay!(img1::AbstractMatrix, img2::AbstractMatrix, x=1, y=1)# 左上角重合时(x=1,y=1)
    img1v, img2v = overlappingarea(img1, img2, x, y)
#     @show (h1, w1),(h2, w2),(x,y)
    img1v .= overlay.(img1v, img2v)
    img1
end
function overlay!(img::AbstractMatrix, imgs, pos)
    for (i, p) in zip(imgs, pos)
#         @show pos
        overlay!(img, i, p...)
    end
    img
end

function overlay(imgs, poss; backgroundcolor=(1, 1, 1, 0), size=size(imgs[1]))
    d = Drawing(size..., :svg)
    Luxor.background(parsecolor(backgroundcolor))
    placeimage.(imgs, (Point(x - 1, y - 1) for (x, y) in poss))# (x,y)=(1,1)时左上角重合，此时Point(0,0)
    finish()
    d
end
function recolor!(img::AbstractArray, color)
    c = parsecolor(color)
    img .= convert.(eltype(img), Colors.alphacolor.(c, Colors.alpha.(img)))
end
function recolor(img::AbstractArray, color)
    c = parsecolor(color)
    convert.(eltype(img), Colors.alphacolor.(c, Colors.alpha.(img)))
end

function squircle(pos, w, h, args...; kargs...)
    Luxor.squircle(pos, w / 2, h / 2, args...; kargs...)
end
function box(pos, w, h, args...; cornerradius=0, kargs...)
    Luxor.box(pos, w, h, cornerradius, args...; kargs...)
end
function ngon(pos, w, h, args...; npoints=5, orientation=0, kargs...)
    r = min(w, h) / 2
    orientation = orientation -π / 2 # 尖朝上
    Luxor.ngon(pos, r, npoints, orientation, args...; kargs...)
end
function star(pos, w, h, args...; npoints=5, starratio=0.5, orientation=0, kargs...)
    r = min(w, h) / 2
    orientation = orientation -π / 2 # 尖朝上
    Luxor.star(pos, r, npoints, starratio, orientation, args...; kargs...)
end

ellipse_area(h, w) = π*h*w/4
function box_area(h, w; cornerradius=0)
    r = cornerradius
    @assert min(h,w) >= 2r
    h*w + (π-4)*r*r
end
function squircle_area(h, w; rt)
    @assert rt < 100
    h * w * (gamma(1+rt/2))^2 / gamma(1+rt)
end
gamma(z) = √(2π/z) * (1/ℯ*(z + 1 / (12z - 1/(10z))))^z
function ngon_area(h, w; npoints=5)
    r = min(w, h) / 2
    θ = 2π / npoints
    (r * r * sin(θ)) * npoints / 2
end
function star_area(h, w; npoints=5, starratio=0.5)
    r = min(w, h) / 2
    r2 = r * starratio
    θ = π / npoints
    (r * r2 * sin(θ)) * npoints
end

"""
generate a box, ellipse, squircle, ngon or star svg image
## Examples
* shape(box, 80, 50) #80*50 box
* shape(box, 80, 50, cornerradius=4) #box with cornerradius=4
* shape(squircle, 80, 50, rt=0.7) #squircle or superellipse. rt=0, rectangle; rt=1, ellipse; rt=2, rhombus
* shape(ngon, 120, 100, npoints=12, orientation=π/6) #regular dodecagon (12 corners) oriented by π/6
* shape(star, 120, 100, npoints=5) #pentagram (5 tips)
* shape(star, 120, 100, npoints=5, starratio=0.7, orientation=π/2) #0.7 specifies the ratio of the smaller radius of the star and the larger; oriented by π/2
* shape(ellipse, 80, 50, color="red") #80*50 red ellipse
* shape(box, 80, 50, backgroundcolor=(0,1,0), backgroundsize=(100, 100)) #80*50 box on 100*100 green background
* shape(squircle, 80, 50, outline=3, linecolor="red", backgroundcolor="gray") #add a red outline to the squircle
outline: an Integer
padding: an Integer or a tuple of two Integers
backgroundsize: a tuple of two Integers
color, linecolor, backgroundcolor: anything that can be parsed to a color
"""
function shape(shape_, width, height, args...;
    outline=0, linecolor="black", padding=0,
    color="white", backgroundcolor=(0, 0, 0, 0), backgroundsize=(width + 2outline, height + 2outline) .+ 2 .* padding,
    kargs...)
    d = Drawing(ceil.(backgroundsize)..., :svg)
    origin()
    background(parsecolor(backgroundcolor))
    if outline > 0
        setline(outline)
        setcolor(parsecolor(linecolor))
        shape_(Point(0, 0), width, height, args...; action=:stroke, kargs...)
    end
    setcolor(parsecolor(color))
    shape_(Point(0, 0), width, height, args...; action=:fill, kargs...)
    finish()
    d
end

using Printf
function try_gif_gen(gifdirectory; framerate=4)
    try
        pipeline(`ffmpeg -f image2 -i $(gifdirectory)/%010d.png -vf
            palettegen -y $(gifdirectory)/palette.png`, stdout=devnull, stderr=devnull) |> run
        pipeline(`ffmpeg -framerate $(framerate) -f image2 -i $(gifdirectory)/%010d.png
            -i $(gifdirectory)/palette.png -lavfi paletteuse -y $(gifdirectory)/animation.gif`,
            stdout=devnull, stderr=devnull) |> run
        try rm("$(gifdirectory)/palette.png", force=true) catch end
    catch e
        @warn "You need to have FFmpeg manually installed to use this function."
        @warn e
    end
end
struct GIF
    counter::Base.Iterators.Stateful{UnitRange{Int64},Union{Nothing,Tuple{Int64,Int64}}}
    directory::String
end
function GIF(directory)
    ispath(directory) && @warn "Directory `$directory` already exists."
    mkpath(directory)
    GIF(Iterators.Stateful(0:typemax(Int)), directory)
end
Base.push!(gif::GIF, img) = save(gif.directory * @sprintf("/%010d.png", popfirst!(gif.counter)), img)
(gif::GIF)(img) = Base.push!(gif, img)
generate(gif::GIF, args...; kargs...) = try_gif_gen(gif.directory, args...; kargs...)
end


text:
module KernelMachines

using Base: tail
using LinearAlgebra: dot, I
using Statistics: mean
using ChainRulesCore: NO_FIELDS
using Zygote: pullback
using Optim: optimize,
             minimizer,
             OptimizationResults,
             only_fg!,
             ConjugateGradient,
             AbstractOptimizer,
             default_options,
             Options

import ChainRulesCore: rrule
import StatsBase: fit!, predict

export KernelMachine, KernelRegression, KernelMachineRegression

export additivegaussiankernel, multiplicativegaussiankernel, gaussiankernel

export fit!, predict

include("utils.jl")
include("kernels.jl")
include("kernelmachine.jl")
include("regression.jl")
include("machineregression.jl")

end # module


text:
import CSV
import DataFrames; const df = DataFrames
import Plots; const pl = Plots
import ColorSchemes; const cs = ColorSchemes
pl.plotly()

# data = CSV.read("/home/tobias/doc/students/Bachelor_Michelle-Tavernaro/Results/output/startposition.csv")
data = CSV.read("/home/tobias/doc/students/Bachelor_Michelle-Tavernaro/Results/neu/zweiSpieler/startposition/dreieck12.csv")

filter!(data) do x
    x.Linear < 4.4
end

colorlist = cs.get(reverse(cs.inferno), data.Stufen, :extrema);
# colorlist = ["#e6194b", "#3cb44b", "#ffe119", "#4363d8", "#f58231", "#911eb4", "#46f0f0", "#f032e6", "#bcf60c", "#fabebe", "#008080", "#e6beff", "#9a6324", "#fffac8", "#800000", "#aaffc3", "#808000", "#ffd8b1", "#000075", "#808080", "#ffffff", "#00000"]
line = 1; cl = data[line, :]; col = colorlist[line];

plt = pl.plot(vcat(cl.x1, cl.x2, cl.x3), vcat(cl.y1, cl.y2, cl.y3), color=col, xlim=(0.0, 4.5), ylim=(0.0, 6.0), markershape=:auto)
for line in 2:df.nrow(data)#(line+1):(line+7 > df.nrow(data) ? df.nrow(data) : line+7)#2:df.nrow(data)
    cl = data[line, :]; col = colorlist[line]
    x_jit, y_jit = rand(2)/10
    pl.plot!(plt, vcat(cl.x1, cl.x2, cl.x3).+x_jit, vcat(cl.y1, cl.y2, cl.y3).+y_jit, color=col, xlim=(0.0, 4.5), ylim=(0.0, 6.0), markershape=:auto)
end
pl.gui(plt)


text:
@testset "4.2.8 (a+b cos)^m (c+d trig)^n" begin
    (A, B, C, a, b, c, d, e, f, x, ) = @variables A B C a b c d e f x

    #= ::Package:: =#

    #= ::Title:: =#
    #=Integrands*of*the*form*(a+b*cos(c+d*x))^m*(A+B*Trig(c+d*x))=#

    #= ::Section::Closed:: =#
    #=Integrands*of*the*form*(a+b*cos(c+d*x))^m*(A+B*sin(c+d*x))=#

    @test_int [(A + B*sin(x))/(a + b*cos(x)), x, 6, (2*A*atan((sqrt(a - b)*tan(x/2))/sqrt(a + b)))/(sqrt(a - b)*sqrt(a + b)) - (B*log(a + b*cos(x)))/b]

    @test_int [(A + B*sin(x))/(1 + cos(x)), x, 5, (-B)*log(1 + cos(x)) + (A*sin(x))/(1 + cos(x))]
    @test_int [(A + B*sin(x))/(1 - cos(x)), x, 5, B*log(1 - cos(x)) - (A*sin(x))/(1 - cos(x))]

    @test_int [(b + c + sin(x))/(a + b*cos(x)), x, 6, (2*(b + c)*atan((sqrt(a - b)*tan(x/2))/sqrt(a + b)))/(sqrt(a - b)*sqrt(a + b)) - log(a + b*cos(x))/b]
    @test_int [(b + c + sin(x))/(a - b*cos(x)), x, 6, (2*(b + c)*atan((sqrt(a + b)*tan(x/2))/sqrt(a - b)))/(sqrt(a - b)*sqrt(a + b)) + log(a - b*cos(x))/b]

    #= ::Section::Closed:: =#
    #=Integrands*of*the*form*(a+b*cos(c+d*x))^m*(A+B*tan(c+d*x))=#

    @test_int [(A + B*tan(x))/(a + b*cos(x)), x, 8, (2*A*atan((sqrt(a - b)*tan(x/2))/sqrt(a + b)))/(sqrt(a - b)*sqrt(a + b)) - (B*log(cos(x)))/a + (B*log(a + b*cos(x)))/a]

    #= ::Section::Closed:: =#
    #=Integrands*of*the*form*(a+b*cos(c+d*x))^m*(A+B*cot(c+d*x))=#

    @test_int [(A + B*cot(x))/(a + b*cos(x)), x, 7, (2*A*atan((sqrt(a - b)*tan(x/2))/sqrt(a + b)))/(sqrt(a - b)*sqrt(a + b)) + (B*log(1 - cos(x)))/(2*(a + b)) + (B*log(1 + cos(x)))/(2*(a - b)) - (a*B*log(a + b*cos(x)))/(a^2 - b^2)]

    #= ::Section:: =#
    #=Integrands*of*the*form*(a+b*cos(c+d*x))^m*(A+B*sec(c+d*x))=#

    #= ::Section::Closed:: =#
    #=Integrands*of*the*form*(a+b*cos(c+d*x))^m*(A+B*csc(c+d*x))=#

    @test_int [(A + B*csc(x))/(a + b*cos(x)), x, 11, (2*A*atan((sqrt(a - b)*tan(x/2))/sqrt(a + b)))/(sqrt(a - b)*sqrt(a + b)) + (B*log(1 - cos(x)))/(2*(a + b)) - (B*log(1 + cos(x)))/(2*(a - b)) + (b*B*log(a + b*cos(x)))/(a^2 - b^2)]

    #= ::Title:: =#
    #=Integrands*of*the*form*(a+b*cos(e+f*x))^m*(c+d*Trig(e+f*x))^n=#

    #= ::Section::Closed:: =#
    #=Integrands*of*the*form*(a+b*cos(e+f*x))^m*(c+d*sec(e+f*x))^n=#

    @test_int [(c + d*sec(e + f*x))^4/(a + b*cos(e + f*x)), x, 12, (2*(a*c - b*d)^4*atan((sqrt(a - b)*tan((1/2)*(e + f*x)))/sqrt(a + b)))/(a^4*sqrt(a - b)*sqrt(a + b)*f) + (d^3*(4*a*c - b*d)*atanh(sin(e + f*x)))/(2*a^2*f) + (d*(2*a*c - b*d)*(2*a^2*c^2 - 2*a*b*c*d + b^2*d^2)*atanh(sin(e + f*x)))/(a^4*f) + (d^4*tan(e + f*x))/(a*f) + (d^2*(6*a^2*c^2 - 4*a*b*c*d + b^2*d^2)*tan(e + f*x))/(a^3*f) + (d^3*(4*a*c - b*d)*sec(e + f*x)*tan(e + f*x))/(2*a^2*f) + (d^4*tan(e + f*x)^3)/(3*a*f)]
    @test_int [(c + d*sec(e + f*x))^3/(a + b*cos(e + f*x)), x, 10, (2*(a*c - b*d)^3*atan((sqrt(a - b)*tan((1/2)*(e + f*x)))/sqrt(a + b)))/(a^3*sqrt(a - b)*sqrt(a + b)*f) + (d^3*atanh(sin(e + f*x)))/(2*a*f) + (d*(3*a^2*c^2 - 3*a*b*c*d + b^2*d^2)*atanh(sin(e + f*x)))/(a^3*f) + (d^2*(3*a*c - b*d)*tan(e + f*x))/(a^2*f) + (d^3*sec(e + f*x)*tan(e + f*x))/(2*a*f)]
    @test_int [(c + d*sec(e + f*x))^2/(a + b*cos(e + f*x)), x, 8, (2*(a*c - b*d)^2*atan((sqrt(a - b)*tan((1/2)*(e + f*x)))/sqrt(a + b)))/(a^2*sqrt(a - b)*sqrt(a + b)*f) + (d*(2*a*c - b*d)*atanh(sin(e + f*x)))/(a^2*f) + (d^2*tan(e + f*x))/(a*f)]
    @test_int [(c + d*sec(e + f*x))^1/(a + b*cos(e + f*x)), x, 5, (2*(a*c - b*d)*atan((sqrt(a - b)*tan((1/2)*(e + f*x)))/sqrt(a + b)))/(a*sqrt(a - b)*sqrt(a + b)*f) + (d*atanh(sin(e + f*x)))/(a*f)]
    @test_int [1/((a + b*cos(e + f*x))*(c + d*sec(e + f*x))^1), x, 6, (2*a*atan((sqrt(a - b)*tan((1/2)*(e + f*x)))/sqrt(a + b)))/(sqrt(a - b)*sqrt(a + b)*(a*c - b*d)*f) - (2*d*atanh((sqrt(c - d)*tan((1/2)*(e + f*x)))/sqrt(c + d)))/(sqrt(c - d)*sqrt(c + d)*(a*c - b*d)*f)]
    @test_int [1/((a + b*cos(e + f*x))*(c + d*sec(e + f*x))^2), x, 7, (2*a^2*atan((sqrt(a - b)*tan((1/2)*(e + f*x)))/sqrt(a + b)))/(sqrt(a - b)*sqrt(a + b)*(a*c - b*d)^2*f) - (2*d*(2*a*c^2 - b*c*d - a*d^2)*atanh((sqrt(c - d)*tan((1/2)*(e + f*x)))/sqrt(c + d)))/((c - d)^(3/2)*(c + d)^(3/2)*(a*c - b*d)^2*f) + (d^2*sin(e + f*x))/((a*c - b*d)*(c^2 - d^2)*f*(d + c*cos(e + f*x)))]
    @test_int [1/((a + b*cos(e + f*x))*(c + d*sec(e + f*x))^3), x, 16, (2*a^3*atan((sqrt(a - b)*tan((1/2)*(e + f*x)))/sqrt(a + b)))/(sqrt(a - b)*sqrt(a + b)*(a*c - b*d)^3*f) - (2*d^3*(3*a*c - 2*b*d)*atanh((sqrt(c - d)*tan((1/2)*(e + f*x)))/sqrt(c + d)))/(c^2*(c - d)^(3/2)*(c + d)^(3/2)*(a*c - b*d)^2*f) - (d^3*(c^2 + 2*d^2)*atanh((sqrt(c - d)*tan((1/2)*(e + f*x)))/sqrt(c + d)))/(c^2*(c - d)^(5/2)*(c + d)^(5/2)*(a*c - b*d)*f) - (2*d*(3*a^2*c^2 - 3*a*b*c*d + b^2*d^2)*atanh((sqrt(c - d)*tan((1/2)*(e + f*x)))/sqrt(c + d)))/(c^2*sqrt(c - d)*sqrt(c + d)*(a*c - b*d)^3*f) - (d^3*sin(e + f*x))/(2*c*(a*c - b*d)*(c^2 - d^2)*f*(d + c*cos(e + f*x))^2) + (3*d^4*sin(e + f*x))/(2*c*(a*c - b*d)*(c^2 - d^2)^2*f*(d + c*cos(e + f*x))) + (d^2*(3*a*c - 2*b*d)*sin(e + f*x))/(c*(a*c - b*d)^2*(c^2 - d^2)*f*(d + c*cos(e + f*x)))]

    #= [(c + d*sec(e + f*x))^(5/2)/(a + b*cos(e + f*x)), x, 0, 0] =#
    #= [(c + d*sec(e + f*x))^(3/2)/(a + b*cos(e + f*x)), x, 0, 0] =#
    @test_int [(c + d*sec(e + f*x))^(1/2)/(a + b*cos(e + f*x)), x, 4, (2*sqrt(c + d)*cot(e + f*x)*Elliptic.F(asin(sqrt(c + d*sec(e + f*x))/sqrt(c + d)), (c + d)/(c - d))*sqrt((d*(1 - sec(e + f*x)))/(c + d))*sqrt(-((d*(1 + sec(e + f*x)))/(c - d))))/(a*f) + (2*(a*c - b*d)*Elliptic.Pi((2*a)/(a + b), asin(sqrt(1 - sec(e + f*x))/sqrt(2)), (2*d)/(c + d))*sqrt((c + d*sec(e + f*x))/(c + d))*tan(e + f*x))/(a*(a + b)*f*sqrt(c + d*sec(e + f*x))*sqrt(-tan(e + f*x)^2))]
    @test_int [1/((a + b*cos(e + f*x))*(c + d*sec(e + f*x))^(1/2)), x, 2, (2*Elliptic.Pi((2*a)/(a + b), asin(sqrt(1 - sec(e + f*x))/sqrt(2)), (2*d)/(c + d))*sqrt((c + d*sec(e + f*x))/(c + d))*tan(e + f*x))/((a + b)*f*sqrt(c + d*sec(e + f*x))*sqrt(-tan(e + f*x)^2))]
    #= [1/((a + b*cos(e + f*x))*(c + d*sec(e + f*x))^(3/2)), x, 0, 0] =#
    #= [1/((a + b*cos(e + f*x))*(c + d*sec(e + f*x))^(5/2)), x, 0, 0] =#

    #= ::Title:: =#
    #=Integrands*of*the*form*(a+b*cos(c+d*x))^m*(A+B*cos(c+d*x)+C*sin(c+d*x)^2)=#

    #= ::Section::Closed:: =#
    #=Integrands*of*the*form*(a+b*cos(c+d*x))^m*(A+B*cos(c+d*x)+C*sin(c+d*x)^2)=#

    @test_int [(A + B*cos(d + e*x) + C*sin(d + e*x))/(a + b*cos(d + e*x))^1, x, 6, (B*x)/b + (2*(A*b - a*B)*atan((sqrt(a - b)*tan((1/2)*(d + e*x)))/sqrt(a + b)))/(sqrt(a - b)*b*sqrt(a + b)*e) - (C*log(a + b*cos(d + e*x)))/(b*e)]
    @test_int [(A + B*cos(d + e*x) + C*sin(d + e*x))/(a + b*cos(d + e*x))^2, x, 7, (2*(a*A - b*B)*atan((sqrt(a - b)*tan((1/2)*(d + e*x)))/sqrt(a + b)))/((a - b)^(3/2)*(a + b)^(3/2)*e) + C/(b*e*(a + b*cos(d + e*x))) - ((A*b - a*B)*sin(d + e*x))/((a^2 - b^2)*e*(a + b*cos(d + e*x)))]
    @test_int [(A + B*cos(d + e*x) + C*sin(d + e*x))/(a + b*cos(d + e*x))^3, x, 8, ((2*a^2*A + A*b^2 - 3*a*b*B)*atan((sqrt(a - b)*tan((1/2)*(d + e*x)))/sqrt(a + b)))/((a - b)^(5/2)*(a + b)^(5/2)*e) + C/(2*b*e*(a + b*cos(d + e*x))^2) - ((A*b - a*B)*sin(d + e*x))/(2*(a^2 - b^2)*e*(a + b*cos(d + e*x))^2) - ((3*a*A*b - a^2*B - 2*b^2*B)*sin(d + e*x))/(2*(a^2 - b^2)^2*e*(a + b*cos(d + e*x)))]
    @test_int [(A + B*cos(d + e*x) + C*sin(d + e*x))/(a + b*cos(d + e*x))^4, x, 9, ((2*a^3*A + 3*a*A*b^2 - 4*a^2*b*B - b^3*B)*atan((sqrt(a - b)*tan((1/2)*(d + e*x)))/sqrt(a + b)))/((a - b)^(7/2)*(a + b)^(7/2)*e) + C/(3*b*e*(a + b*cos(d + e*x))^3) - ((A*b - a*B)*sin(d + e*x))/(3*(a^2 - b^2)*e*(a + b*cos(d + e*x))^3) - ((5*a*A*b - 2*a^2*B - 3*b^2*B)*sin(d + e*x))/(6*(a^2 - b^2)^2*e*(a + b*cos(d + e*x))^2) - ((11*a^2*A*b + 4*A*b^3 - 2*a^3*B - 13*a*b^2*B)*sin(d + e*x))/(6*(a^2 - b^2)^3*e*(a + b*cos(d + e*x)))]
    end

max_stars_repo_path: test/4 Trig functions/4.2 Cosine/4.2.8 (a+b cos)^m (c+d trig)^n.jl


text:
function convert_func(
  op::String,
  @nospecialize(T1),
  W1::Int,
  @nospecialize(T2),
  W2::Int = W1,
)
  typ1 = LLVM_TYPES[T1]
  typ2 = LLVM_TYPES[T2]
  vtyp1 = vtype(W1, typ1)
  vtyp2 = vtype(W2, typ2)
  instrs = """
  %res = $op $vtyp2 %0 to $vtyp1
  ret $vtyp1 %res
  """
  quote
    $(Expr(:meta, :inline))
    Vec($LLVMCALL($instrs, _Vec{$W1,$T1}, Tuple{_Vec{$W2,$T2}}, data(v)))
  end
end
# For bitcasting between signed and unsigned integers (LLVM does not draw a distinction, but they're separate in Julia)
function identity_func(W, T1, T2)
  vtyp1 = vtype(W, LLVM_TYPES[T1])
  instrs = "ret $vtyp1 %0"
  quote
    $(Expr(:meta, :inline))
    Vec($LLVMCALL($instrs, _Vec{$W,$T1}, Tuple{_Vec{$W,$T2}}, data(v)))
  end
end

### `vconvert(::Type{<:AbstractSIMDVector}, x)` methods
### These are the critical `vconvert` methods; scalar and `VecUnroll` are implemented with respect to them.
if (Sys.ARCH === :x86_64) || (Sys.ARCH === :i686)
  @generated function _vconvert(
    ::Type{Vec{W,F}},
    v::Vec{W,T},
    ::True,
  ) where {W,F<:FloatingTypes,T<:IntegerTypesHW}
    convert_func(T <: Signed ? "sitofp" : "uitofp", F, W, T)
  end
  @inline reinterpret_half(v::AbstractSIMD{W,Int64}) where {W} = reinterpret(Int32, v)
  @inline reinterpret_half(v::AbstractSIMD{W,UInt64}) where {W} = reinterpret(UInt32, v)
  @inline function _vconvert(::Type{Vec{W,F}}, v::VecUnroll, ::True) where {W,F}
    VecUnroll(fmap(_vconvert, Vec{W,F}, getfield(v, :data), True()))
  end
  @inline function _vconvert(
    ::Type{Vec{W,F}},
    v::AbstractSIMD{W,I},
    ::False,
  ) where {W,F,I<:Union{Int64,UInt64}}
    v32 = reinterpret_half(v)
    vl = extractlower(v32)
    vu = extractupper(v32)
    vfmadd_fast(
      F(4.294967296e9),
      _vconvert(Vec{W,F}, vl, True()),
      _vconvert(Vec{W,F}, vu % UInt32, True()),
    )
  end
  @inline function vconvert(
    ::Type{Vec{W,F}},
    v::Vec{W,T},
  ) where {W,F<:FloatingTypes,T<:IntegerTypesHW}
    _vconvert(Vec{W,F}, v, True())
  end
  @inline function vconvert(
    ::Type{Vec{W,F}},
    v::Vec{W,T},
  ) where {W,F<:FloatingTypes,T<:Union{UInt64,Int64}}
    _vconvert(
      Vec{W,F},
      v,
      has_feature(Val(:x86_64_avx512dq)) | (!has_feature(Val(:x86_64_avx2))),
    )
  end
  @inline function vconvert(
    ::Type{F},
    v::VecUnroll{N,W,T,Vec{W,T}},
  ) where {N,W,F<:FloatingTypes,T<:Union{UInt64,Int64}}
    _vconvert(
      Vec{W,F},
      v,
      has_feature(Val(:x86_64_avx512dq)) | (!has_feature(Val(:x86_64_avx2))),
    )
  end
  @inline function vconvert(
    ::Type{Vec{W,F}},
    v::VecUnroll{N,W,T,Vec{W,T}},
  ) where {N,W,F<:FloatingTypes,T<:Union{UInt64,Int64}}
    _vconvert(
      Vec{W,F},
      v,
      has_feature(Val(:x86_64_avx512dq)) | (!has_feature(Val(:x86_64_avx2))),
    )
  end
  @inline function vconvert(
    ::Type{VecUnroll{N,W,F,Vec{W,F}}},
    v::VecUnroll{N,W,T,Vec{W,T}},
  ) where {N,W,F<:FloatingTypes,T<:Union{UInt64,Int64}}
    _vconvert(
      Vec{W,F},
      v,
      has_feature(Val(:x86_64_avx512dq)) | (!has_feature(Val(:x86_64_avx2))),
    )
  end
else
  @generated function vconvert(
    ::Type{Vec{W,F}},
    v::Vec{W,T},
  ) where {W,F<:FloatingTypes,T<:IntegerTypesHW}
    convert_func(T <: Signed ? "sitofp" : "uitofp", F, W, T)
  end
end

@generated function vconvert(
  ::Type{Vec{W,T}},
  v::Vec{W,F},
) where {W,F<:FloatingTypes,T<:IntegerTypesHW}
  convert_func(T <: Signed ? "fptosi" : "fptoui", T, W, F)
end
@generated function vconvert(
  ::Type{Vec{W,T1}},
  v::Vec{W,T2},
) where {W,T1<:IntegerTypesHW,T2<:IntegerTypesHW}
  sz1 = sizeof(T1)::Int
  sz2 = sizeof(T2)::Int
  if sz1 < sz2
    convert_func("trunc", T1, W, T2)
  elseif sz1 == sz2
    identity_func(W, T1, T2)
  else
    convert_func(((T1 <: Signed) && (T2 <: Signed)) ? "sext" : "zext", T1, W, T2)
  end
end

@inline vconvert(::Type{Vec{W,Float16}}, v::Vec{W,Float64}) where {W} =
  vconvert(Vec{W,Float16}, vconvert(Vec{W,Float32}, v))
@inline vconvert(::Type{Vec{W,Float64}}, v::Vec{W,Float16}) where {W} =
  vconvert(Vec{W,Float64}, vconvert(Vec{W,Float32}, v))
@generated vconvert(::Type{Vec{W,Float16}}, v::Vec{W,Float32}) where {W} =
  convert_func("fptrunc", Float16, W, Float32, W)
@generated vconvert(::Type{Vec{W,Float32}}, v::Vec{W,Float16}) where {W} =
  convert_func("fpext", Float32, W, Float16, W)
@generated vconvert(::Type{Vec{W,Float32}}, v::Vec{W,Float64}) where {W} =
  convert_func("fptrunc", Float32, W, Float64, W)
@generated vconvert(::Type{Vec{W,Float64}}, v::Vec{W,Float32}) where {W} =
  convert_func("fpext", Float64, W, Float32, W)

@inline vconvert(::Type{<:AbstractMask{W}}, v::Vec{W,Bool}) where {W} = tomask(v)
@inline vconvert(::Type{M}, v::Vec{W,Bool}) where {W,U,M<:AbstractMask{W,U}} = tomask(v)
@inline vconvert(
  ::Type{<:VectorizationBase.AbstractMask{W,U} where {U}},
  v::Vec{W,Bool},
) where {W} = VectorizationBase.tomask(v)
@inline vconvert(
  ::Type{<:VectorizationBase.AbstractMask{L,U} where {L,U}},
  v::Vec{W,Bool},
) where {W} = VectorizationBase.tomask(v)
# @inline vconvert(::Type{Mask}, v::Vec{W,Bool}) where {W} = tomask(v)
# @generated function vconvert(::Type{<:AbstractMask{W}}, v::Vec{W,Bool}) where {W}
#     instrs = String[]
#     push!(instrs, "%m = trunc <$W x i8> %0 to <$W x i1>")
#     zext_mask!(instrs, 'm', W, '0')
#     push!(instrs, "ret i$(max(8,W)) %res.0")
#     U = mask_type_symbol(W);
#     quote
#         $(Expr(:meta,:inline))
#         Mask{$W}($LLVMCALL($(join(instrs, "\n")), $U, Tuple{_Vec{$W,Bool}}, data(v)))
#     end
# end
@inline vconvert(::Type{Vec{W,Bit}}, v::Vec{W,Bool}) where {W,Bool} = tomask(v)

@inline vconvert(::Type{Vec{W,T}}, v::Vec{W,T}) where {W,T<:IntegerTypesHW} = v
@inline vconvert(::Type{Vec{W,T}}, v::Vec{W,T}) where {W,T} = v
@inline vconvert(::Type{Vec{W,T}}, s::NativeTypes) where {W,T} = vbroadcast(Val{W}(), T(s))
@inline vconvert(::Type{Vec{W,T}}, s::IntegerTypesHW) where {W,T<:IntegerTypesHW} =
  _vbroadcast(StaticInt{W}(), s % T, StaticInt{W}() * static_sizeof(T))
@inline vconvert(::Type{V}, u::VecUnroll) where {V<:AbstractSIMDVector} =
  VecUnroll(fmap(vconvert, V, getfield(u, :data)))
@inline vconvert(::Type{V}, u::VecUnroll{N,W,T,V}) where {N,W,T,V<:AbstractSIMDVector} = u

@inline vconvert(::Type{<:AbstractSIMDVector{W,T}}, i::MM{W,X}) where {W,X,T} =
  vrangeincr(Val{W}(), T(data(i)), Val{0}(), Val{X}())
@inline vconvert(::Type{MM{W,X,T}}, i::MM{W,X}) where {W,X,T} = MM{W,X}(T(getfield(i, :i)))

@inline function vconvert(
  ::Type{V},
  v::AbstractMask{W},
) where {W,T<:Union{Base.HWReal,Bool},V<:AbstractSIMDVector{W,T}}
  vifelse(v, one(T), zero(T))
end
@inline vconvert(::Type{V}, v::AbstractMask{W}) where {W,V<:AbstractSIMDVector{W,Bit}} = v
@inline function vconvert(
  ::Type{V},
  v::Vec{W,Bool},
) where {W,T<:Base.HWReal,V<:AbstractSIMDVector{W,T}}
  vifelse(v, one(T), zero(T))
end

### `vconvert(::Type{<:NativeTypes}, x)` methods. These forward to `vconvert(::Type{Vec{W,T}}, x)`
@inline vconvert(::Type{T}, s::T) where {T<:NativeTypes} = s
@inline vconvert(::Type{T}, s::T) where {T<:IntegerTypesHW} = s
@inline vconvert(::Type{T}, s::NativeTypes) where {T<:NativeTypes} = s
@inline vconvert(::Type{T}, s::IntegerTypesHW) where {T<:IntegerTypesHW} = s % T
@inline vconvert(::Type{T}, v::AbstractSIMD{W,T}) where {T<:NativeTypes,W} = v
@inline vconvert(::Type{T}, v::AbstractSIMD{W,S}) where {T<:NativeTypes,S,W} =
  vconvert(Vec{W,T}, v)

### `vconvert(::Type{<:VecUnroll}, x)` methods
@inline vconvert(::Type{VecUnroll{N,W,T,V}}, s::NativeTypes) where {N,W,T,V} =
  VecUnroll{N,W,T,V}(vconvert(V, s))
@inline function _vconvert(
  ::Type{VecUnroll{N,W,T,V}},
  v::AbstractSIMDVector{W},
) where {N,W,T,V}
  VecUnroll{N,W,T,V}(vconvert(V, v))
end
@inline function vconvert(::Type{VecUnroll{N,W,T,V}}, v::VecUnroll{N}) where {N,W,T,V}
  VecUnroll(fmap(vconvert, V, getfield(v, :data)))
end
@inline vconvert(::Type{VecUnroll{N,W,T,V}}, v::VecUnroll{N,W,T,V}) where {N,W,T,V} = v
@generated function vconvert(::Type{VecUnroll{N,1,T,T}}, s::NativeTypes) where {N,T}
  quote
    $(Expr(:meta, :inline))
    x = convert($T, s)
    VecUnroll((Base.Cartesian.@ntuple $(N + 1) n -> x))
  end
end
@generated function VecUnroll{N,W,T,V}(x::V) where {N,W,T,V<:Real}
  q = Expr(:block, Expr(:meta, :inline))
  t = Expr(:tuple)
  for n ∈ 0:N
    push!(t.args, :x)
  end
  push!(q.args, :(VecUnroll($t)))
  q
end

# @inline vconvert(::Type{T}, v::T) where {T} = v

@generated function splitvectortotuple(
  ::StaticInt{N},
  ::StaticInt{W},
  v::AbstractMask{L},
) where {N,W,L}
  N * W == L ||
    throw(ArgumentError("Can't split a vector of length $L into $N pieces of length $W."))
  t = Expr(:tuple, :(Mask{$W}(u)))
  s = 0
  for n ∈ 2:N
    push!(t.args, :(Mask{$W}(u >>> $(s += W))))
  end
  # This `vconvert` will dispatch to one of the following two `vconvert` methods
  Expr(:block, Expr(:meta, :inline), :(u = data(v)), t)
end
@generated function splitvectortotuple(
  ::StaticInt{N},
  ::StaticInt{W},
  v::AbstractSIMDVector{L},
) where {N,W,L}
  N * W == L ||
    throw(ArgumentError("Can't split a vector of length $L into $N pieces of length $W."))
  t = Expr(:tuple)
  j = 0
  for i ∈ 1:N
    val = Expr(:tuple)
    for w ∈ 1:W
      push!(val.args, j)
      j += 1
    end
    push!(t.args, :(shufflevector(v, Val{$val}())))
  end
  Expr(:block, Expr(:meta, :inline), t)
end
@generated function splitvectortotuple(
  ::StaticInt{N},
  ::StaticInt{W},
  v::LazyMulAdd{M,O},
) where {N,W,M,O}
  # LazyMulAdd{M,O}(splitvectortotuple(StaticInt{N}(), StaticInt{W}(), getfield(v, :data)))
  t = Expr(:tuple)
  for n ∈ 1:N
    push!(t.args, :(LazyMulAdd{$M,$O}(splitdata[$n])))
  end
  Expr(
    :block,
    Expr(:meta, :inline),
    :(splitdata = splitvectortotuple(StaticInt{$N}(), StaticInt{$W}(), getfield(v, :data))),
    t,
  )
end

@generated function vconvert(
  ::Type{VecUnroll{N,W,T,V}},
  v::AbstractSIMDVector{L},
) where {N,W,T,V,L}
  if W == L # _vconvert will dispatch to one of the two above
    Expr(:block, Expr(:meta, :inline), :(_vconvert(VecUnroll{$N,$W,$T,$V}, v)))
  else
    Expr(
      :block,
      Expr(:meta, :inline),
      :(vconvert(
        VecUnroll{$N,$W,$T,$V},
        VecUnroll(splitvectortotuple(StaticInt{$(N + 1)}(), StaticInt{$W}(), v)),
      )),
    )
  end
end

@inline Vec{W,T}(v::Vec{W,S}) where {W,T,S} = vconvert(Vec{W,T}, v)
@inline Vec{W,T}(v::S) where {W,T,S<:NativeTypes} = vconvert(Vec{W,T}, v)

@inline vsigned(v::AbstractSIMD{W,T}) where {W,T<:Base.BitInteger} = v % signed(T)
@inline vunsigned(v::AbstractSIMD{W,T}) where {W,T<:Base.BitInteger} = v % unsigned(T)

@generated function _vfloat(v::AbstractSIMD{W,I}, ::StaticInt{RS}) where {W,I<:Integer,RS}
  ex = if 8W ≤ RS
    :(vconvert(Vec{$W,Float64}, v))
  else
    :(vconvert(Vec{$W,Float32}, v))
  end
  Expr(:block, Expr(:meta, :inline), ex)
end
@inline vfloat(v::AbstractSIMD{W,I}) where {W,I<:Integer} = _vfloat(v, register_size())

@inline vfloat(v::AbstractSIMD{W,T}) where {W,T<:Union{Float32,Float64}} = v
@inline vfloat(v::AbstractSIMD{W,Float16}) where {W} = vconvert(Float32, v)
# @inline vfloat(vu::VecUnroll) = VecUnroll(fmap(vfloat, getfield(vu, :data)))
@inline vfloat(x::Union{Float32,Float64}) = x
@inline vfloat(x::UInt64) = Base.uitofp(Float64, x)
@inline vfloat(x::Int64) = Base.sitofp(Float64, x)
@inline vfloat(x::Union{UInt8,UInt16,UInt32}) = Base.uitofp(Float32, x)
@inline vfloat(x::Union{Int8,Int16,Int32}) = Base.sitofp(Float32, x)
# @inline vfloat(v::Vec{W,I}) where {W, I <: Union{UInt64, Int64}} = Vec{W,Float64}(v)

@inline vfloat_fast(v::AbstractSIMDVector{W,T}) where {W,T<:Union{Float32,Float64}} = v
@inline vfloat_fast(vu::VecUnroll{W,T}) where {W,T<:Union{Float32,Float64}} = vu
@inline vfloat_fast(vu::VecUnroll) = VecUnroll(fmap(vfloat_fast, getfield(vu, :data)))

@generated function __vfloat_fast(v::Vec{W,I}, ::StaticInt{RS}) where {W,I<:Integer,RS}
  arg = if (2W * sizeof(I) ≤ RS) || sizeof(I) ≤ 4
    :v
  elseif I <: Signed
    :(v % Int32)
  else
    :(v % UInt32)
  end
  ex = if 8W ≤ RS
    :(Vec{$W,Float64}($arg))
  else
    :(Vec{$W,Float32}($arg))
  end
  Expr(:block, Expr(:meta, :inline), ex)
end
@inline _vfloat_fast(v, ::False) = __vfloat_fast(v, register_size())
@inline _vfloat_fast(v, ::True) = vfloat(v)

@inline vfloat_fast(v::Vec) = _vfloat_fast(v, has_feature(Val(:x86_64_avx512dq)))

@inline vreinterpret(::Type{T}, x::S) where {T,S<:NativeTypes} = reinterpret(T, x)
@inline vreinterpret(::Type{Vec{1,T}}, x::S) where {T,S<:NativeTypes} = reinterpret(T, x)
@inline vrem(x::NativeTypes, ::Type{T}) where {T} = x % T
@generated function vreinterpret(::Type{T1}, v::Vec{W2,T2}) where {W2,T1<:NativeTypes,T2}
  W1 = W2 * sizeof(T2) ÷ sizeof(T1)
  Expr(:block, Expr(:meta, :inline), :(vreinterpret(Vec{$W1,$T1}, v)))
end
@inline vreinterpret(::Type{Vec{1,T1}}, v::Vec{W,T2}) where {W,T1,T2<:Base.BitInteger} =
  reinterpret(T1, fuseint(v))
@generated function vreinterpret(::Type{Vec{W1,T1}}, v::Vec{W2,T2}) where {W1,W2,T1,T2}
  @assert sizeof(T1) * W1 == W2 * sizeof(T2)
  convert_func("bitcast", T1, W1, T2, W2)
end

@inline vunsafe_trunc(::Type{I}, v::Vec{W,T}) where {W,I,T} = vconvert(Vec{W,I}, v)
@inline vrem(v::AbstractSIMDVector{W,T}, ::Type{I}) where {W,I,T} = vconvert(Vec{W,I}, v)
@inline vrem(v::AbstractSIMDVector{W,T}, ::Type{V}) where {W,I,T,V<:AbstractSIMD{W,I}} =
  vconvert(V, v)
@inline vrem(r::IntegerTypesHW, ::Type{V}) where {W,I,V<:AbstractSIMD{W,I}} =
  convert(V, r % I)