import threading
import time





startTime = time.time()
nowTime = 0;

currentMotor = 0
motorcfg = [0] * 10
motorcfg[1] = {"positive":1, "negative":2, 'speedslot':2}
motorcfg[2] = {"positive":3, "negative":4, 'speedslot':4}
motorcfg[3] = {"positive":5, "negative":6, 'speedslot':6}

friction = 0.99
movement_time = 5
max_movement_time = 10
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:
		speed += adjustment * (abs(speed)/max_speed)
		stopTime += movement_time/2 * max_movement_time/(stopTime - nowTime)
	else:
		currentMotor = motor
		speed = adjustment
		stopTime = nowTime + movement_time
		if(speed>0):
			print("JOG command {}".format(motorcfg[currentMotor]['positive']))
		else:
			print("JOG command {}".format(motorcfg[currentMotor]['negative']))
			

def stopMotorFunctions():
	global currentMotor, speed
	currentMotor = 0
	speed = 0
	print ("JOB command stop all motors")

def updateLoop():
	global nowTime, stopTime, currentMotor, speed, friction, min_speed, motorcfg
	nowTime = (time.time() - startTime)
	threading.Timer(0.2, updateLoop).start()
	
	if currentMotor != 0:
		if nowTime > stopTime:
			stopMotorFunctions()
			return
		else:
			speed *= friction
			if abs(speed) < min_speed:
				stopMotorFunctions()
				return
			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)

updateLoop();