import time
import threading
import DobotDllType as dType

class Candybot:
    def __init__(self):
        CON_STR = {
            dType.DobotConnect.DobotConnect_NoError:  "DobotConnect_NoError",
            dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound",
            dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied"}

        #Load Dll
        self.api = dType.load()

        #Connect Dobot
        state = dType.ConnectDobot(self.api, "", 115200)[0]
        print("Connect status:",CON_STR[state])

        if (state == dType.DobotConnect.DobotConnect_NoError):

            #Clean Command Queued
            dType.SetQueuedCmdClear(self.api)

            #Async Motion Params Setting
            dType.SetHOMEParams(self.api, 250, 0, 50, 0, isQueued = 1)
            dType.SetJOGJointParams(self.api, 50, 50, 50, 50, 50, 50, 50, 50, isQueued = 1)
            dType.SetJOGCommonParams(self.api, 100, 100, isQueued = 1)

            #linear rail
            dType.SetDeviceWithL(self.api, 1)
            dType.SetJOGLParams(self.api,  50, 50, isQueued=0)

            #Async Home
            dType.SetHOMECmd(self.api, temp = 0, isQueued = 1)

    def disconnect(self):
        #Disconnect Dobot
        dType.DisconnectDobot(self.api)

    def base(self, direction):
        if direction == 1:
            dType.SetJOGCmd(self.api,1,1)
    
        elif direction == -1:
            dType.SetJOGCmd(self.api,1,2)

        else :
            return
        threading.Timer(0.1,self.stop).start()

    def rearArm(self, direction):
        if direction == 1:
            dType.SetJOGCmd(self.api,1,3)
    
        elif direction == -1:
            dType.SetJOGCmd(self.api,1,4)

        else :
            return
        dType.SetWAITCmd(api, 1, isQueued=0)
        #threading.Timer(0.1,self.stop).start()

    def forearm(self, direction):
        if direction == 1:
            dType.SetJOGCmd(self.api,1,5)
    
        elif direction == -1:
            dType.SetJOGCmd(self.api,1,6)

        else :
            return
        dType.SetWAITCmd(api, 1, isQueued=0)
        #threading.Timer(0.1,self.stop).start()

    def gripperRotation(self, direction):
        if direction == 1:
            dType.SetJOGCmd(self.api,1,7)
    
        elif direction == -1:
            dType.SetJOGCmd(self.api,1,8)

        else :
            return
        dType.SetWAITCmd(api, 1, isQueued=0)
        #threading.Timer(0.1,self.stop).start()

    def linear(self, direction):
        if direction == 1:
            dType.SetJOGCmd(self.api,1,9)
    
        elif direction == -1:
            dType.SetJOGCmd(self.api,1,10)

        else :
            return
        dType.SetWAITCmd(api, 1, isQueued=0)
        #threading.Timer(0.1,self.stop).start()


    def stop(self):
        dType.SetJOGCmd(self.api, 1, 0)

    def panic(self):
        dType.SetQueuedCmdForceStopExec(self.api)