#!/usr/bin/env python
# -*- Python -*-

import sys
import time
import os

#
# set up user environment
#  RtmToolsDir, RtmDir, etc.
#
from set_env import *   

#
# import user tools
#
sys.path.append(".")
save_path = sys.path[:]
sys.path.append(Rh2LibDir)
from rtc_handle import *
sys.path = save_path

#
# import stub files
# 
#import _GlobalIDL

def assign_variables(env) :
  # rtc
  global rh2,odometry,positioninput,pathfollower
  global joystick,switch
  global camera,ceilingnav,dispposition,localization
  global navigation,pathplanning
  rh2 = env.name_space[Rh2BaseNS1].rtc_handles['RefHardRh20.rtc']
  odometry = env.name_space[Rh2BaseNS1].rtc_handles['Odometry0.rtc']
  navigation = env.name_space[Rh2BaseNS1].rtc_handles['Navigation0.rtc']
  pathfollower = env.name_space[Rh2BaseNS1].rtc_handles['PathFollower0.rtc']
  positioninput =env.name_space[Rh2BaseNS3].rtc_handles['PositionInput0.rtc']
  pathplanning = env.name_space[Rh2BaseNS3].rtc_handles['PathPlanning0.rtc']
  localization = env.name_space[Rh2BaseNS1].rtc_handles['LocalizeCenter0.rtc']
  switch = env.name_space[Rh2BaseNS1].rtc_handles['SwitchInputRTC0.rtc']
  joystick = env.name_space[Rh2BaseNS3].rtc_handles['GamePad0.rtc']
  camera = env.name_space[Rh2BaseNS2].rtc_handles['CameraEye0.rtc']
  ceilingnav = env.name_space[Rh2BaseNS2].rtc_handles['CeilingNavigation0.rtc']
  dispposition = env.name_space[Rh2BaseNS3 ].rtc_handles['DispPosition0.rtc']

def make_connectors(env):
  env.con_list=[]
  ## [PositionInput] -> [PathPlanning]
  con = IOConnector([positioninput.outports['position'],pathplanning.inports['goal']])
  env.con_list.append(con)
  ## [PathPlanning] -> [Navigation]
  con = IOConnector([pathplanning.outports['min_path'],navigation.inports['min_path']])
  env.con_list.append(con)
  ## [Navigation] -> [PathFollower]
  con = IOConnector([navigation.outports['path'],pathfollower.inports['target_path']])
  env.con_list.append(con)
  ## [SwitchComp] -> [RefHard2]
  con=IOConnector([switch.outports['outVel'],rh2.inports['InTargetVelocity']])
  env.con_list.append(con)
  ## [RefHard2] -> [Odometry]
  con=IOConnector([rh2.outports['OutWheelAngle'],odometry.inports['CurrentWheelAngle']])
  env.con_list.append(con)
  ## [Camera] -> [CeilingNav]
  con=IOConnector([camera.outports['ImageData'],ceilingnav.inports['CameraData']])
  env.con_list.append(con)
  ## [CeilingNav] -> [LocalizeCenter]
  con=IOConnector([ceilingnav.outports['CeilingPosition'],localization.inports['dp_ceilIn']])
  env.con_list.append(con)
  ## [Odometry] -> [LocalizeCenter]
  con=IOConnector([odometry.outports['OdometryPosition'],localization.inports['dp_odmIn']])
  env.con_list.append(con)
  ## [LocalizeCenter] -> [Odometry]
  con=IOConnector([localization.outports['dp_out0'],odometry.inports['LocalizedPosition']])
  env.con_list.append(con)
  ## [LocalizeCenter] -> [CeilingNav]
  con=IOConnector([localization.outports['dp_out0'],ceilingnav.inports['LocalizedPosition']])
  env.con_list.append(con)
  ## [LocalizeCenter] -> [PathPlanning]
  con = IOConnector([localization.outports['dp_out0'],pathplanning.inports['current_position']])
  env.con_list.append(con)
  ## [LocalizeCenter] -> [Navigation]
  con = IOConnector([localization.outports['dp_out0'],navigation.inports['position']])
  env.con_list.append(con)
  ## [LocalizeCenter] -> [PathFollower]
  con = IOConnector([localization.outports['dp_out0'],pathfollower.inports['position']])
  env.con_list.append(con)
  ## [Joystick] -> [SwitchComp]
  con=IOConnector([joystick.outports['VelocityIIS'],switch.inports['inVelJoy']])
  env.con_list.append(con)
  ## [LocalizeCenter] -> [DispPosition]
  con=IOConnector([localization.outports['dp_out0'],dispposition.inports['position']])
  env.con_list.append(con)
  ## [PathFollower] -> [SwitchComp]
  con = IOConnector([pathfollower.outports['velocity'],switch.inports['inVelAuto']])
  env.con_list.append(con)

#
# connection control
#
def connect_all(env) :
  for con in env.con_list :
    con.connect()
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_local() :
  camera.activate()
  time.sleep(1)
  ceilingnav.activate()
  switch.activate()
  rh2.activate()
  odometry.activate()
  localization.activate()
def deactivate_local() :
  camera.deactivate()
  switch.deactivate()
  ceilingnav.deactivate()
  rh2.deactivate()
  odometry.deactivate()
  localization.deactivate()
def activate_all() :
  rh2.activate()
  camera.activate()
  time.sleep(1)
  ceilingnav.activate()
  odometry.activate()
  localization.activate()
  joystick.activate()
  switch.activate()
  pathplanning.activate()
  navigation.activate()
  pathfollower.activate()
  dispposition.activate()
def deactivate_all() :
  rh2.deactivate()
  camera.deactivate()
  ceilingnav.deactivate()
  odometry.deactivate()
  localization.deactivate()
  joystick.deactivate()
  switch.deactivate()
  pathplanning.deactivate()
  navigation.deactivate()
  pathfollower.deactivate()
  dispposition.deactivate()

def moveto(goal):
  pos=['start','dock','entrance','pa10','drinkserve','room', 'dumpsite']
  g_flg=0
  for i in range(len(pos)):
    if(goal == pos[i]):
      g_pos=pos[i]
      print "goal position:" + g_pos
      g_flg=1
  if(g_flg == 1):
      positioninput.conf_ref.activate_configuration_set(g_pos)
      positioninput.activate()
  else:
      print "error:start or goal is invalid."

def stop():
  positioninput.deactivate()

#
# create env
#
#env = RtmEnv(sys.argv,[Rh2BaseNS1,Rh2BaseNS2,Rh2BaseNS3])
#raw_input("are you sure that each rtc needed is running?")
#for ns in env.name_space :
#  env.name_space[ns].list_obj()
#assign_variables(env)
#make_connectors(env)
#connect_all(env)
