#!/usr/bin/env python3
#coding=utf-8
#Date: 2024-12-01
#Author: Zongdao Li

import sys

# def load_api(path : str):
#     from ctypes import CDLL
#     # scriptLibraryPath='/home/imi/ros2_ws/src/diana_py/lib/libDianaApi.so'
#     if sys.version_info < (3, 8):
#         return  CDLL(path)
#     else:
#         return  CDLL(path, winmode=0)

   
# api_mod = load_api(LIB_PATH)    

import DianaApi_v2 as DA
import time

class Diana():
    def __init__(self,ip):
        self.robot_ip = ip
        self.netInfo=(self.robot_ip, 0, 0, 0, 0, 0)
        self.init_srv()
        print("")

    def init_srv(self):
        DA.initSrv(self.netInfo)

    def start(self):
        DA.initSrv(self.netInfo)
        DA.releaseBrake(self.robot_ip)

    def move_spec_J(self,joint_value,joint_index):
        v = 0.1
        a = 0.1
        DA._MoveSpecificJoint(joint_value,joint_index,v,a,self.robot_ip)

    def moveJ(self,joints,v,a):
        ret = DA._MoveJ(joints,v,a,self.robot_ip)

    def moveL(self,pose,v,a):
        ret = DA.moveJToPose(pose,v,a,self.robot_ip)

    def servoJ(self,joints):
        ret = DA._ServoJ(joints,ipAddress=self.robot_ip)
        if ret:
            print("servoJ complete")
        else:
            raise Exception(" get servoJ failed")
        
    def servoL(self,pose):
        ret = DA.servoL(pose, t=0.01, ah_t=0.03, gain=300, scale=1,ipAddress=self.robot_ip)
        if ret:
            print("servoL complete")
        else:
            raise Exception(" get servoL failed")

    def pathPos(self,pos_group,v=0.2,a=0.2):
        ret,path_id = DA._CreatePathbyJoints(self.robot_ip)
        times = len(pos_group)
        if times < 1:
            raise Exception(" pos group is zero")
        else:
            for i in range(times):
                DA.addMoveL(path_id, pos_group[i], v, a, 0.3,self.robot_ip)
            ret1 = DA._ExecutePath(path_id,self.robot_ip)
            if ret1:
                DA.destroyPath(path_id,self.robot_ip)
    

    def getRobotState(self):
        return DA.getRobotState(self.robot_ip)
             
    def pathJnts(self,joints_group,v=0.2,a=0.2):
        ret,path_id = DA._CreatePathbyJoints(self.robot_ip)
        times = len(joints_group)
        if times < 1:
            raise Exception(" joints group is zero")
        else:
            for i in range(times):
                DA.addMoveJ(path_id, joints_group[i], v, a, 0.3,self.robot_ip)
            DA._ExecutePath(path_id,self.robot_ip)
            DA.destroyPath(path_id,self.robot_ip)
        
    def getJointStates(self):
        jointsPosition = [0,0,0,0,0,0,0]
        ret = DA.getJointPos(jointsPosition, self.robot_ip)
        if ret == True:
            return jointsPosition
        else:
            raise Exception(" get jointsPosition failed")
        
    def getTCPpos(self):
        TCP_pose = DA._GetTcpPos(self.robot_ip)
        return TCP_pose

    def stop(self):
        # DA.stop(self.robot_ip)
        DA.holdBrake(self.robot_ip)
        DA.destroySrv(self.robot_ip)
        print("Good Bye !")

if __name__ == '__main__':
        a = Diana('192.168.10.76')
        a.start()
        i = 0
        while True:
            i+=1
            print("run")
            if i == 5000:
                a.stop()
                break
        print("close")
            
