import serial #Allows you to talk to the Arduino board
import time #Allows you to tell the code to wait
import Tkinter
import tkMessageBox
from Tkinter import *

#Connect to COM3, your USB port may be different so check which COM port your
#Arduino board is connected to
usbport = 'COM9'

#Don't change this line of code
ser = serial.Serial(usbport, 9600, timeout=1)
mainWindow = Tkinter.Tk()
currPositions = [90,90,90,35]

#Middle
middleLabel = Tkinter.Label(None, text='Middle', font=('Times', '18'),fg='blue')
middleValue = Tkinter.Label(None, text=currPositions[0], font=('Times', '18'),fg='blue')
middleButtonPlus = Tkinter.Button(mainWindow, text ="+", command = lambda: moveIncrement(0, 1))
middleButtonMinus = Tkinter.Button(mainWindow, text ="-", command = lambda: moveIncrement(0, 0))

middleLabel.grid(row=0, column=0)
middleButtonPlus.grid(row=0, column=1)
middleButtonMinus.grid(row=0, column=2)
middleValue.grid(row=0, column=3)


#Left
leftLabel = Tkinter.Label(None, text='Left', font=('Times', '18'),fg='blue')
leftValue = Tkinter.Label(None, text=currPositions[1], font=('Times', '18'),fg='blue')
leftButtonPlus = Tkinter.Button(mainWindow, text ="+", command = lambda: moveIncrement(1, 1))
leftButtonMinus = Tkinter.Button(mainWindow, text ="-", command = lambda: moveIncrement(1, 0))

leftLabel.grid(row=1, column=0)
leftButtonPlus.grid(row=1, column=1)
leftButtonMinus.grid(row=1, column=2)
leftValue.grid(row=1, column=3)


#right
rightLabel = Tkinter.Label(None, text='Right', font=('Times', '18'),fg='blue')
rightValue = Tkinter.Label(None, text=currPositions[2], font=('Times', '18'),fg='blue')
rightButtonPlus = Tkinter.Button(mainWindow, text ="+", command = lambda: moveIncrement(2, 1))
rightButtonMinus = Tkinter.Button(mainWindow, text ="-", command = lambda: moveIncrement(2, 0))

rightLabel.grid(row=2, column=0)
rightButtonPlus.grid(row=2, column=1)
rightButtonMinus.grid(row=2, column=2)
rightValue.grid(row=2, column=3)

#Claw
clawLabel = Tkinter.Label(None, text='Claw', font=('Times', '18'),fg='blue')
clawValue = Tkinter.Label(None, text=currPositions[3], font=('Times', '18'),fg='blue')
clawButtonPlus = Tkinter.Button(mainWindow, text ="+", command = lambda: moveIncrement(3, 1))
clawButtonMinus = Tkinter.Button(mainWindow, text ="-", command = lambda: moveIncrement(3, 0))

clawLabel.grid(row=3, column=0)
clawButtonPlus.grid(row=3, column=1)
clawButtonMinus.grid(row=3, column=2)
clawValue.grid(row=3, column=3)

#Button and text box to enter angle for Middle servo
E1 = Entry(mainWindow, bd =5, width = 7)
E1.insert(END, "90")
B1 = Tkinter.Button(mainWindow, text ="Middle", command = lambda: moveButton(E1, 0))

#Button and text box to enter angle for Left servo
E2 = Entry(mainWindow, bd =5, width = 7)
E2.insert(END, "90")
B2 = Tkinter.Button(mainWindow, text ="Left", command = lambda: moveButton(E2, 1))

#Button and text box to enter angle for Right servo
E3 = Entry(mainWindow, bd =5, width = 7)
E3.insert(END, "90")
B3 = Tkinter.Button(mainWindow, text ="Right", command = lambda: moveButton(E3, 2))


#Button and text box to enter angle for Claw servo
E4 = Entry(mainWindow, bd =5, width = 7)
E4.insert(END, "35")
B4 = Tkinter.Button(mainWindow, text ="Claw", command = lambda: moveButton(E4, 3))

#Entry locations
E1.grid(row=4, column=1)
E2.grid(row=5, column=1)
E3.grid(row=6, column=1)
E4.grid(row=7, column=1)

#Button locations
B1.grid(row=4, column=0)
B2.grid(row=5, column=0)
B3.grid(row=6, column=0)
B4.grid(row=7, column=0)

#Function to move the arm, you should not need to change this code
def move(servo, angle):
    if (0 <= angle <= 180):
        ser.write(chr(servo))
        ser.write(chr(angle))
        currPositions[servo] = angle
        refreshLabels()
    else:
        print(angle, ": Servo angle must be an integer between 0 and 180.\n")

def moveButton(E, servo):
    move(servo, int(E.get()))

def moveIncrement(servo, direction):
    angle = 0
    if direction == 1:
        angle = currPositions[servo]+1
    else:
        angle = currPositions[servo]-1
    move(servo, angle)

#This function will move 1 servo slowly.  You need to specify which motor to move (servo),
#the position the motor is currently at (currAngle), the angle you wish it to move to (angle),
#the number of degrees it will move at a time (increment) and how long it will wait after
#each movement
def moveSlow(servo, angle, increment, delay):
    if currPositions[servo] > angle:
        increment = increment*-1
    for i in range (currPositions[servo], angle, increment):
        move(servo, i)
        time.sleep(delay)
    move(servo, angle)

#This function will move multiple servos slowly.  You need to specify which motors to move (servo),
#the positions the motors are currently at (currAngle), the angles you wish it to move the motors to (angle),
#the number of degrees it will move at a time (increment) and how long it will wait after
#each movement
def moveMultipleSlow(servo, angle, increment, delay):
    inPosition = 0
    while(inPosition < len(servo)):
        inPosition = 0
        inc = 0
        for i in range(0, len(servo)):
            if currPositions[servo[i]] > angle[i]:
                inc = increment*-1
            else:
                inc = increment
            if currPositions[servo[i]] != angle[i]:
                currPositions[servo[i]] = currPositions[servo[i]]+inc
                move(servo[i], currPositions[servo[i]])
                time.sleep(delay)
            if(abs(currPositions[servo[i]] - angle[i]) < abs(increment)):
                move(servo[i], angle[i])
                time.sleep(delay)
                inPosition = inPosition + 1

def refreshLabels():
    middleValue.config(text=currPositions[0])
    leftValue.config(text=currPositions[1])
    rightValue.config(text=currPositions[2])
    clawValue.config(text=currPositions[3])



time.sleep(2)

mainWindow.mainloop()

ser.close()