#! /usr/bin/env python
"""
This script tests the KUKA LWR arm, both the data and service api
"""

import sys
from time import sleep
from morse.testing.testing import MorseTestCase
from pymorse import Morse

# Include this import to be able to use your test file as a regular
# builder script, ie, usable with: 'morse [run|exec] base_testing.py
try:
    from morse.builder import *
except ImportError:
    pass

def send_pose(s, x, y, yaw):
    s.publish({'x' : x, 'y' : y, 'z' : 0.0, \
               'yaw' : yaw, 'pitch' : 0.0, 'roll' : 0.0})
    sleep(0.1)

class gripperTest(MorseTestCase):
    def setUpEnv(self):
        """ Defines the test scenario, using the Builder API.
        """

        robot = ATRV('robot')

        arm = KukaLWR()
        robot.append(arm)
        arm.translate(x=0.5, z=0.9)
        arm.add_stream('socket')
        arm.add_service('socket')

        gripper = Gripper('gripper')
        gripper.translate(z=1.28)
        arm.append(gripper)
        gripper.properties(Angle = 180.0, Distance=2.0)
        gripper.add_service('socket')

        teleport = Teleport()
        robot.append(teleport)
        teleport.add_stream('socket')

        tape1 = PassiveObject(prefix='BlackVideotape')
        tape1.properties(Object = True, Graspable = True, Label = "BlackTape")
        tape1.translate(x=5, y=5, z=0)

        tape2 = PassiveObject(prefix='WhiteVideotape')
        tape2.properties(Object = True, Graspable = True, Label = "WhiteTapee")
        tape2.translate(x=5, y=-5, z=0)

        env = Environment('empty', fastmode=True)
        env.add_service('socket')


    def test_gripper(self):
        """ This test is guaranteed to be started only when the simulator
        is ready.
        """
        with Morse() as morse:
            # kuka controller socket
            kuka_client = morse.robot.arm
            teleport_stream = morse.robot.teleport

            kuka_client.set_rotation('kuka_2', -1.57)
            sleep(0.1)

            obj = morse.robot.arm.gripper.grab().result()
            self.assertEqual(obj, None)

            obj = morse.robot.arm.gripper.release().result()
            self.assertEqual(obj, None)

            send_pose(teleport_stream, 3.0, 5, 0.0)
            sleep(0.1)

            obj = morse.robot.arm.gripper.grab().result()
            self.assertEqual(obj, 'tape1')

            send_pose(teleport_stream, 3.0, -5.0, 0.0)
            obj = morse.robot.arm.gripper.grab().result()
            self.assertEqual(obj, 'tape1')

            send_pose(teleport_stream, 3.0, 8.0, 0.0)
            obj = morse.robot.arm.gripper.release().result()
            self.assertEqual(obj, True)

            send_pose(teleport_stream, 3.0, -5.0, 0.0)
            obj = morse.robot.arm.gripper.grab().result()
            self.assertEqual(obj, 'tape2')

            obj = morse.robot.arm.gripper.release().result()
            self.assertEqual(obj, True)

            send_pose(teleport_stream, 3.0, 8.0, 0.0)
            obj = morse.robot.arm.gripper.grab().result()
            self.assertEqual(obj, 'tape1')


########################## Run these tests ##########################
if __name__ == "__main__":
    import unittest
    from morse.testing.testing import MorseTestRunner
    suite = unittest.TestLoader().loadTestsFromTestCase(gripperTest)
    sys.exit(not MorseTestRunner().run(suite).wasSuccessful())

