import time
import threading
import DobotDllType as dType

"""
Six actuators:
    linear rail
    base rotation 
    rear arm
    forarm
    gripperrotation
    gripper

First five takes argument 1 or -1 (direction)
Example:
#initialisation 
c = Candybot()
c.linear(1) # move linear in positive direction
c.gripp() # gripp candy
c.letgo() # let go of candy
c.stopcmp() # stoop compressor

c.panic() # stops everything
"""

class Candybot:
    def __init__(self):
        self.jointspeed = 100
        self.lastjointspeed = 100

        self.linearspeed = 100
        self.lastleinearspeed = 100
        
        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, 2, 200, 2, 200, 2, 200, 2, 200, isQueued = 1)
            dType.SetJOGCommonParams(self.api, 100, 100, isQueued = 1)

            #linear rail
            dType.SetDeviceWithL(self.api, 1)
            dType.SetJOGLParams(self.api,  200, 200, 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()
        #dType.SetWAITCmd(self.api, 1, isQueued=0)
        #dType.SetJOGCmd(self.api, 1, 0)

    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(self.api, 1, isQueued=0)
        #dType.SetJOGCmd(self.api, 1, 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(self.api, 1, isQueued=0)
        #dType.SetJOGCmd(self.api, 1, 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(self.api, 1, isQueued=0)
        #dType.SetJOGCmd(self.api, 1, 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(self.api, 1, isQueued=0)
        #dType.SetJOGCmd(self.api, 1, 0)
        self.lint = threading.Timer(0.2,self.stop)
        self.lint.start()

    def gripp(self):
        dType.SetEndEffectorGripper(self.api, 1,  1, isQueued=0)
    
    def letgo(self):
        dType.SetEndEffectorGripper(self.api, 1,  0, isQueued=0)

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

    def stopcmp(self):
        dType.SetEndEffectorGripper(self.api, 0,  0, isQueued=0)

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

    def getCurrentCmd(self):
        return dType.GetQueuedCmdCurrentIndex(self.api)
    
    def JOGtest(self):
        dType.SetJOGCmd(self.api,1,1)
        dType.SetJOGCmd(self.api,1,9)
    
    def JOGtest2(self):
        dType.SetJOGLParams(self.api,  100, 200, isQueued=0)
        dType.SetJOGCmd(self.api,1,9)
        time.sleep(1)
        for speed in range(100,10,-10):
            time.sleep(0.3)
            dType.SetJOGLParams(self.api,  speed, 200, isQueued=0)
         

    def setparams(self, name, speed):
        lookup = {'base' : 0, 'reararm' : 1, 'forearm' : 2, 'gripperroation' :3}
        if name == 'rail':
            dType.SetJOGLParams(self.api,  speed, 200, isQueued=0)
    
        else:
            speeds = [0,0,0,0]
            speeds[lookup[name]] = speed
            dType.SetJOGJointParams(self.api, speeds[0], 200, speeds[1], 200, speeds[2], 200, speeds[3], 200, isQueued = 1)

    def setJOGcmd(self, cmd):
        dType.SetJOGCmd(self.api,1,cmd)

candy = Candybot()


startTime = time.time()
nowTime = 0

currentMotor = 0
motorcfg = [0] * 10
motorcfg[1] = {"name" : 'base' , "positive":1, "negative":2, 'speedslot':2}
motorcfg[2] = {"name" : 'reararm' ,"positive":3, "negative":4, 'speedslot':4}
motorcfg[3] = {"name" : 'forearm' ,"positive":5, "negative":6, 'speedslot':6}
motorcfg[4] = {"name" : 'rail' ,"positive":9, "negative":10, 'speedslot':1}

friction = 0.95
movement_time = 3
max_movement_time = 5
min_speed = 1
max_speed = 50

speed = 0
stopTime = 0
isRunning = False


def adjustSpeed(motor, adjustment):
    global nowTime, stopTime, currentMotor, speed, motorcfg, movement_time, max_speed, max_movement_time

    if currentMotor==motor:
        if (adjustment>0 and speed>0) or (adjustment<0 and speed<0):=
            speed += adjustment * 1-(abs(speed)/max_speed)
        else:
            stopMotorFunctions()
        stopTime += movement_time/2 * max_movement_time/(stopTime - nowTime)
    else:
        currentMotor = motor
        speed = adjustment
        stopTime = nowTime + movement_time
        if(speed>0):
            candy.setJOGcmd(motorcfg[currentMotor]['positive'])
            #print("JOG command {}".format(motorcfg[currentMotor]['positive']))
        else:
            candy.setJOGcmd(motorcfg[currentMotor]['negative'])
            #   print("JOG command {}".format(motorcfg[currentMotor]['negative']))
            

def stopMotorFunctions():
    global currentMotor, speed
    currentMotor = 0
    speed = 0
    candy.stop()

def updateLoop():
    global nowTime, stopTime, currentMotor, speed, friction, min_speed, motorcfg
    nowTime = (time.time() - startTime)
    threading.Timer(1, updateLoop).start()
    
    if currentMotor != 0:
        if nowTime > stopTime:
            stopMotorFunctions()
            return
        else:
            speed *= friction
            if abs(speed) < min_speed:
                stopMotorFunctions()
                return
            candy.setparams(motorcfg[currentMotor]['name'], abs(speed))
            #print("JOG parameter {} to {}".format( motorcfg[currentMotor]['speedslot'], abs(speed)) )


def rt():
    adjustSpeed(1, 5)

def lt():
    adjustSpeed(1, -5)

def up():
    adjustSpeed(2, 5)

def dn():
    adjustSpeed(2, -5)

def lr():
    adjustSpeed(4, 10)

def ll():
    adjustSpeed(4, -10)

updateLoop()

#
#c = Candybot()
#
#
#def slide_left():
#    c.linear(-1)
#
#def slide_right():
#    c.linear(1)
#    
#def rotate_left():
#    c.base(-1)
#
#def rotate_right():
#    c.base(1)
#
#def forward():
#    c.forearm(1)
#
#def back():
#    c.forearm(-1)
#
#def up():
#    c.rearArm(1)
#
#def down():
#    c.rearArm(-1)
#
def stopCmp():
    candy.stopcmp()
    
def grab():
    candy.gripp()
#    c.stopcmp()
#
def drop():
    candy.letgo()
#    c.stopcmp()
#

def stop():
    candy.panic()
#
#def getCurrentCmd():
#    return c.getCurrentCmd()
#
#def updateLinearSpeed():    
#    pass
#