#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import time
import signal
from RobotiqHand import RobotiqHand

#------------------------------------------------------------------------------
# test_robotiq.py
#------------------------------------------------------------------------------

HOST = "192.168.56.2"
PORT = 54321

cont = True


def handler(signal, frame):
    global cont
    cont = False


def test_robotiq():

    # # while True:
    # print('test_robotiq start')
    hand = RobotiqHand()
    hand.connect(HOST, PORT)
    #
    # # exit(0)
    try:
        print ('activate: start')
        hand.reset()
        hand.activate()
        result = hand.wait_activate_complete()
        print ('activate: result = 0x{:02x}'.format(result))
        if result != 0x31:
            hand.disconnect()
            return
        print ('adjust: start')
        hand.adjust()
        print ('adjust: finish')

        time.sleep(2)
        print('close slow')
        hand.move(255, 0, 255)
        time.sleep(1)
        # print(hand.status())
        print(hand.get_instant_gripper_status())
        # # (status, position, force) = hand.wait_move_complete()
        #
        # time.sleep(1)
        # print('open fast')
        # hand.move(0, 255, 255)
    #     # (status, position, force) = hand.wait_move_complete()
    except:
        print('Ctrl-c pressed')
        #TODO create handler to close the door

    hand.disconnect()


if __name__ == '__main__':
    signal.signal(signal.SIGINT, handler)
    test_robotiq()