#
# Bouncing off code
#

#------IMPORTS-------
from pygestalt import nodes
from pygestalt import interfaces
from pygestalt import machines
from pygestalt import functions
from pygestalt.machines import elements
from pygestalt.machines import kinematics
from pygestalt.machines import state
from pygestalt.utilities import notice
from pygestalt.publish import rpc #remote procedure call dispatcher
import time
import io
import numpy as np

# example parameters
move_speed = 10 # speed of movement when extruding
extrude_speed = 1 # speed of icing extrusion
extrude_height = 0 # z coordinate of extrusion
jog_speed = 15 # speed of movement when jogging to next place to extrude
jog_height = 1 # z coorinate of jog

# example coords for extrusion line
coord_start = [1,1]
coord_end = [2,3]



# take two coordinates and extrude a line between them
def extrude (coord_start, coord_end):
  if coord_current () != coord_start: 
    # jog there
    end_extrude ()
    move_z (jog_height, jog_speed)
    move_xy (coord_start, jog_speed)
    move_z (extrude_height, jog_speed)
  # Extrude
  start_extrude (extrude_speed)
  move_xy (coord_end, move_speed)

# Now define 2D movement
def move_xy (coord_end, speed):
  # find distances
  xy_dist = coord_end - coord_current ()
  x_dist = xy_dist[0]
  y_dist = xy_dist[1] 
  # calculate time to get there, deduce x and y speeds
  time_extrude = np.sqrt (x_dist^2 + y_dist^2) / speed
  x_move_speed = x_dist / time_extrude
  y_move_speed = y_dist / time_extrude
  # move the axes
  #
  # How do we do this in parallel?
  #
  move_x (coord_end[0], x_move_speed)
  move_y (coord_end[1], y_move_speed)

#
# Some implementing left:
#

# find current coord
def coord_current ():
    # implement

# move the positions
def move_x (x_coord_end, x_speed):
  # implement
def move_x (y_coord_end, y_speed):
  # implement
def move_x (z_coord_end, z_speed):
  # implement

# extrusion
def start_extrude (extrude_speed):
  # implement
def stop_extrude ():
  # implement