################################################################################
#
#  Copyright (C) 2001-2023 Force Dimension, Switzerland.
#  All Rights Reserved.
#
#  $PROJECT $VERSION
#  $DESCRIPTION
#
#  THIS FILE CAN NOT BE COPIED AND/OR DISTRIBUTED WITHOUT EXPRESS
#  PERMISSION FROM FORCE DIMENSION.
#
################################################################################

# Import the Force Dimension SDK module.
import fdsdk

# Initialize the FDSDK Python wrapper.
fdsdk.Init()
print('Force Dimension - Python Wrapper Example');
print('Copyright (C) 2001-2023 Force Dimension');
print('All Rights Reserved.\n');

# Open aconnection to the first haptic device on the system.
id = fdsdk.Open()
if id < 0:
    print('no device found')
    exit(-1)
print('opened device id ' + str(id))

# Report haptic device information.
systemType = fdsdk.GetSystemType(id)
serialNumber = fdsdk.GetSerialNumber(id)
print('device type ' + str(systemType) + ' (S/N ' + str(serialNumber) + ')')

# Initialize the haptic device if necessary.
if not fdsdk.IsInitialized(id):
    print('initializing...')
    if fdsdk.AutoInit(id) < 0:
        print('error: failed to initialize')
        exit(-1)
    print("\033[Fdevice initialized")
    if fdsdk.Stop(True, id) < 0:
        print('error: failed to stop robotic regulation')
        exit(-1)

# Otherwise, enable forces on the haptic device.
else:
    if fdsdk.EnableForce(1, id) < 0:
        print('error: failed to enable force')
        exit(-1)

# Run a simple gravity compensation haptic loop.
print('\npress user button to quit haptic loop\n')
while fdsdk.GetButton(0, id) == 0:

    # Get the haptic device position.
    ret, x, y, z = fdsdk.GetPosition(id)
    print('position: ' + f'{x:+5.3f}' + ' ' + f'{y:+5.3f}' + ' ' + f'{z:+5.3f}')
    if (ret < 0):
        print('error: failed to get position')
        exit(-1)

    # Get the haptic device rotation.
    ret, r = fdsdk.GetOrientationFrame(id)
    print('rotation: ' + f'{r[0][0]:+5.3f}' + ' ' + f'{r[0][1]:+5.3f}' + ' ' + f'{r[0][2]:+5.3f}' )
    print('          ' + f'{r[1][0]:+5.3f}' + ' ' + f'{r[1][1]:+5.3f}' + ' ' + f'{r[1][2]:+5.3f}' )
    print('          ' + f'{r[2][0]:+5.3f}' + ' ' + f'{r[2][1]:+5.3f}' + ' ' + f'{r[2][2]:+5.3f}' )
    if (ret < 0):
        print('error: failed to get rotation')
        exit(-1)

    # Get the gripper opening.
    ret, g = fdsdk.GetGripperGap(id)
    print('gripper : ' + f'{g:+5.3f}')
    if (ret < 0):
        print('error: failed to get gripper opening')
        exit(-1)

    # Set a null force to compensate gravity.
    if fdsdk.SetForceAndTorqueAndGripperForce(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, id) < 0:
        print('error: failed to apply force')
        exit(-1)

    # Move the display cursor back (assuming a terminal that understands ANSI escape codes).
    print("\033[F\033[F\033[F\033[F\033[F\033[F")

# Close the connection to the haptic device.
fdsdk.Close(id)
print('\n\n\n\n\ndone.')
