# AddGeometry.py - contents of the AddGeometry ghpython script

# inputs
#   layer  - string naming the layer to address within the current Rhino document
#   gobjs  - list of geometry objects to add, either a raw Geometry type or a ghdoc GUID
#   names  - list of object name strings
# enabled  - Boolean to indicate the action is enabled
#
# Note: gobjs and names must be set to 'List Access'
#
# outputs
#   out    - console debugging output
#   guids  - list of GUID strings for the new geometry primitive
#
# Note: the GUIDs are for objects located in the RhinoDoc database, *not* the
# Grasshopper document database, so they are only useful for requesting updates
# to the current Rhino document.

import Rhino
import System.Guid
import pythonlibpath; pythonlibpath.add_library_path()
import ghutil.ghrhinodoc as ghrhinodoc

# Fetch the layer index, creating the layer if not present.
if enabled:
    layer_name = str(layer)
    layer_index = ghrhinodoc.fetch_or_create_layer_index(layer_name)
    guids = list()
    
    for gobj, name in zip(gobjs, names):
        # If needed, find the geometry corresponding with the given input, possibly looking it up by GUID.
        item = ghrhinodoc.find_ghdoc_geometry(ghdoc, gobj)

        # Add the geometry to the RhinoDoc with the given layer and name, returning the new RhinoDoc GUID as a string.
        guids.append(str(ghrhinodoc.add_geometry(item, layer_index, name)))

# Counter.py - general purpose counter block for use in ghpython object
# inputs
#  name   - counter name token string
#  enable - bool to enable counting
#  reset  - bool to reset to lowest value
#  low    - lowest output value
#  high   - upper bound on output value
#  step   - interval to step

# outputs
#   value - integer count value

import scriptcontext as sc

value = sc.sticky.get(name)

if value is None or reset or value >= high:
    value = low

elif enable:
    value = value + step

sc.sticky[name] = value
print value

# CSVLoader.py - the file loader script in the ghpython object in MocapDemo.gh.

# Demonstration of parsing an Optitrack motion capture CSV file.
#
# inputs
#   path    - string with the full path to the CSV file
#   stride  - the sequence interval between successive points; 1 returns all points, >1 subsamples
#
# outputs
#   out      - debugging text stream
#   names    - data tree of body names
#   planes   - data tree of Plane trajectories, one branch per rigid body, each leaf a list of Planes

# import the Optitrack file loader from the same folder
import optiload

# load the file
take = optiload.load_csv_file(path)

print "Found rigid bodies:", take.rigid_bodies.keys()

# emit all return values
names = take.rigid_bodies.keys()
planes = optiload.all_Planes(take, int(stride))

# DeleteGeometry.py - contents of the DeleteGeometry ghpython script

# inputs
#   layer  - string naming the layer to address within the current Rhino document
#   guid   - GUID string identifying a single object within the RhinoDoc

# outputs
#   out    - console debugging output
#
# Note: the GUIDs are for objects located in the RhinoDoc database, *not* the
# Grasshopper document database, so they are only useful for requesting updates
# to the current Rhino document.

import Rhino
import System.Guid
import pythonlibpath; pythonlibpath.add_library_path()
import ghutil.ghrhinodoc as ghrhinodoc

if guid is not None:
    # Fetch the layer index, creating the layer if not present.
    layer_name = str(layer)
    layer_index = ghrhinodoc.fetch_or_create_layer_index(layer_name)
    
    # Look up the object to delete.
    print "GUID to modify is", guid, type(guid)
    
    # convert from a string to a Guid object
    guid = System.Guid(guid)
    
    existing = Rhino.RhinoDoc.ActiveDoc.Objects.Find(guid)
    print "Found RhinoDoc object", existing
    
    # Delete the geometry from the RhinoDoc with the given layer and name.
    Rhino.RhinoDoc.ActiveDoc.Objects.Delete(guid, True)

# DetectFlick.py - contents of the ghpython object in MocapDemo.gh
#
# inputs
#   reset   - Boolean to deactivate and reset state
#   points  - list of Point3D objects representing the path of the tip of an indicating stick
#             Note: the 'points' input must be set to 'List Access'
# threshold - the acceleration value above which to output an event

# outputs
#   out      - debugging text stream
#   sample   - None, or an integer sample offset for the flick event; zero is 'now', negative values are relative to now.
#   pos      - list of points in buffer
#  accel     - list of estimated accelerations

# use persistent state context
import scriptcontext
sc_identifier = 'flick_detector'

# use point filter with acceleration estimator
reload(pointfilter)
import pointfilter

# If the user reset input is set, then clear any existing state and do nothing.
if reset:
    print "Resetting receiver state."
    scriptcontext.sticky[sc_identifier] = None 

else:
    # fetch or create the persistent filter object
    filter = scriptcontext.sticky.get(sc_identifier)
    if filter is None:
        filter = pointfilter.Point3dFilter(80)
        scriptcontext.sticky[sc_identifier] = filter
        
    # add the new data
    filter.add_points(points)
    # print filter._position
    # print "Adding %d points to filter." % len(points)
    # offset, mag, acc, pos = filter.find_accel_peak()
    # print "Peak at %d of %f" % (offset, mag)
    
    sample = filter.detect_acceleration_event( threshold )
    if sample is not None:
        print "Acceleration peak observed at %d" % sample
    
    print "Filter has seen %d points, buffer starts at %d, blanking ends at %d." % (filter.samples, filter.samples - filter._buf_len, filter.last_event + filter.blanking)
    pos = filter._position
    accel = filter._accel
    

# GestureLogic - state machine for interface logic for the gesture-based editing.
#
# This encompasses all the logic for the editor which is easier to write in
# Python than Grasshopper objects.  Only one GestureLogic instance is expected to
# exist since it holds and tracks user inputs.
#
# Objectives for this block:
#
# 1. Only hold transient user state.  All persistent data is read from the
#    RhinoDoc, manipulated in a transient way, and then written back to the
#    RhinoDoc or discarded.
#
# 2. Emit signals to read and write the model rather than directly manipulate
#    the RhinoDoc.  This does increase the number of I/O variables, but
#    is intended to make the operation easier to observe, debug, and extend.
# 
# inputs:
#  name    - name token string for sticky
#  reset   - bool to reset to lowest value
#  gesture - None or relative sample index (integer) of detected gesture event
#  cursor  - list of Planes with recent cursor object trajectory
#  poses   - list of Planes saved at gesture events
#  mode
#  update
#  selection
#  selguids
#  setselect
#  clear
#  all_names
#  layer_name  - name of the editable layer in the RhinoDoc
#  create_interval - integer number of cycles between creating new objects

# Note: the following must have 'List Access' set: cursor, poses, selection, selguids, all_names
#
# outputs:
#  out    - log string output for display; the log is persistent to reduce the amount of flickering
#  add
#  move
#  names
#  newloc
#  status
#  objects
#  guids
#  xform

################################################################
import scriptcontext as sc
import clr
import System.Guid
import math

import Rhino
import pythonlibpath; pythonlibpath.add_library_path()
import ghutil.ghrhinodoc as ghrhinodoc

################################################################
class EditorLogic(object):
    """Utility class to manage the state of the interactive editor."""

    def __init__(self, _layer_name = 'Cards', _all_names = []):
        self.layer_name = _layer_name
        self._last_setselect = False # debounce variable
        self.attached = False
        self.interval_counter = 0
        self.mocap_dt = 1.0 / 120 # sampling rate of the motion capture stream

        # list of strings in which to accumulate messages for output
        self.log       = ["Editor initialized for layer %s." % self.layer_name]    
         
        # initialize the default sets of object names based on those found on the RhinoDoc layer
        self._update_namesets(_all_names)
        
        # freshly created objects: poses and names
        self.new_object_poses = list()
        self.new_object_names = list()

        # the current selection
        self.selection = None
        self.docguids = None
        self.selection_bb = None
        self.selection_bb_size = None
        
        # coordinate transformation for group edits
        self.transform = None  
        self.motion    = None
        self.xforms    = []    # list of transforms, one per selected object
        
        return

    def add_new_object_pose(self, plane):   
        if plane is not None:
            name = self.choose_new_name()
            if name is not None:
                self.new_object_poses.append(plane)
                self.new_object_names.append(name)
        return
        
    def clear_edits(self):
        """Reset all transient editor state, either at user request or after editing cycle is complete."""
        self.new_object_poses = list()
        self.new_object_names = list()
        self.attached = False
        self.selection = None
        self.docguids = None
        return

    def logprint(self, msg):
        self.log.append(msg)
        
    def clear_log(self):
        self.log = []
        
    def set_namesets(self, all_names, used_names):
        """Update the name manager given a list of all possible object names and the list of object names currently in use."""
        self.all_names = set(all_names)
        self.used_names = set()
        
        # check for duplicate names
        for used in used_names:
            if used in self.used_names:
                self.logprint("Warning: object name %s appears more than once." % used)
            else:
                self.used_names.add(used)
                
        # check for used names not listed in the all_names set
        invalid_names = self.used_names - self.all_names
        if invalid_names:
            self.logprint("Warning: invalid names in use: %s" % invalid_names)
            
        # compute the list of available names
        self.unused_names = self.all_names - self.used_names
        self.logprint("Found the following unused object names: %s" % self.unused_names)
        return

    def choose_new_name(self):
        """Pick an name arbitrarily from the set of unused names."""
        if len(self.unused_names) == 0:
            self.logprint("Warning: no more object names available.")
            return None
        # return new names in numerical order for clarity
        new_name = sorted(self.unused_names)[0]
        self.unused_names.remove(new_name)
        return new_name

    def _update_namesets(self, all_names):
        all_objects = ghrhinodoc.all_doc_objects(layer_name)        
        names = [obj.Attributes.Name for obj in all_objects]
        self.set_namesets(all_names, names)
        return
  
    def _compute_set_bounding_box(self, selection):
        if selection is None or len(selection) == 0:
            return None
            
        # compute bounding box for all objects in a set
        boxes = [obj.GetBoundingBox(True) for obj in selection]

        # compute union of all boxes
        union = boxes[0]
        # destructively merge this with the other boxes
        for b in boxes[1:]:
            union.Union(b)
        return union, union.Diagonal.Length


    def manage_user_selection(self, setselect, selection, selguids, all_names):
        """Process the user 'Set Selection' input, updating the editor state for any new
        objects and names as needed."""
        
        if setselect != self._last_setselect:
            # debounce input to just trigger once
            self._last_setselect = setselect
            if setselect == True:
                self.selection = selection
                self.docguids = selguids
                
                self.selection_bb, self.selection_bb_size = self._compute_set_bounding_box(selection)
                self.logprint("Updated selection bounding box to %s, diagonal size %f" % (self.selection_bb, self.selection_bb_size))

                # reset the pick and place state
                self.attached  = False
                self.transform = None
                self.xforms    = []

                self.logprint("Selection set with %d objects." % len(selection))
                self._update_namesets(all_names)
                
    #================================================================
    def read_objects_from_layer(self, layer_name):
        """Read the user-visible names of all objects on a specific RhinoDoc layer.
        Returns a tuple (geoms, guids, names) with lists of all geometry
        objects, RhinoDoc GUID strings, and name attributes.
        """
        layer_index = ghrhinodoc.fetch_or_create_layer_index(layer_name)

        # Fetch all objects on the layer and report out individual properties.
        all_objects = ghrhinodoc.all_doc_objects(layer_name)

        geoms = [obj.Geometry for obj in all_objects]
        guids = [str(obj.Id) for obj in all_objects]
        names = [obj.Attributes.Name for obj in all_objects]
        return geoms, guids, names

    #================================================================
    def update_tap_create_mode(self, gesture, cursor, update, clear):
        """Update state for the 'tap create' mode in which gestures create individual cards.
        gesture - the integer gesture sample index or None
        cursor  - the list of recent cursor poses

        returns newloc, names, add
        """

        # default outputs
        names, add = None, None
        
        # tap create mode: each gesture creates a new object
        if gesture is not None:
            self.logprint("Gesture detected with sample offset %d." % gesture)
            # gesture is an integer sample where zero is the most recent pose;
            # index the current cursor poses from the end to select the correct
            # pose
            self.add_new_object_pose(cursor[gesture-1])

        if clear == True:
            self.logprint( "Abandoning editor changes (dropping new object poses).")
            self.clear_edits()         

        # by default, always emit the new poses so they can be visualized
        newloc = self.new_object_poses

        if update == True:
            self.logprint("Writing new objects to RhinoDoc: %s" % self.new_object_names)
            names = self.new_object_names
            add = True
            self.clear_edits() # note: the current list has already been emitted, this just resets the buffer

        return newloc, names, add
        
    #================================================================
    def update_path_create_mode(self, _gesture, _cursor, _update, _clear, _all_names, _create_rate):

        """Update state for the 'symbol sprayer' mode which places new objects along a
        cursor path.  Each gesture event toggles the creation events on or off.

        returns newloc, names, add
        """

        # default outputs
        names, add = None, False
        
        # detect the singular gesture events (for now, a flick of the wand)
        if _gesture is not None:
            if self.attached:
                self.logprint("Creation path ended.")
            else:
                self.logprint("Creation path beginning.")
                self.interval_counter = 0
                self._update_namesets(_all_names)
                
            # toggle the 'attached' state
            self.attached = not self.attached

        # while 'attached' to the sprayer, create new objects at regular intervals
        if self.attached:
            self.interval_counter += 1
            
            if self.interval_counter > _create_rate:
               self.interval_counter = 0
               self.add_new_object_pose(_cursor[-1])

        if _clear == True:
            self.logprint( "Abandoning editor changes (dropping new object poses).")
            self.clear_edits()         

        # by default, always emit the new poses so they can be visualized
        newloc = self.new_object_poses

        if _update == True:
            self.logprint("Writing new objects to RhinoDoc: %s" % self.new_object_names)
            names = self.new_object_names
            add = True
            self.clear_edits() # note: the current list has already been emitted, this just resets the buffer

        return newloc, names, add
        
    #================================================================
    def update_block_move_mode(self, gesture, cursor, poses, update, clear):
        """Update state for the 'block move' mode in which each gesture alternately
        attaches or detaches the selection from the cursor.

        returns objects, guids, xform, move
        """

        # set default outputs
        objects = self.selection
        guids = self.docguids
        move = False
        
        motion = Rhino.Geometry.Transform(1) # motion transform is identity value by default

        if clear == True:
            self.logprint("Abandoning editor changes (clearing movement).")
            self.transform = None
            
        # detect the singular gesture events (for now, a flick of the wand)
        if gesture is not None:
            # if we are ending a motion segment, save the most recent transformation as the new base transform
            if self.attached:
                self.transform = self.transform * self.motion
                self.logprint("Motion ended, new transform saved.")
            else:
                self.logprint("Motion beginning.")
                
            # toggle the 'attached' state
            self.attached = not self.attached
            
        if self.attached:
            if len(poses) > 0 and len(cursor) > 0:
                # compute a tranform the from most recent saved pose to the newest cursor position
                motion = Rhino.Geometry.Transform.PlaneToPlane(poses[-1], cursor[-1])
        
        # compute an output transformation from the accumulated transform plus any transient movement
        if self.transform is None:
            self.transform = Rhino.Geometry.Transform(1) # identity
        xform = self.transform * motion
        self.motion = motion

        if update == True:
            self.logprint("Updating RhinoDoc selection with new poses.")
            move = True
            self.clear_edits()

        return objects, guids, xform, move

    #================================================================
    def update_path_move_mode(self, gesture, cursor, poses, update, clear):
        """Update state for the 'path move' mode in which each gesture toggles the
        enable, and the cursor velocity affects object positions within a 'brush'
        radius.

        returns objects, guids, xform, move
        """

        # set default outputs
        objects = self.selection
        guids = self.docguids
        move = False
        
        delta = Rhino.Geometry.Transform(1) # motion transform is identity value by default

        # FIXME: this is probably moot
        if self.transform is None:
            self.transform = Rhino.Geometry.Transform(1) # identity

        if self.selection is not None and (self.xforms is None or len(self.xforms) != len(self.selection)):
            self.xforms = [Rhino.Geometry.Transform(1) for x in self.selection]
            
        if clear == True:
            self.logprint("Abandoning editor changes (clearing movement).")
            self.transform = Rhino.Geometry.Transform(1)
            
        # detect the singular gesture events (for now, a flick of the wand)
        if gesture is not None:
            # if we are ending a motion segment
            if self.attached:
                self.logprint("Motion deactivated.")
            else:
                self.logprint("Motion activated.")
                
            # toggle the 'attached' state
            self.attached = not self.attached
            
        if self.attached:
            if len(cursor) > 1 and cursor[-1] is not None and cursor[-2] is not None:
                # Compute separate translation and rotation thresholds to
                # determine whether the velocity is high enough to be a gesture.

                # Find the rotation and translation between the last pair of samples:
                rot = Rhino.Geometry.Quaternion.Rotation(cursor[-2], cursor[-1])
                delta = cursor[-1].Origin - cursor[-2].Origin
                displacement = delta.Length

                # Convert the rotation to axis-angle form to find the magnitude.  The function uses C# call by reference to return
                # the parameters as 'out' values:
                angle = clr.Reference[float]()
                axis  = clr.Reference[Rhino.Geometry.Vector3d]()
                rot.GetRotation(angle, axis)
                angle = angle.Value # get rid of the StrongBox around the number
                axis  = axis.Value  # get rid of the StrongBox around the vector

                # The angle is returned on (0,2*pi); manage the wraparound
                if angle > math.pi:
                    angle -= 2*math.pi
                    
                # normalize to a velocity measure: m/sec, radians/sec
                speed = displacement / self.mocap_dt
                omega = angle / self.mocap_dt
                
                # compute object to cursor distances
                boxes = [obj.GetBoundingBox(False) for obj in self.selection]
                center = cursor[-1].Origin 
                distances = [box.Center.DistanceTo(center) for box in boxes]
                
                # Apply thresholds to determine whether the gesture represents intentional motion:
                if speed > 1.0 and True:
                    self.transform = self.transform * Rhino.Geometry.Transform.Translation(delta)
                    
                if abs(omega) > 2.0 and True:
                    # self.logprint("detected motion on speed %f and angular rate %f" % (speed, omega))
                    # apply the movement to the output tranform
                    # FIXME: transform should be a list, one per object, selective via a spherical cursor                                       
                    # choose a specific method from the set of overloaded signatures
                    Rotation_Factory = Rhino.Geometry.Transform.Rotation.Overloads[float, Rhino.Geometry.Vector3d, Rhino.Geometry.Point3d]
                    rot_xform = Rotation_Factory(angle, axis, center)
                    self.transform = self.transform * rot_xform
                    
                    # Apply a weighted displacement to each object transform.  The scaling matches the rolloff of the 
                    # effect to be proportional to the size of the bounding box of the moving objects.
                    scale = 0.1 * self.selection_bb_size * self.selection_bb_size
                    weights = [min(1.0, scale/(dist*dist)) if dist > 0.0 else 1.0 for dist in distances]
                    # self.logprint("Weights: %s" % (weights,))
                    rotations = [Rotation_Factory(angle*weight, axis, center) for weight in weights]
                    self.xforms = [xform*rot for xform,rot in zip(self.xforms, rotations)]

        if update == True:
            self.logprint("Updating RhinoDoc selection with new poses.")
            move = True
            self.clear_edits()

        return objects, guids, self.xforms, move


################################################################
# create or re-create the editor state as needed
editor = sc.sticky.get(name)
if editor is None or reset:
    editor = EditorLogic('Cards', all_names)
    sc.sticky[name] = editor

# set default output values
add    = False
move   = False
names  = None
newloc = None
objects = None
guids   = None
xform   = None

if reset:
    print "Interaction logic in reset state."
    status = "Reset"
    
else:
    # for all modes, record the set of selected objects when indicated
    editor.manage_user_selection(setselect, selection, selguids, all_names)

    # handle the state update for each individual mode
    if mode == 1:
        newloc, names, add = editor.update_tap_create_mode(gesture, cursor, update, clear)

    elif mode == 2:
        newloc, names, add = editor.update_path_create_mode(gesture, cursor, update, clear, all_names, create_interval)
        
    elif mode == 3:
        objects, guids, xform, move = editor.update_block_move_mode(gesture, cursor, poses, update, clear)

    elif mode == 4:
        objects, guids, xform, move = editor.update_path_move_mode(gesture, cursor, poses, update, clear)
        
    # emit terse status for remote panel
    status = "M:%s C:%d P:%d N:%d" % (editor.attached, len(cursor), len(poses), len(editor.new_object_poses))

    # re-emit the log output
    for str in editor.log: print str

# LayerGeometry.py - contents of the LayerGeometry ghpython script

# inputs
#   layer  - string naming the layer to address within the current Rhino document
#  update  - any input will force recomputing the output (dummy value)

# outputs
#   out     - console debugging output
#   geoms   - list of geometry primitives currently present in the layer
#   guids   - list of GUID strings for the geometry primitives
#   names   - list of user-visible object names

# Note: the GUIDs are for objects located in the RhinoDoc database, *not* the
# Grasshopper document database, so they are only useful for requesting updates
# to the current Rhino document.

import Rhino
import System.Guid
import pythonlibpath; pythonlibpath.add_library_path()
import ghutil.ghrhinodoc as ghrhinodoc

# Fetch the layer index, creating the layer if not present.
layer_name = str(layer)
layer_index = ghrhinodoc.fetch_or_create_layer_index(layer_name)

# Fetch all objects on the layer and report out individual properties.
all_objects = ghrhinodoc.all_doc_objects(layer_name)

geoms = [obj.Geometry for obj in all_objects]
guids = [str(obj.Id) for obj in all_objects]
names = [obj.Attributes.Name for obj in all_objects]

# MocapReceiver.py - the motion capture script in the ghpython object in MocapDemo.gh

# Demonstration of receiving and parsing an Optitrack motion capture real-time stream.
# This is normally polled from a timer.
#
# inputs
#   reset   - Boolean flag to reset the receiver system
#   version - string defining the protocol version number (e.g. "2900" for Motive 1.9)
#
# outputs
#   out      - debugging text stream
#   received - integer number of new frames received
#   planes   - list of Plane objects or None, one per rigid body stream

# use persistent state context
import scriptcontext

# import the Optitrack stream receiver
# reload(optirecv)
import optirecv

# If the user reset input is set, then clear any existing state and do nothing.
if reset:
    print "Resetting receiver state."
    scriptcontext.sticky['mocap_port'] = None
 
else:
    # fetch or create the persistent motion capture streaming receiver port
    port = scriptcontext.sticky.get('mocap_port')
    if port is None:
        port = optirecv.OptitrackReceiver(version)
        scriptcontext.sticky['mocap_port'] = port

    # Poll the port as long as data is available, accumulating all
    # frames.  It is unlikely that Grasshopper will keep up with the
    # 120 Hz mocap sampling rate, butit is important to have the
    # continuous trajectory available for analysis and recording.
    receiving = True
    frames = list()
    while receiving:
        receiving = port.poll()
        if receiving:
            frames.append(port.make_plane_list())

    # Convert the frame list into a data tree for output.  As accumulated, it is a Python list of lists:
    # [[body1_sample0, body2_sample0, body3_sample0, ...], [body1_sample1, body2_sample1, body3_sample1, ...], ...]
    planes = optirecv.frames_to_tree(frames)
    received = len(frames)
    
    # Emit the list of body names in the order corresponding to the
    # branches in the trajectory data tree.  The ordering is stable as
    # determined by the configuration in the Motive project.
    names  = port.bodynames

# PlaneRecord.py - the plane data recording script in the ghpython object in MocapDemo.gh.
#
# inputs
#  inputs   - list of Plane objects
#  capture  - None or integer sample time offset indicating which recent object should be added to the set
#  clear    - Boolean indicating the the data set should be erased
#  N.B. the 'inputs' input must be set to 'List Access'
#  N.B. the 'capture' input should have a type hint of Int

# other ideas:
#   path       - string with the full path to the CSV file
#   selection  - currently selected set

# outputs
#   out      - debugging text stream
#   planes   - list of Plane objects

# use RhinoCommon API
import Rhino

# use persistent state context
import scriptcontext

# use the history utility class
import historybuffer

################################################################
class PlaneRecorder(object):
    def __init__(self):
        # list of selected objects
        self.planes = []
 
        # keep track of about a quarter-second of mocap data
        self.buffer = historybuffer.History(30)
        return

    def add_planes(self, planes):
        if planes is not None:
            self.buffer.append(planes)
        return
    
    def capture_plane(self, offset):
        self.planes.append(self.buffer[offset])
        return

################################################################
if clear:
    # create an empty data recorder
    recorder = PlaneRecorder()
    scriptcontext.sticky['plane_recorder'] = recorder

else:
    # fetch or create a persistent data recorder
    recorder = scriptcontext.sticky.get('plane_recorder')
    if recorder is None:
        recorder = PlaneRecorder()
        scriptcontext.sticky['plane_recorder'] = recorder
    recorder.add_planes(inputs)

if capture is not None:
    # The capture index is non-positive: zero means 'now', negative means a recent sample.  So the value is
    # biased by -1 to be an index relative to the end of the recorded poses.
    recorder.capture_plane(capture-1)
 
planes = recorder.planes
print "Buffer has seen %d planes, currently holding %d selected planes." % (recorder.buffer.samples, len(planes))

# ReplaceGeometry.py - contents of the ReplaceGeometry ghpython script
#
# inputs
#   layer  - string naming the layer to address within the current Rhino document
#  update  - Boolean indicating the action is enabled
#   guids  - list of GUID strings identifying a single object within the RhinoDoc to replace
#   gobjs  - equal-length list of geometry objects to add, either a raw Geometry type or a ghdoc GUID
#
# Note: guids and gobjs must be set to 'List Access'
#
# outputs
#   out    - console debugging output
#  newids  - list of GUID strings for the new geometry primitive
#
# Note: the GUIDs are for objects located in the RhinoDoc database, *not* the
# Grasshopper document database, so they are only useful for requesting updates
# to the current Rhino document.

# Note: the new object Name attributes are copied from the objects to be deleted so they persist.  This could
# be extended to other user-specified properties if desired.

import Rhino
import System.Guid
import pythonlibpath; pythonlibpath.add_library_path()
import ghutil.ghrhinodoc as ghrhinodoc

if update and guids is not None:
    # Fetch the layer index, creating the layer if not present.
    layer_name = str(layer)
    layer_index = ghrhinodoc.fetch_or_create_layer_index(layer_name)
    newids = list()
    
    for guid, gobj in zip(guids, gobjs):
        # Look up the object to delete.  Convert from a string to a Guid object, then search the active document.
        guid = System.Guid(guid)
        existing = Rhino.RhinoDoc.ActiveDoc.Objects.Find(guid)
        print "Found RhinoDoc object", existing

        if existing is not None:
            name = existing.Attributes.Name
            print "Existing object has name", name
            # Delete the geometry from the RhinoDoc with the given layer and name.
            Rhino.RhinoDoc.ActiveDoc.Objects.Delete(guid, True)

            # If needed, find the geometry corresponding with the given input, possibly looking it up by GUID.
            item = ghrhinodoc.find_ghdoc_geometry(ghdoc, gobj)
            print "Input item is ", item, type(item)

            # Add the geometry to the RhinoDoc with the given layer and name.
            newids.append(str(ghrhinodoc.add_geometry(item, layer_index, name)))
            
        else:
            # if something goes wrong, make sure the output indicates a null for this object
            newids.append(None)

# SelectedGeometry.py - contents of the SelectedGeometry ghpython script

# inputs
#   layer  - string naming the layer to address within the current Rhino document
#  update  - any input will force recomputing the output (dummy value)

# outputs
#   out    - console debugging output
#   geom   - list of geometry primitives currently selected in the layer
#   guid   - list of GUID strings for the geometry primitives
#
# Note: the GUIDs are for objects located in the RhinoDoc database, *not* the
# Grasshopper document database, so they are only useful for requesting updates
# to the current Rhino document.

import Rhino
import System.Guid
import pythonlibpath; pythonlibpath.add_library_path()
import ghutil.ghrhinodoc as ghrhinodoc

# Fetch the layer index, creating the layer if not present.
layer_name = str(layer)
layer_index = ghrhinodoc.fetch_or_create_layer_index(layer_name)

# Fetch all objects on the layer, choose the selected ones,  and report out individual properties.
# See the definition of RhinoObject.IsSelected; if the argument is true then it also reports on sub-objects.
all_objects = ghrhinodoc.all_doc_objects(layer_name)
selected_objects = [obj for obj in all_objects if obj.IsSelected(False) > 0] 

print "Found %d objects in layer, %d are selected." % (len(all_objects), len(selected_objects))

geom = [obj.Geometry for obj in selected_objects]
guid = [str(obj.Id) for obj in selected_objects]

# SortedPaths.py - contents of the ghpython object
# inputs
#  names - list of body names from a mocap stream

# outputs
#  paths - list of data tree paths in order sorted by name
#  Pn    - individual path

sorted_names = sorted(names)
indices = [names.index(name) for name in sorted_names]
paths = ["0;%d" % index for index in indices]

p1 = paths[0]
p2 = paths[1]
p3 = paths[2]

"""\
historybuffer.py : fixed-length buffer for a time-series of objects

Copyright (c) 2016, Garth Zeglin. All rights reserved. Licensed under the
terms of the BSD 3-clause license.
"""

class History(object):
    """Implement a fixed-length buffer for keeping the recent history of a
    time-series of objects.  This is optimized for the case of a relatively
    short buffer receiving blocks of elements at a time, possibly longer than
    the buffer.  It keeps track of the total number of objects appended whether
    or not they are stored.

    The [] (__getitem__) operator uses time-offset addressing: history[0] is the
    most recent element, history[-1] the one before that, etc.

    Attributes:

    samples - the total number of samples appended

    """

    def __init__(self, length):
        # length of the fixed-length history buffer
        self._buf_len = length

        # Keep track of the number of samples observed.
        # N.B. the most recent (last) point in the buffer has sample index = self.samples - 1
        #      the   oldest (first)   point in the buffer has sample index = self.samples - self._buf_len
        self.samples = 0  

        # create fixed-length data buffer filled with None
        self.reset()

        return
    
    def reset(self):
        """Reset buffer state."""
        self._buffer = [None for i in range(self._buf_len)]
        return

    def append(self, object_list):
        """Given a list of objects, append them to the fixed-length history buffer.
        """

        # Compute the pieces of the old and new buffers to concatenate,
        # considering the possibility there may be an excess of new data.
        num_new_samples = min(self._buf_len, len(object_list))
        first_source_sample = len(object_list) - num_new_samples
        first_new_dest_sample = self._buf_len - num_new_samples
        self.samples += len(object_list)

        # construct a new buffer array
        self._buffer = self._buffer[num_new_samples:] + object_list[first_source_sample:]
        return

    def __getitem__(self, index):
        """Look up an element using a time-offset based address.  An index of zero is
        the most recent sample and negative indices return values farther back
        in time. Positive indices are not available as they represent the
        future.
        """

        if index > 0:
            raise IndexError('History time offset must be non-positive', index)

        elif index < (1 - self._buf_len):
            raise IndexError('History time offset out of range', index)

        # use Python negative indexing relative to buffer end
        return self._buffer[index-1]

# optiload.py : motion capture data loader for use within Grasshopper ghpython objects

# Copyright (c) 2016, Garth Zeglin. All rights reserved. Licensed under the
# terms of the BSD 3-clause license.

# use RhinoCommon API
import Rhino

# Make sure that the Python libraries that are also contained within this course
# package are on the load path. This adds the python/ folder to the load path
# *after* the current folder.  The path manipulation assumes that this module is
# still located within the Grasshopper/MocapDemo subfolder, and so the package
# modules are at ../../python.
import sys, os
sys.path.insert(1, os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(os.path.dirname(__file__)))), "python"))

# use itertools for subsampling sequences
import itertools

# import the Optitrack CSV file parser
import optitrack.csv_reader as csv

# import a quaternion conversion function
from optitrack.geometry import quaternion_to_xaxis_yaxis

# load the Grasshopper utility functions from the course packages
from ghutil import *

#================================================================
def load_csv_file(path):
    take = csv.Take().readCSV(path)
    return take

#================================================================
# Convert from default Optitrack coordinates with a XZ ground plane to default
# Rhino coordinates with XY ground plane.

def rotated_point(pt):
    if pt is None:
        return None
    else:
        return Rhino.Geometry.Point3d( pt[0], -pt[2], pt[1])

def rotated_orientation(q):
    if q is None:
        return [0,0,0,1]
    else:
        return [q[0], -q[2], q[1], q[3]]

def plane_or_null(origin,x,y):
    """Utility function to create a Plane unless the origin is None, in which case it returns None."""
    if origin is None:
        return None
    else:
        return Rhino.Geometry.Plane(origin, x, y)

#================================================================
def all_Planes(take, stride=1):
    """Return a DataTree of trajectories containing Planes or None.

    The tree has one branch for each rigid body; each branch contains a list of
    objects, either Plane for a valid sample or None if the sample is missing.

    Due to implicit ghpython conversions, the branches will end up described by
    paths {0;0},{0;1},{0;2}, etc.

    :param stride: the skip factor to apply for subsampling input (default=1, no subsampling)
    """

    # Extract the origin position data and convert to Point3d objects within a
    # Python list structure.  The subsampling is handled within the
    # comprehension.  Note that missing data is returned as None.  Each 'body'
    # in the take.rigid_bodies dictionary is a RigidBody object.  body.positions
    # is a list with one element per frame, either None or [x,y,z].
    origins = [ [rotated_point(pos) for pos in itertools.islice(body.positions, 0, len(body.positions), stride)] \
                for body in take.rigid_bodies.values()]

    # Similar to extract a tree of quaternion trajectories.  The leaves are
    # numbers, the dimensions are (num_bodies, num_frames, 4).
    quats = [ [rotated_orientation(rot) for rot in itertools.islice(body.rotations, 0, len(body.rotations), stride)] \
              for body in take.rigid_bodies.values()]

    # Generate a tree of basis vector pairs (xaxis, yaxis).  Dimensions are (num_bodies, num_frames, 2, 3)
    basis_vectors = [[quaternion_to_xaxis_yaxis(rot) for rot in body] for body in quats]

    # Extract the X axis basis elements into a tree of Vector3d objects with dimension (num_bodies, num_frames).
    xaxes = [[Rhino.Geometry.Vector3d(*(basis[0])) for basis in body] for body in basis_vectors]

    # Same for Y.
    yaxes = [[Rhino.Geometry.Vector3d(*(basis[1])) for basis in body] for body in basis_vectors]

    # Iterate over the 2D list structures, combining them into a 2D list of Plane objects.
    planes = [[plane_or_null(origin, x, y) for origin,x,y in zip(os,xs,ys)] for os,xs,ys in zip(origins, xaxes, yaxes)]

    # Recursively convert from a Python tree to a data tree.
    return list_to_tree(planes)

# optirecv.py : motion capture data receiver for use within Grasshopper ghpython objects

# Copyright (c) 2016, Garth Zeglin. All rights reserved. Licensed under the
# terms of the BSD 3-clause license.

# use RhinoCommon API
import Rhino

# Make sure that the Python libraries that are also contained within this course
# package are on the load path. This adds the python/ folder to the load path
# *after* the current folder.  The path manipulation assumes that this module is
# still located within the Grasshopper/MocapDemo subfolder, and so the package
# modules are at ../../python.
import sys, os
sys.path.insert(1, os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(os.path.dirname(__file__)))), "python"))

# import the Optitrack stream decoder
import optirx

# import a quaternion conversion function
from optitrack.geometry import quaternion_to_xaxis_yaxis

# load the Grasshopper utility functions from the course packages
from ghutil import *

# share the mocap coordinate conversion code with the CSV loader
from optiload import rotated_point, rotated_orientation, plane_or_null

#================================================================
class OptitrackReceiver(object):
    def __init__(self, version_string, ip_address=None):
        # The version string should be of the form "2900" and should match the SDK version of the Motive software.
        # E.g. Motive 1.9 == SDK 2.9.0.0 == "2900"
        #      Motive 1.8 == SDK 2.8.0.0 == "2800"
        self.sdk_version = tuple(map(int,version_string)) # e.g. result is (2,9,0,0)

        # create a multicast UDP receiver socket
        self.receiver = optirx.mkdatasock(ip_address=ip_address)

        # set non-blocking mode so the socket can be polled
        self.receiver.setblocking(0)

        # Keep track of the most recent results.  These are stored as normal Python list structures, but
        # already rotated into Rhino coordinate conventions.
        self.positions = list()  # list of Point3d objects
        self.rotations = list()  # list of [x,y,z,w] quaternions as Python list of numbers
        self.bodynames = list()  # list of name strings associated with the bodies
        self.markers = list()    # list of individual rigid body markers locations
        return

    #================================================================
    def make_plane_list(self):
        """Return the received rigid body frames as a list of Plane or None (for missing data), one entry per rigid body stream."""

        # convert each quaternion into a pair of X,Y basis vectors
        basis_vectors = [quaternion_to_xaxis_yaxis(rot) for rot in self.rotations]

        # Extract the X and Y axis basis elements into lists of Vector3d objects.
        xaxes = [Rhino.Geometry.Vector3d(*(basis[0])) for basis in basis_vectors]
        yaxes = [Rhino.Geometry.Vector3d(*(basis[1])) for basis in basis_vectors]

        # Generate either Plane or None for each coordinate frame.
        planes = [plane_or_null(origin, x, y) for origin,x,y in zip(self.positions, xaxes, yaxes)]
        return planes

    #================================================================
    def get_markers(self):
        return self.markers
    #================================================================
    def _markers_coincide(self, m1, m2):
        """For now, an exact match (could be fuzzy match)."""
        return m1[0] == m2[0] and m1[1] == m2[1] and m1[2] == m2[2]
        
    def _identify_rigid_bodies(self, sets, bodies):
        """Compare marker positions to associate a named marker set with a rigid body.
        :param sets: dictionary of lists of marker coordinate triples
        :param bodies: list of rigid bodies
        :return: dictionary mapping body ID numbers to body name

        Some of the relevant fields:
        bodies[].markers  is a list of marker coordinate triples
        bodies[].id       is an integer body identifier with the User Data field specified for the body in Motive
        """

        # for now, do a simple direct comparison on a single marker on each body
        mapping = dict()
        for body in bodies:
            marker1 = body.markers[0]
            try:
                for name,markerset in sets.items():
                    if name != 'all':
                        for marker in markerset:
                            if self._markers_coincide(marker1, marker):
                                mapping[body.id] = name
                                raise StopIteration
            except StopIteration:
                pass

        return mapping
                         
    #================================================================
    def poll(self):
        """Poll the mocap receiver port and return True if new data is available."""
        try:
            data = self.receiver.recv(optirx.MAX_PACKETSIZE)
        except:
            return False

        packet = optirx.unpack(data, version=self.sdk_version)

        if type(packet) is optirx.SenderData:
            version = packet.natnet_version
            print "NatNet version received:", version

        elif type(packet) is optirx.FrameOfData:
            nbodies = len(packet.rigid_bodies)
            # print "Received frame data with %d rigid bodies." % nbodies
            # print "Received FrameOfData with sets:", packet.sets
            # There appears to be one marker set per rigid body plus 'all'.
            # print "Received FrameOfData with names:", packet.sets.keys()
            # print "First marker of first marker set:", packet.sets#.values()#[0][0]
            # print "Received FrameOfData with rigid body IDs:", [body.id for body in packet.rigid_bodies]
            # print "First marker of first rigid body:", packet.rigid_bodies[0].markers[0]
            # print "First tracking flag of first rigid body:", packet.rigid_bodies[0].tracking_valid

            # compare markers to associate the numbered rigid bodies with the named marker sets
            mapping = self._identify_rigid_bodies( packet.sets, packet.rigid_bodies)
            # print "Body identification:", mapping
            
            if nbodies > 0:
                # print packet.rigid_bodies[0]

                # rotate the coordinates into Rhino conventions and save them in the object instance as Python lists
                self.positions = [ rotated_point(body.position) for body in packet.rigid_bodies]
                self.rotations = [ rotated_orientation(body.orientation) for body in packet.rigid_bodies] 
                self.bodynames = [ body.id for body in packet.rigid_bodies]
                # self.markers = [rotated_point(point) for point in body.markers for body in packet.rigid_bodies]
                # self.markers = [body.markers for body in packet.rigid_bodies]
                captureSet = []
                for body in packet.rigid_bodies:
                    markerSet = []
                    for marker in body.markers:
                        markerSet.append(rotated_point(marker))
                    captureSet.append(markerSet)
                self.markers = captureSet
                
                # return a new data indication
                return True

        elif type(packet) is optirx.ModelDefs:
            print "Received ModelDefs:", packet

        else:
            print "Received unhandled NatNet packet type:", packet
            
        # else return a null result
        return False


#================================================================
def frames_to_tree(frame_list):
    """Utility function to convert a list of list of Plane objects representing a trajectory segment into a GH data tree."""

    # Transpose the frame list for output.  As accumulated, it is a list of lists:
    # [[body1_sample0, body2_sample0, body3_sample0, ...], [body1_sample1, body2_sample1, body3_sample1, ...], ...]
    segment = zip(*frame_list)

    # Convert a Python list-of-lists into a data tree.  Segment is a list of trajectories:
    # [[body1_sample0, body1_sample1, body1_sample2, ...], [body2_sample0, body2_sample1, body2_sample2, ...], ...]
    return list_to_tree(segment)

#================================================================

# pointfilter.py : filtering operations on Point3d trajectories

# Copyright (c) 2016, Garth Zeglin. All rights reserved. Licensed under the
# terms of the BSD 3-clause license.

# normal Python packages
import math

# use RhinoCommon API
import Rhino

# Define a Savitzky-Golay filter for estimating acceleration, assuming a 120Hz sampling rate.
# See generate_filter_coefficients.py for details.
accel_filter_coeff = [ 872.727273, 218.181818, -249.350649, -529.870130, -623.376623, -529.870130, -249.350649, 218.181818, 872.727273]

class Point3dFilter(object):

    def __init__(self, length):
        # length of the estimation filter
        self._filter_len = len(accel_filter_coeff)

        # length of the fixed-length history buffer
        self._buf_len = length

        # Keep track of the number of samples observed.
        # N.B. the most recent (last) point in the buffer has sample index = self.samples - 1
        #      the   oldest (first)   point in the buffer has sample index = self.samples - self._buf_len
        self.samples = 0

        # number of samples to ignore after an event
        self.blanking = 60

        # sample number for the last 'event' observed
        self.last_event = None

        # create empty fixed-length data buffers
        self.reset()

        return

    #================================================================
    def reset(self):
        """Reset filter state."""
        self._position  = [None] * self._buf_len
        self._accel     = [None] * self._buf_len
        self._accel_mag = [None] * self._buf_len

        # reset the blanking interval
        self.last_event = self.samples - self.blanking

        return

    #================================================================
    def add_points(self, point_list):
        """Given a list of objects which are either Point3d or None, append them to the
        fixed-length filter history buffer.  Accelerations are not computed for
        filter windows including null samples.
        """

        # Compute the pieces of the old and new buffers to concatenate,
        # considering the possibility there may be an excess of new data.
        num_new_samples = min(self._buf_len, len(point_list))
        first_source_sample = len(point_list) - num_new_samples
        first_new_dest_sample = self._buf_len - num_new_samples
        self.samples += len(point_list)

        # Construct a new position buffer by concatenating sections of the old
        # and new lists.  (Note that a ring buffer would be more efficient for a
        # large buffer size.)
        self._position = self._position[num_new_samples:] + point_list[first_source_sample:]

        # Compute acceleration vectors for the new data.
        new_accel = [self._estimate_acceleration(p) for p in range(first_new_dest_sample, self._buf_len)]
        self._accel = self._accel[num_new_samples:] + new_accel

        # Compute acceleration magnitudes for the new data.
        new_mag = [(math.sqrt(a*a) if a is not None else None) for a in new_accel]
        self._accel_mag = self._accel_mag[num_new_samples:] + new_mag

        return

    #================================================================
    def _estimate_acceleration(self, pos):
        """Estimate the acceleration vector for the given buffer position.  Returns None
        if acceleration is not computable, either due to missing data, or a
        position for for which not enough samples are present for the filter
        length.
        :param pos:  buffer position for which to compute acceleration, e.g. self._buf_len-1 is the newest data point
        :return:     Rhino Vector3d object with acceleration, or None if not possible
        """

        # Identify the set of points to filter, returning None if any required
        # points are out of range.
        first_datum = pos - self._filter_len + 1
        if first_datum < 0 or pos >= self._buf_len:
            return None

        # Check for null values, returning None if any are found
        pts = self._position[first_datum:first_datum+self._filter_len]
        if None in pts:
            return None

        # Compute the dot product of the filter coefficients and the point history.
        accel = Rhino.Geometry.Point3d(0,0,0)
        for pt,coeff in zip(pts, accel_filter_coeff):
            accel = accel + (pt * coeff)

        # A Point multiplied by a scalar is a Point, but the output of the
        # filter should be interpreted as a Vector.
        return Rhino.Geometry.Vector3d(accel)

    #================================================================
    def find_accel_peak(self):
        """Finds the peak acceleration within the current time history.  The
        offset is the negative time position in samples; 0 is the most
        current data, -1 one sample prior, etc.

        :return:   (offset, magnitude, acceleration, position) tuple
        """

        maximum = max(self._accel_mag)
        idx = self._accel_mag.index(maximum)
        offset = idx - self._buf_len + 1
        return offset, maximum, self._accel[idx], self._position[idx]

    #================================================================
    def detect_acceleration_event(self, threshold):
        """Check the point trajectory buffer for a new event in which the acceleration
        magnitude is greater than a threshold.  A blanking interval is applied
        after each event to suppress multiple returns from spiky signals.  A
        sample offset of zero means the most recent point is the peak, other
        offsets are negative.

        :return: None, or the integer sample offset of the peak
        """

        # check blanking interval to see which samples can be inspected
        first_buffer_sample = self.samples - self._buf_len
        after_blanking = self.last_event + self.blanking
        first_checked_sample = max(first_buffer_sample, after_blanking)

        # if the next data to inspect is still in the future, return False
        if first_checked_sample >= self.samples:
            return None

        # Find the maximum within the valid range.  Note that if all values are None this may return None.
        first_valid_index = first_checked_sample - first_buffer_sample
        maximum = max(self._accel_mag[first_valid_index:])

        if maximum is not None and maximum > threshold:
            # compute the offset of the maximum
            idx = self._accel_mag[first_valid_index:].index(maximum)
            offset = first_valid_index + idx + 1 - self._buf_len

            # start the blanking interval
            self.last_event = self.samples - 1 + offset

            # and return a valid peak indication
            return offset

        else:
            return None

"""\
pythonlibpath.py : utility module to ensure the course Python library is included in the load path.

The add_library_path function should be run from any ghpython object which needs
to directly import a module from the course library. This file should be present
in the same folder as the .gh file so it can be found using the default ghpython
load path.
"""

# Make sure that the Python libraries that are also contained within this course
# package are on the load path. This adds the python/ folder to the load path
# *after* the current folder.  The path manipulation assumes that this module is
# still located within a Grasshopper/* subfolder, and so the package modules are
# at ../../python.

# In Rhino Python, sys.path does not appear to be shared *between* ghpython
# blocks, but *is* persistent across invocations of the same block.

# However, the module cache *is* shared, so different blocks may not see the
# same value of sys.path, but they will use a cached module during import.  For
# this reason, if only one block successfully sets sys.path and loads a library
# module, other blocks can also import it even if sys.path doesn't include the
# library.  However, it is tricky to guarantee evaluation order of ghpython
# blocks, so it isn't reliable to depend upon other scripts having imported a
# module first.

import sys, os

def add_library_path():
    library_path = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(os.path.dirname(__file__)))), "python")
    if sys.path[1] != library_path:
        sys.path.insert(1, library_path)

#!/usr/bin/env python
"""\
generate_filter_coefficients.py : use scipy to generate coefficients for an estimation filter

This script generates an FIR filter for estimating acceleration from a uniformly sampled signal.

It uses the scipy libraries, so it doesn't work within Rhino Python; this must
be run using a CPython installation which has scipy installed.

References:

  https://en.wikipedia.org/wiki/Savitzky%E2%80%93Golay_filter
  http://docs.scipy.org/doc/scipy-0.16.1/reference/generated/scipy.signal.savgol_coeffs.html

"""

import numpy as np
import scipy.signal

window_length = 9       # filter length (number of samples required, must be an odd number)
polyorder     = 2       # fit a quadratic curve
sampling_rate = 120     # Hz

if __name__=="__main__":

    coeff = scipy.signal.savgol_coeffs( window_length = window_length, \
                                     polyorder = polyorder, \
                                     deriv = 2, \
                                     delta = (1.0/sampling_rate), \
                                     pos = window_length-1,
                                     use = 'dot')

    # emit a single line of Python to insert in the code
    print "accel_filter_coeff = [",
    for c in coeff[:-1]:
        print ("%f," % c),
    print "%f]" % coeff[-1]
    
# accel_filter_coeff = [ 872.727273, 218.181818, -249.350649, -529.870130, -623.376623, -529.870130, -249.350649, 218.181818, 872.727273]