segment_distance_gui.jl

using LinearAlgebra
using PyPlot
using GeometryTypes
using EnhancedGJK
using Setfield # https://github.com/jw3126/Setfield.jl


import Polyhedra
import CoordinateTransformations

# sort points ccw
function convexhull_andrew(points)
  #from Polyhedra.jl/recipe.jl
  counterclockwise(p1, p2) = dot(cross([p1; 0], [p2; 0]), [0, 0, 1])

  points = sort!(points, by = x -> x[1]) # TODO  don't mutate arguments
  return [Polyhedra.getsemihull(points, 1, counterclockwise); Polyhedra.getsemihull(points, -1, counterclockwise)[2:end-1]]
end

function arraycycle(v); vcat(v, [v[1]]) end

"""
find nearest point on segment
"""
function findnearest(segment::LineSegment{Point{N, T}}, query::Point{N, T}) where {N, T}
  t = findnearestline(segment..., query)
  tc = clamp(t, 0, 1)
  return lerp(segment, tc)
end


function lerp(segment::LineSegment{Point{N, T}}, t) where {N, T}
  (p1, p2) = segment
  return ((1-t) * p1 + t * p2)
end

"""
given line that goes through p1 and p2, defined parametrically as
l(t) = ((1-t) * p1 + t * p2)
find t such that
norm(l(t) - pq) is minimized
or equivalently
(l(t) - pq)' * (p2 - p1) = 0
"""
function findnearestline(p1::Point{N,T}, p2::Point{N,T}, pq::Point{N,T}) where {N, T}
  d = p2 - p1
  t̄ = ((pq - p1)' * d) / (d' * d)
  return t̄
end



PyPlot.plt[:ion]()


mutable struct GUIScene{T}
  segments::Vector{LineSegment{Point{2, T}}}
  gjk_simplex::Any
end

mutable struct GUIClosure
  clicked::Bool
  selection::Any
  click_point::Any
  items::Any
  freeze::Any
  segment_vizs::Any
  gjk_simplex_viz::Any
end

struct RenderedSegment
  line
end

struct RenderedShape
  line
end

function make_viz!(ax, s::Type{LineSegment{Point{2, T}}}) where {T<:Real}
  RenderedSegment(ax[:plot](zeros(0))[1])
end

function make_viz!(ax, s::Type{Vector{Point{2, T}}}) where {T<:Real}
  # TODO should be a matplotlib patch
  RenderedShape(ax[:plot](zeros(0))[1])
end

function render!(rs::RenderedSegment, s::LineSegment{Point{2, T}}) where {T}
  rs.line[:set_data]([[s[i][xy] for i=1:2] for xy=1:2]...)
end

function render!(rs::RenderedShape, s)
  rs.line[:set_data]([[point[xy] for point=s] for xy=1:2]...)
end

function drag_update!(gui::GUIClosure, dragged::Point{2,T}) where {T}
  drag = dragged - gui.click_point
  gui.items, gui.freeze, gui.click_point

  drags = [(i in gui.selection.selection_s) ? drag : [0.0, 0.0] for i = 1:2]
  gui.items.segments[gui.selection.segment_i] = LineSegment(
      (gui.freeze.segments[gui.selection.segment_i][i] + drags[i] for i = 1:2)...)
end

function gui_distance(segment::LineSegment{Point{2, T}}, clicked::Point{2, T}, context::Any) where {T}
  s1 = norm(segment[1] - clicked)
  s2 = norm(segment[2] - clicked)
  s = norm(findnearest(segment, clicked) - clicked)  # distance to segment

  l = norm(segment[2] - segment[1])

  vmin, imin = findmin([s1, s2])
  if vmin > max(l/10, 0.01) && vmin > s
    # segment is selected. Drag both endpoints
    return (s, [1, 2])
  else
    return (vmin, [imin])
  end
end

pacman = GUIScene(
  [
    LineSegment(Point(-1.0, -1.0), Point(1.0, 1.0)),
    LineSegment(Point(-1.2, -0.8), Point(1.2, -0.8)),
  ],
  Point{2, Float64}[]
)

(fig, (ax, ax_tc)) = PyPlot.plt[:subplots](1,2)

ax[:set_aspect](1.0)
ax[:set_xlim](-2.0, 2.0)
ax[:set_ylim](-2.0, 2.0)
ax[:set_title]("workspace")

ax_tc[:set_aspect](1.0)
ax_tc[:set_xlim](-4.0, 4.0)
ax_tc[:set_ylim](-4.0, 4.0)
ax_tc[:set_title]("tc-space")

full_tcso = ax_tc[:plot](zeros(0))[1] # translational configuration space
full_tcso[:set_linestyle]("--")

circle_patch = PyPlot.matplotlib[:patches][:Circle]((0.0, 0.0), 0.0)
ax_tc[:add_patch](circle_patch)
circle_patch[:set_fill](false)

#GeometryTypes.register_ax_debug(ax)

gui = GUIClosure(false, nothing, nothing, nothing, nothing, nothing, nothing)
gui.items = pacman
gui.segment_vizs = [make_viz!(ax, LineSegment{Point{2, Float64}}) for _ in pacman.segments]
gui.gjk_simplex_viz = make_viz!(ax_tc, Vector{Point{2, Float64}})

render!.(gui.segment_vizs, gui.items.segments)

function compute_click!(gui::GUIClosure)
  # should measure all distances in screen space (pixels), not data
  distance_selections = [gui_distance(segment, gui.click_point, nothing) for segment in gui.freeze.segments]

  segment_i = argmin([d for (d,s) in distance_selections])
  (distance_s, selection_s) = distance_selections[segment_i]
  gui.selection = (segment_i=segment_i, selection_s=selection_s)
end


function onclick!(gui, event)
    if event[:inaxes] != ax # TODO accessing global variable
      return
    end
    gui.clicked = true
    gui.click_point = Point(event[:xdata], event[:ydata])
    gui.freeze = deepcopy(gui.items) # how to copy! into a mutable struct?
    compute_click!(gui)
end


function onunclick!(gui, event)
    gui.clicked = false
end

debug = Dict()
function onmove!(gui, event)
    if gui.clicked && event[:xdata] != nothing && event[:ydata] != nothing
      #@show event[:xdata] event[:ydata]
      drag_update!(gui, Point(event[:xdata], event[:ydata]))

      render!.(gui.segment_vizs, gui.items.segments)

      cache = EnhancedGJK.CollisionCache(gui.items.segments[1], gui.items.segments[2])
      global debug[:cache] = cache
      gjk_result = EnhancedGJK.gjk!(cache,
        CoordinateTransformations.IdentityTransformation(), CoordinateTransformations.IdentityTransformation())

      debug[:simplex] = gjk_result.simplex
      simplex = arraycycle(gjk_result.simplex)
      render!(gui.gjk_simplex_viz, simplex)

      circle_patch[:set_radius](abs(gjk_result.signed_distance))
      circle_patch[:set_linestyle](if gjk_result.signed_distance > 0 "-" else "--" end)

      tcso_points = [a - b for a in gui.items.segments[1] for b in gui.items.segments[2]]

      tcso_points_ordered = convexhull_andrew(tcso_points)
      render!(RenderedShape(full_tcso), arraycycle(tcso_points_ordered))
      clear = true

      if clear
        gui.segment_vizs[1].line[:set_linestyle]("-")
        gui.segment_vizs[2].line[:set_linestyle]("-")
      else
        gui.segment_vizs[1].line[:set_linestyle]("--")
        gui.segment_vizs[2].line[:set_linestyle]("--")
      end

    end
end

cid = fig[:canvas][:mpl_connect]("button_press_event", e->onclick!(gui, e))
cid = fig[:canvas][:mpl_connect]("button_release_event", e->onunclick!(gui, e))
cid = fig[:canvas][:mpl_connect]("motion_notify_event", e->onmove!(gui, e))