#!/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 mode_rmrc(env) :
  #deactivate_all()
  #disconnect_all(env)
  #RH2ArmControl.inports['InArmVelocity'].close()
  #RH2ArmControl.outports['OutArmAngle'].close()
  #time.sleep(2)
  #connect_list(env,[0,1,2,3,4,5])
  activate_seq([RH2ArmControl, RMRC, ACT_PA10_RH, HandWrap, 3])
  set_tool(tool_data)
  RMRC.services['RMRCProv'].provided['com_frm_ctrl'].ref.set_param(1.0, 100, 200)
##
#
def arm_release(env) :
  deactivate_all()
  #disconnect_all(env)
  #RH2ArmControl.inports['InArmVelocity'].close()
  #RH2ArmControl.outports['OutArmAngle'].close()
#
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()
##
# 各軸の関節
#
#j_park = [0.0,0.0,0.0,0.0,0.0,0.0,0.0]
#j_ready = [0.0,0.5,1.6,0.0,1.04,0.0,0.0]
#j_squat = [0.0,-1.13,2.6,0.0,0.9,0.0,0.0]
#j_escape = [0.0,2.0,0.0,0.0,0.0,0.0,0.0]
j_park=[0.0,0.0,0.0,0.0,0.0,0.0,0.0]
j_ready=[0.0,28.7,91.7,0.0,59.6,0.0,0.0]
j_squat = [0.0,-60.3,144.0,0.0,51.6,0.0,0.0]
j_escape = [0.0,114.6,0.15,0.0,0.0,0.0,0.0]
j_holder = [-142.0, 28.0, 55.0, 0.0, 94.0, -143.0, 3.0]
j_table1 = [89.0, 19.0, 62.0, 0.0, 98.0, 0.0, 0.0]
j_table2 = [45.0, 19.0, 62.0, 0.0, 98.0, 0.0, 0.0]
j_trushbox = [0.0, 52.7, 36.0, 0.0, 91.0, 0.1, 0.0]
#
# 角度制御
#   jlist:各軸の関節 List型 [x,y,z,a,b,c](mm,degree)
def move_joint(jlist) :
   global m_dseq
   for i in range(6):
     m_dseq.data[i] = jlist[i]*pi/180.0
   RMRC.services['RMRCProv'].provided['com_move'].ref.move_joint(m_dseq)
##
#
def get_joint():
  RH2ArmControl.outports['OutArmAngle'].open()
  th = RH2ArmControl.outports['OutArmAngle'].read().data
  for i in range(len(th)):
    th[i] = th[i] * 180.0 / pi
  RH2ArmControl.outports['OutArmAngle'].close()
  return th

##
# 手先の現在位置・姿勢を取得
def get_pos():
   cur_pos = RMRC.services['RMRCProv'].provided['com_fk'].ref.get_cur_frm()
   print"current position:"
   print" ROT MATRIX"
   for i in range(3):
     for j in range(3):
       print"  %.3f  " %cur_pos.mat[i][j],  
     print""
   print" POSITION"
   for i in range(3):
     print"  %.3f  " %cur_pos.pos[i],  
   print""
   
##
# 手先ツール長設定
#   tool:ツール(把持点)オフセット List型 [x,y,z](mm)
def set_tool(tool):
   global m_xyz, m_abc
   for i in range(3):
     m_xyz.pos[i] = tool[i]
   for i in range(3):
     m_abc.pos[i] = 0.0
   tl = ACT_PA10_RH.services['ACTProv'].provided['com_ACT'].ref.createCarPosWithElbow(m_xyz,m_abc)
   ACT_PA10_RH.services['MiddleProv'].provided['ManipulatorCommonInterface_Middle'].ref.setControlPointOffset(tl.carPos)

##
# 角速度制御
#    pos:移動目標値 List型 [x,y,z,a,b,c](mm,degree)
def go_to(pos) :
   global m_xyz, m_abc
   for i in range(3):
     m_xyz.pos[i] = pos[i]
     m_abc.pos[i] = pos[i+3]
   m_pos = ACT_PA10_RH.services['ACTProv'].provided['com_ACT'].ref.createCarPosWithElbow(m_xyz,m_abc)
   ACT_PA10_RH.services['MiddleProv'].provided['ManipulatorCommonInterface_Middle'].ref.moveLinearCartesianAbs(m_pos)
##
# 連速移動（角速度制御）
def demo(pos_list) :
     for pos in pos_list :
       go_to(pos)
##
# グリッパ開口
def gripper_open() :
   ACT_PA10_RH.services['MiddleProv'].provided['ManipulatorCommonInterface_Middle'].ref.openGripper()
##
# グリッパ閉口
def gripper_close() :
   ACT_PA10_RH.services['MiddleProv'].provided['ManipulatorCommonInterface_Middle'].ref.closeGripper()
##
# グリッパ開閉停止
def gripper_stop() :
   HandWrap.services['ComRHHandWrap'].provided['com_hand'].ref.StopHand()
##
# グリッパ状態取得
def get_cond() :
   cond = HandWrap.services['ComRHHandWrap'].provided['com_hand'].ref.GetHandCond()
   if (cond == 0) :
     print "Close completed!"
   elif (cond == 1) :
     print "Open completed!"
   elif (cond == 2) :
     print "Now moving!"
   elif (cond == 3) :
     print "Interrupted!"
##
# グリッパ動作モード取得
def get_mode() :
   mode = HandWrap.services['ComRHHandWrap'].provided['com_hand'].ref.GetHandMode()
   if (mode == 0) :
     print "Mode : READY"
   elif (mode == 1) :
     print "Mode : OPEN"
   elif (mode == 2) :
     print "Mode : CLOSE"
##
# 荷台ドリンクの把持
#    menu: 空き缶名 str型
#    flg: 動作開始時のグリッパの状態把握に使用
def holder_pickup(menu, flg=0) :
   global pc,pd,pe,drink
   if (flg == 0) :
      if (get_cond() != 1) :
         gripper_open()
      move_joint(j_holder)
   go_to(offset(drink[menu],0.0,0.0,110.0))
   go_to(drink[menu])
   go_to(offset(drink[menu],0.0,-7.0,0.0))
   gripper_close()
   time.sleep(4)
   go_to(offset(drink[menu],0.0,-7.0,110.0))
##
# オフセットの設定
#    pos: 移動目標値 List型 [x,y,z]
#    x_offset: x方向のオフセット
#    y_offset: y方向のオフセット
#    z_offset: z方向のオフセット
def offset(pos,x_offset,y_offset,z_offset) :
   off = [x_offset,y_offset,z_offset, 0, 0, 0]
   abv_pos = copy.deepcopy(pos)
   abv_pos = map((lambda x,y:x+y),abv_pos,off)
   return abv_pos
##
# ホルダから空き缶回収
#   trush: 空き缶名 str型
#   flg: holder_pickup()で使用する
def trush_clean(trush, flg=0) :
   print "pick up a can on my back."
   holder_pickup(trush, flg)
   move_joint(j_trushbox)
   print "and carry it to a trush box."
   gripper_open()
   time.sleep(8)

##
# 最大関節動作速度の設定
def set_joint_vel(vel):
  global m_vel
  for i in range(7):
    m_vel.data[i] = vel
  RMRC.services['RMRCProv'].provided['com_move'].ref.set_max_joint_vel(m_vel)

#
# create env
#
env = RtmEnv(sys.argv,[Rh2ArmNS])
for ns in env.name_space :
  env.name_space[ns].list_obj()
assign_variables(env)
mode_rmrc(env)
set_joint_vel(0.8)

