from robolink import *    # API to communicate with RoboDK
from robodk import *      # library for basic matrix operations
import datetime
from PIL import Image
import time

# Use RoboDK API
RDK = Robolink()

# Set Variables
robot = RDK.Item('Mecademic Meca500x')
red = (227, 0, 0)  # RGB value for red
green = (0, 227, 0)  # RGB value for green
blue = (0, 0, 227)  # RGB value for blue
delay = 3 # time delay seconds

# Set Program Variables

StartUp = RDK.Item('StartUp')
PP1 = RDK.Item('PP1')
PP2 = RDK.Item('PP2')
PP3 = RDK.Item('PP3')
PP4 = RDK.Item('PP4')
PP5 = RDK.Item('PP5')
PP6 = RDK.Item('PP6')
PP7 = RDK.Item('PP7')
PP8 = RDK.Item('PP8')
PA12 = RDK.Item('PA12')
PA23 = RDK.Item('PA23')
PA34 = RDK.Item('PA34')
PA45 = RDK.Item('PA45')
PA56 = RDK.Item('PA56')
PA67 = RDK.Item('PA67')
PA78 = RDK.Item('PA78')
PA8HOME = RDK.Item('PA8HOME')

#Define Subprograms

## User Selects the color to keep
def get_choice():
    options = ["RED", "BLUE", "GREEN"]
    print("Hi, Baxter will remove any unwanted color block")
    print("Options are RED, GREEN and BLUE")
    while True:
        choice1 = input("Which color would you like to keep?\n")
        selection = choice1.upper()  # Make input uppercase
        if selection in options:
            return selection
        else:
            print(selection,"is not an option. Please try again")
choice = get_choice()
print("You have chosen",choice)


# Run Decision Program with trajectories


StartUp.RunCode() # Starts Up the Simulation

file_name = RDK.getParam('PATH_OPENSTATION') + "/Image1.png"
RDK.Cam2D_Snapshot(file_name)

time.sleep(delay)

img = Image.open(file_name) # Open picture of cube
color = img.getpixel((320,240))
print (color)

if choice == "RED" and color == red:
    PA12.RunCode()
elif choice == "BLUE" and color == blue:
    PA12.RunCode()
elif choice == "GREEN" and color == green:
    PA12.RunCode()
else:
    PP1.RunCode()

file_name = RDK.getParam('PATH_OPENSTATION') + "/Image2.png"
RDK.Cam2D_Snapshot(file_name)

time.sleep(delay)

img = Image.open(file_name) # Open picture of cube
color = img.getpixel((320,240))
print (color)

if choice == "RED" and color == red:
    PA23.RunCode()
elif choice == "BLUE" and color == blue:
    PA23.RunCode()
elif choice == "GREEN" and color == green:
    PA23.RunCode()
else:
    PP2.RunCode()

file_name = RDK.getParam('PATH_OPENSTATION') + "/Image3.png"
RDK.Cam2D_Snapshot(file_name)

time.sleep(delay)

img = Image.open(file_name) # Open picture of cube
color = img.getpixel((320,240))
print (color)

if choice == "RED" and color == red:
    PA34.RunCode()
elif choice == "BLUE" and color == blue:
    PA34.RunCode()
elif choice == "GREEN" and color == green:
    PA34.RunCode()
else:
    PP3.RunCode()

file_name = RDK.getParam('PATH_OPENSTATION') + "/Image4.png"
RDK.Cam2D_Snapshot(file_name)

time.sleep(delay)

img = Image.open(file_name) # Open picture of cube
color = img.getpixel((320,240))
print (color)

if choice == "RED" and color == red:
    PA45.RunCode()
elif choice == "BLUE" and color == blue:
    PA45.RunCode()
elif choice == "GREEN" and color == green:
    PA45.RunCode()
else:
    PP4.RunCode()

file_name = RDK.getParam('PATH_OPENSTATION') + "/Image5.png"
RDK.Cam2D_Snapshot(file_name)

time.sleep(delay)

img = Image.open(file_name) # Open picture of cube
color = img.getpixel((320,240))
print (color)

if choice == "RED" and color == red:
    PA56.RunCode()
elif choice == "BLUE" and color == blue:
    PA56.RunCode()
elif choice == "GREEN" and color == green:
    PA56.RunCode()
else:
    PP5.RunCode()

file_name = RDK.getParam('PATH_OPENSTATION') + "/Image6.png"
RDK.Cam2D_Snapshot(file_name)

time.sleep(delay)

img = Image.open(file_name) # Open picture of cube
color = img.getpixel((320,240))
print (color)

if choice == "RED" and color == red:
    PA67.RunCode()
elif choice == "BLUE" and color == blue:
    PA67.RunCode()
elif choice == "GREEN" and color == green:
    PA67.RunCode()
else:
    PP6.RunCode()

file_name = RDK.getParam('PATH_OPENSTATION') + "/Image7.png"
RDK.Cam2D_Snapshot(file_name)

time.sleep(delay)

img = Image.open(file_name) # Open picture of cube
color = img.getpixel((320,240))
print (color)

if choice == "RED" and color == red:
    PA78.RunCode()
elif choice == "BLUE" and color == blue:
    PA78.RunCode()
elif choice == "GREEN" and color == green:
    PA78.RunCode()
else:
    PP7.RunCode()

file_name = RDK.getParam('PATH_OPENSTATION') + "/Image8.png"
RDK.Cam2D_Snapshot(file_name)

time.sleep(delay)

img = Image.open(file_name) # Open picture of cube
color = img.getpixel((320,240))
print (color)

if choice == "RED" and color == red:
    PA8HOME.RunCode()
elif choice == "BLUE" and color == blue:
    PA8HOME.RunCode()
elif choice == "GREEN" and color == green:
    PA8HOME.RunCode()
else:
    PP8.RunCode()