Weifang2::Shuttle

from __future__ import print_function
from __future__ import unicode_literals

import csv
from io import StringIO
from itertools import cycle
from collections import namedtuple

from vcScript import *
import vcVector
import vcMatrix


INIT = False

def printVec(vec):
  """
  Usefull utility function for printing matrix value
  """
  fmt = "{:<6.3f}\t{:<6.3f}\t{:<6.3f}\t{:<6.3f}"
  print(fmt.format(vec.X, vec.Y, vec.Z, vec.W))


def printMatrix(mat):
  """
  Usefull utility function for printing matrix value
  """
  print('  X\t  Y\t  Z\t  W')
  for Vec in [mat.N, mat.O, mat.A, mat.P]:
    printVec(Vec)


class Action(object):
    wp = 'WP'
    stop = 'STOP'


class vcShuttle(object):

  def __init__(self):
    global INIT
    vehicle = getComponent().findBehaviour('Vehicle')
    vehicle.clearMove()
    vehicle.detachAllWagons()
    vehicle.Acceleration = 1000
    vehicle.Deceleration = 1000
    vehicle.MaxSpeed = 2000
    vehicle.Interpolation = 0.13
    if not INIT:
      INIT = True
      vehicle.OnFinalDestinationReached = self.target_reached
    self.vehicle = vehicle
    self.trigger = None

  def add(self, position):
    self.vehicle.addControlPoint(position)

  def clear(self):
    self.vehicle.clearMove()

  def target_reached(self, comp, duration):
    print(comp.Name, 'reached in', round(duration, 1), 's')
    if self.trigger:
      print('Trigger:', self.trigger.Name)

  def trigger_target(self, comp_name, frame_name):
    app = getApplication()
    comp = app.findComponent(comp_name)
    frame = comp.findFeature(frame_name)
    station = app.findComponent(frame.Name)
    self.trigger = station


class Shuttle(object):
  sid = 0

  @classmethod
  def _enumerate(cls):
    cls.sid += 10
    return cls.sid

  def __init__(self, paths, sid=None):
    self.sid = sid or self._enumerate()
    self.paths = paths
    self.vcShuttle = vcShuttle()
    self.trigger()

  def trigger(self, *args):
    self.vcShuttle.clear()
    for position in self.paths:
      self.vcShuttle.add(position)
    last = self.paths.last
    self.vcShuttle.trigger_target(*last)


class Path(object):
  def __init__(self, comp, frame, action):
    self.comp_name = comp
    self.frame_name = frame

    comp = getApplication().findComponent(comp)
    frame = comp.getFeature(frame)
    npm = frame.NodePositionMatrix
    self.frame = frame
    #self.position = wpm * frame.PositionMatrix.P
    self.position = npm.P
    self.action = action


class Paths(object):
  def __init__(self, paths, initial=0):
    if not any([p.action == Action.stop for p in paths]):
      last_path = paths[-1]
      last_path.action = Action.stop
      paths[-1] = last_path

    self._paths = iter(cycle(paths))
    self._last = None
    [next(self._paths) for _ in range(initial)]

  @classmethod
  def from_csv(cls, input_string):
    fake_fd = StringIO(input_string)
    reader = csv.reader(fake_fd)
    next(reader)
    positions = []
    for comp, frame, action in reader:
      path = Path(comp, frame, action)
      positions.append(path)
    return cls(positions)

  def __iter__(self):
    for path in self._paths:
      yield path.position
      self._last = (path.comp_name, path.frame_name)
      if path.action == Action.stop:
        break

  @property
  def last(self):
    return self._last


def OnSignal(signal):
  print('Got signal', signal.Name if signal else None)
  if signal and signal.Value:
    global_context['Shuttle'].trigger()


def OnRun():
  paths = Paths.from_csv(csv_data)
  shuttle = Shuttle(paths)
  global_context['Shuttle'] = shuttle
  comp = getComponent()
  start_button = comp.getProperty('Start')
  start_signal = comp.getBehaviour('Start')
  start_button.OnChanged = start_signal.signal


global_context = {'Shuttle': None}

csv_data = """
comp,frame,action
Transfersystem,B60,WP
Transfersystem,B61,WP
Transfersystem,B62,WP
Transfersystem,Station 10,STOP
Transfersystem,Station 20,STOP
Transfersystem,B1,WP
Transfersystem,E1,WP
Transfersystem,B2,WP
Transfersystem,G1,WP
Transfersystem,B3,WP
Transfersystem,E2,WP
Transfersystem,B4,WP
Transfersystem,Station 30,STOP
Transfersystem,Station 40,STOP
Transfersystem,B5,WP
Transfersystem,E3,WP
Transfersystem,B6,WP
Transfersystem,B7,WP
Transfersystem,E4,WP
Transfersystem,B8,WP
Transfersystem,Station 50,STOP
Transfersystem,Station 60,STOP
Transfersystem,B9,WP
Transfersystem,E5,WP
Transfersystem,B51,WP
Transfersystem,B52,WP
Transfersystem,E6,WP
Transfersystem,B10,WP
Transfersystem,Station 70,WP
Transfersystem,Station 80,WP
Transfersystem,Station 80,WP
Transfersystem,B20,WP
Transfersystem,B21,WP
Transfersystem,B22,WP
Transfersystem,B23,WP
Transfersystem,B24,WP
Transfersystem,B25,WP
Transfersystem,Station 90,STOP
Transfersystem,B30,WP
Transfersystem,B31,WP
Transfersystem,B32,WP
Transfersystem,B33,WP
Transfersystem,B34,WP
Transfersystem,B35,WP
Transfersystem,Hochregallager 1,STOP
Transfersystem,Hochregallager 2,STOP
Transfersystem,B40,WP
Transfersystem,B41,WP
Transfersystem,B42,WP
""".strip()

#def getPath(comp_name, owp_name):
#  comp = getApplication().findComponent(comp_name)
#  if comp:
#    owp = comp.findBehaviour(owp_name)
#    if owp:
#      return owp.Path
#  return []

#def getPositions(comp_name, owp_name):
#  return [pos.PositionMatrix.P for pos in getPath(comp_name, owp_name)]

#pos = getPositions('Transfersystem', 'OneWayPath')
#print(pos)