#!/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

#
# run components
#
def run_components() :
  com_rh="exec gnome-terminal "
  com_rh+=" --tab-with-profile=default --working-directory="+Rh2RtcDir+" -t Odometry -e \"./OdometryComp -f "+Rh2ConfDir+"/Odometry.conf\""
  com_rh+=" --tab-with-profile=default --working-directory="+Rh2RtcDir+" -t LocalizeCenter -e \"./LocalizeCenterComp -f "+Rh2ConfDir+"/LocalizeCenter.conf\""
  com_rh+=" --tab-with-profile=default --working-directory="+Rh2BinDir+"/script/openhrp3 -t bridge -e \"sh bridge_withoutSensor.sh\""
  os.popen2(com_rh)

  com_sj="exec gnome-terminal "
  com_sj+=" --tab-with-profile=default --working-directory="+Rh2RtcDir+" -t Navigation -e \"./NavigationComp -f "+Rh2ConfDir+"/Navigation.conf\""
  com_sj+=" --tab-with-profile=default --working-directory="+Rh2RtcDir+" -t PathFollower -e \" ./PathFollowerComp -f "+Rh2ConfDir+"/PathFollower.conf\""
  com_sj+=" --tab-with-profile=default --working-directory="+Rh2RtcDir+" -t SwitchInput -e \" ./SwitchInputRTCComp -f "+Rh2ConfDir+"/SwitchInput.conf\""
  com_sj+=" --tab-with-profile=default --working-directory="+Rh2RtcDir+" -t Tmvel2D2Tmvel -e \" ./Tmvel2D2TmvelComp -f "+Rh2ConfDir+"/Tmvel2D2Tmvel.conf\""
  os.popen2(com_sj)

def assign_variables(env) :
  # rtc
  global odometry, navigation, pathfollower
  global motor, positioninput, pathplanning
  global localization, switch, joystick, dispposition
  global tmvel2d2tmvel
  odometry = env.name_space[Rh2BaseNS3].rtc_handles['Odometry0.rtc']
  navigation = env.name_space[Rh2BaseNS3].rtc_handles['Navigation0.rtc']
  pathfollower = env.name_space[Rh2BaseNS3].rtc_handles['PathFollower0.rtc']
  motor = env.name_space[Rh2BaseNS3].rtc_handles['MotorControl0.rtc']
  positioninput =env.name_space[Rh2BaseNS3].rtc_handles['PositionInput0.rtc']
  pathplanning = env.name_space[Rh2BaseNS3].rtc_handles['PathPlanning0.rtc']
  localization = env.name_space[Rh2BaseNS3].rtc_handles['LocalizeCenter0.rtc']
  switch = env.name_space[Rh2BaseNS3].rtc_handles['SwitchInputRTC0.rtc']
  joystick = env.name_space[Rh2BaseNS3].rtc_handles['GamePad0.rtc']
  dispposition = env.name_space[Rh2BaseNS3].rtc_handles['DispPosition0.rtc']
  tmvel2d2tmvel = env.name_space[Rh2BaseNS3].rtc_handles['Tmvel2D2Tmvel0.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)
  env.con_list.append(con)
  ## [SwitchComp] -> [Tmvel2D2Tmvel]
  con=IOConnector([switch.outports['outVel'],tmvel2d2tmvel.inports['in']])
  env.con_list.append(con)
  ## [Tmvel2D2Tmvel] -> [MotorControl]
  con=IOConnector([tmvel2d2tmvel.outports['out'],motor.inports['TargetVelocity']])
  env.con_list.append(con)
  ## [MotorControl] -> [Odometry]
  con=IOConnector([motor.outports['OutCurrentWheelAngle'],odometry.inports['CurrentWheelAngle']])
  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)
  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)


def set_config_param() :
  navigation.conf_ref.activate_configuration_set('rtc-center')

#
# 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() :
  switch.activate()
  tmvel2d2tmvel.activate()
  odometry.activate()
  localization.activate()
def deactivate_local() :
  switch.deactivate()
  tmvel2d2tmvel.activate()
  odometry.deactivate()
  localization.deactivate()
def activate_all() :
  tmvel2d2tmvel.activate()
  odometry.activate()
  localization.activate()
  joystick.activate()
  switch.activate()
  pathplanning.activate()
  navigation.activate()
  pathfollower.activate()
  dispposition.activate()
def deactivate_all() :
  tmvel2d2tmvel.activate()
  odometry.deactivate()
  localization.deactivate()
  joystick.deactivate()
  switch.deactivate()
  pathplanning.deactivate()
  navigation.deactivate()
  pathfollower.deactivate()
  dispposition.deactivate()

#
# create env
#
env = RtmEnv(sys.argv,[Rh2BaseNS3])
run_components()
time.sleep(2)
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)
raw_input("please start simulation.")
activate_all()
