################################################################################
#
#  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 ctypes
import numpy as np

def Init():
    global libdrd

    # Uncomment the following on Windows:
    # libdrd = ctypes.cdll.drd64

    # Uncomment the following on Linux:
    # (you should also create a local softlink to the libdrd.so.x.y library)
    # libdrd = ctypes.CDLL('./libdrd.so.3')

def Open():
    return libdrd.drdOpen()

def EnableForce(on, id):
    return libdrd.dhdEnableForce(on, id)

def EmulateButton(on, id):
    return libdrd.dhdEmulateButton(on, id)

def IsInitialized(id):
    return libdrd.drdIsInitialized(id)

def AutoInit(id):
    return libdrd.drdAutoInit(id)

def Start(id):
    return libdrd.drdStart(id)

def Stop(keepForcesOn, id):
    return libdrd.drdStop(keepForcesOn, id)

def GetSystemType(id):
    return libdrd.dhdGetSystemType(id)

def GetSystemName(id):
    systemName = ctypes.c_char_p(libdrd.dhdGetSystemName(id))
    return systemName.value.decode("utf-8")

def GetSerialNumber(id):
    sn = ctypes.c_ushort(0)
    libdrd.dhdGetSerialNumber(ctypes.pointer(sn), id)
    return sn.value

def GetPosition(id):
    x = ctypes.c_double(0.0)
    y = ctypes.c_double(0.0)
    z = ctypes.c_double(0.0)
    ret = libdrd.dhdGetPosition(ctypes.pointer(x), ctypes.pointer(y), ctypes.pointer(z), id)
    return ret, x.value, y.value, z.value

def GetOrientationFrame(id):
    rotation = np.zeros((3, 3))
    rotation = rotation.astype(np.double)
    ret = libdrd.dhdGetOrientationFrame(rotation.ctypes.data_as(ctypes.POINTER(ctypes.c_double)), id)
    return ret, rotation

def GetGripperGap(id):
    g = ctypes.c_double(0.0)
    ret = libdrd.dhdGetGripperGap(ctypes.pointer(g), id)
    return ret, g.value

def SetForce(fx, fy, fz, id):
    return libdrd.dhdSetForce(ctypes.c_double(fx), ctypes.c_double(fy), ctypes.c_double(fz), id)

def SetForceAndTorqueAndGripperForce(fx, fy, fz, tx, ty, tz, fg, id):
    return libdrd.dhdSetForceAndTorqueAndGripperForce(ctypes.c_double(fx),
                                                      ctypes.c_double(fy),
                                                      ctypes.c_double(fz),
                                                      ctypes.c_double(tx),
                                                      ctypes.c_double(ty),
                                                      ctypes.c_double(tz),
                                                      ctypes.c_double(fg),
                                                      id)

def SetForceAndWristJointTorquesAndGripperForce(fx, fy, fz, ta, tb, tg, fg, id):
    return libdrd.dhdSetForceAndWristJointTorquesAndGripperForce(ctypes.c_double(fx),
                                                                 ctypes.c_double(fy),
                                                                 ctypes.c_double(fz),
                                                                 ctypes.c_double(ta),
                                                                 ctypes.c_double(tb),
                                                                 ctypes.c_double(tg),
                                                                 ctypes.c_double(fg),
                                                                 id)

def GetButton(index, id):
    return libdrd.dhdGetButton(index, id)

def Close(id):
    return libdrd.dhdClose(id)
