import qi
import argparse
import sys
import time
import threading

import action_base
from action_base import *
import conditions
from conditions import get_condition


actionName = "followuntil"


def actionThread_exec (params):
    t = threading.currentThread()
    memory_service = getattr(t, "mem_serv", None)
    session = getattr(t, "session", None)

    print "Action "+actionName+" started with params "+params

    # action init
    tracker_service = session.service("ALTracker")

    #We need to obtain the id of the person to follow
    params = params.split('_')

    if len(params) == 2:
        try:
            personid = memory_service.getData(params[1])
            params = params[0]
        except:
            pass
    else:
        try:
            personid = memory_service.getData("EngagementZones/PersonEnteredZone1")
        except:
            pass
    print "Person ID = ",personid

    tracker_service.setMode("Navigate")
    # The robot stays a 50 centimeters of target with 10 cm precision
    tracker_service.setRelativePosition([-0.5, 0.0, 0.0, 0.1, 0.1, 0.3])

    tracker_service.registerTarget("People",personid)
    tracker_service.track("People")
    val = False
    # action init

    while (getattr(t, "do_run", True) and (not val)): 
        #print "Action "+actionName+" "+params+" exec..."
        # action exec
        val = get_condition(memory_service, params)
        # action exec
        time.sleep(0.25)
		
    print "Action "+actionName+" "+params+" terminated"
    # action end
    tracker_service.stopTracker()
    tracker_service.unregisterAllTargets()
    # action end
    memory_service.raiseEvent("PNP_action_result_"+actionName,"success");


def init(session):
    print actionName+" init"
    action_base.init(session, actionName, actionThread_exec)


def quit():
    print actionName+" quit"
    actionThread_exec.do_run = False


if __name__ == "__main__":

    app = action_base.initApp(actionName)
    	
    init(app.session)

    #Program stays at this point until we stop it
    app.run()

    quit()


@@ -537,16 +537,31 @@
  follow%0A
+    #try:%0A    #
     para
@@ -582,22 +582,61 @@
 lit('_')
+%0A    #except:%0A    #    params = params
 %0A%0A    
+#
 if len(p
@@ -648,24 +648,25 @@
 ) == 2:%0A    
+#
     try:%0A   
@@ -658,32 +658,33 @@
   #    try:%0A    
+#
         personid
@@ -720,24 +720,25 @@
 ams%5B1%5D)%0A    
+#
         para
@@ -752,24 +752,25 @@
 rams%5B0%5D%0A    
+#
     except:%0A
@@ -765,32 +765,33 @@
     except:%0A    
+#
         pass%0A   
@@ -791,16 +791,17 @@
 ass%0A    
+#
 else:%0A  
@@ -798,24 +798,25 @@
  #else:%0A    
+#
     try:%0A   
@@ -816,24 +816,25 @@
 ry:%0A    
+#
         
 personid
@@ -817,32 +817,112 @@
 y:%0A    #        
+%0A    #    except:%0A    #        pass%0A    #print %22Person ID = %22,personid%0A    %0A    
 personid = memor
@@ -981,76 +981,8 @@
 1%22)%0A
-        except:%0A            pass%0A    print %22Person ID = %22,personid%0A%0A
     

change in follow