#!/usr/bin/env python
# -*- coding: utf-8 -*-
# -*- Python -*-

import sys
import time
import copy
import subprocess

#
# set up user environment
#  RtmToolsDir, MyRtcDir, etc.
#
from set_env import *
from armpos_data import *
#
# import user tools
#
save_path = sys.path[:]
sys.path.append(Rh2LibDir)
from rtc_handle import *
sys.path.append(Rh2LibDir+'/tools/geo')
from geo import *

#
# import stub files
# 
sys.path.append(Rh2IdlDir)
import _GlobalIDL
sys.path = save_path

def assign_variables(env) :
  # rtc
  global RH2ArmControl, RHdisp, RMRC, HandWrap, ACT_PA10_RH
  RMRC = env.name_space[Rh2ArmNS].rtc_handles['RMRC0.rtc']
  RH2ArmControl = env.name_space[Rh2ArmNS].rtc_handles['RH2ArmControl0.rtc']
  ACT_PA10_RH = env.name_space[ Rh2ArmNS ].rtc_handles['ACT_PA10_RH0.rtc']
  ACT_PA10_RH.services['MiddleProv'].provided['ManipulatorCommonInterface_Middle'].narrow_ref(globals())
  ACT_PA10_RH.services['ACTProv'].provided['com_ACT'].narrow_ref(globals())
  HandWrap = env.name_space[Rh2ArmNS].rtc_handles['RH_HandWrapper0.rtc']
  HandWrap.services['ComRHHandWrap'].provided['com_hand'].narrow_ref(globals())
  RMRC.services['RMRCProv'].provided['com_frm_ctrl'].narrow_ref(globals())
  RMRC.services['RMRCProv'].provided['com_fk'].narrow_ref(globals())
  RMRC.services['RMRCProv'].provided['com_move'].narrow_ref(globals())
  RMRC.conf_ref.activate_configuration_set('RefHardArm')
  # data & class
  global c_frame, c_xyz, c_abc, c_dseq, m_xyz, m_abc, m_dseq, tool_data, c_vel, m_vel
  c_frame=_GlobalIDL.Frame
  c_xyz = _GlobalIDL.Vector
  c_abc = _GlobalIDL.Vector
  c_dseq =_GlobalIDL.DoubleSeq
  c_vel =_GlobalIDL.DoubleSeq
  m_xyz = c_xyz(VECTOR())
  m_abc = c_abc(VECTOR())
  m_dseq = c_dseq([0.0,0.0,0.0,0.0,0.0,0.0,0.0])
  m_vel = c_vel([0.0,0.0,0.0,0.0,0.0,0.0,0.0])
  tool_data = [0,0,130.5]

def make_connectors(env):
  env.con_list=[]
#0
  con = IOConnector([RH2ArmControl.outports['OutArmAngle'],HandWrap.inports['ThPos']])
  env.con_list.append(con)
#1
  con = IOConnector([HandWrap.outports['Th'],RMRC.inports['Th']])
  env.con_list.append(con)
#2
  con = IOConnector([HandWrap.outports['JointGripVel'],RH2ArmControl.inports['InArmVelocity']])
  env.con_list.append(con)
#3
  con = IOConnector([RMRC.outports['Vel'],HandWrap.inports['JointVel']])
  env.con_list.append(con)
#4
  con = ServiceConnector([ACT_PA10_RH.services['RMRCCons'],RMRC.services['RMRCProv']])
  env.con_list.append(con)
#5
  con = ServiceConnector([ACT_PA10_RH.services['HandCons'],HandWrap.services['ComRHHandWrap']])
  env.con_list.append(con)


#
# ポート接続
#
def disconnect_all(env) :
  for con in env.con_list :
    con.disconnect()
def connect_list(env,ll) :
  for i in ll :
    env.con_list[i].connect()
def disconnect_list(env,ll) :
  for i in ll :
    env.con_list[i].disconnect()
##
#

def activate_seq(a_seq) :
  for tmp in a_seq :
    if isinstance(tmp,RtcHandle) :
      tmp.activate()
    elif isinstance(tmp, int) :
      time.sleep(tmp)
    elif isinstance(tmp, float) :
      time.sleep(tmp)
    else :
      eval(tmp)
#
def deactivate_all() :
  RH2ArmControl.deactivate()
  RMRC.deactivate()
  HandWrap.deactivate()
  ACT_PA10_RH.deactivate()

#
# create env
#
env = RtmEnv(sys.argv,[Rh2ArmNS])
for ns in env.name_space :
  env.name_space[ns].list_obj()
assign_variables(env)
make_connectors(env)
connect_list(env,[0,1,2,3,4,5])

