''' Program by Bjoern Bundschuh, 10/2020
Program reads a string of letters from the Mindstorms hub.
This string consists of the letter WYORBG representing the  colors of all 54 facelets of a cube (White, Yellow, Orange, Red, Blue, Green)
These letters are converted to face positions UP, DOWN, RIGHT, LEFT, FRONT, BACK which are represented by the letters UDRLFB
The color of the center facelet of a face determines its position.
The resulting string of letters is fed to a cube-solving algorithm based on the algorithm Herbert Kociemb but converted to a pure python version by tcbegley.
The returned string of letters UDRLFB represents the required moves UP, DOWN, RIGHT, LEFT, FRONT, BACK to solve the cube. Any cube can be solved with 20 moves or less.
'''

# Module imports
import serial  # module from pyserial for serial communication
from time import sleep # function for delays
import solver # python version of the kociemba algorithm
 
# Constant definitions
HUBADDRESS = "COM12"  # Bluetooth address of the Mindstorms Hub, see readme to obtain address from your os
#HUBADDRESS = "/dev/tty.usbmodem3255387B33381" # USB-Address of the Mindstorms Hub (optional)

SEARCHSTRING = "cube=" # Prefix of the string of cubecolors
RESULTSTRINGLEN = 54 # Length of cubestring = all facelets of a cube

def readcube():
    ''' Reads cubestring from Mindstorms Hub via bluetooth or USB depending on definition of PORT ''' 
    pos=-1    # set pos to a value that no SEARCH string was found, yet
    while pos==-1:  # as long as the correct string has not been found, yet, repeat the following
        try:    # try to read from hub
            ser_bytes = ser.readline()  # read one full line till a new line command is found \n
            decoded_bytes = str(ser_bytes.decode("utf-8"))  #convert received bytes to a string 
            pos=decoded_bytes.rfind(SEARCHSTRING) # search for position of the SEARCH Prefix of cubestring
        except: # Do this if reading from hub is not possible at all or results in an error
            print("Error while reading from Hub")
            ser.close()  
            break

        if pos!=-1:
            cubestring=decoded_bytes[pos+len(SEARCHSTRING):len(decoded_bytes)-1]
            if len(cubestring)!=RESULTSTRINGLEN: pos=-1  # if cubestring is not of the correct length, read again
            break
    return cubestring


def convertcube(colors):
    '''convert cubecolors from colors WYORBG into URFDLB'''
    center_positions =  [4 , 13 , 22 , 31 , 40 , 49]   # position of center facelets within colorstring
    center_code = "URFDLB"  # code letters of center facelets
    center_colors= ""
    
    for facelet in center_positions:
        center_colors += colors[facelet]  # get the colors of the center facelets of each face

    codes="" # empty the cubecode string
    for position in range(54):
        codes += center_code[center_colors.find(colors[position])]  # convert each facelet based on the center facelet colors
    return codes

def drawcube(colors):
    '''creates a graphical representation of the cube'''
    rowcolors=""    # to be printed string to display one row of the cube 
    facesequence=[0,1,2,3,4,5,6,7,8,
                  36,37,38,18,19,20,9,10,11,45,46,47,
                  39,40,41,21,22,23,12,13,14,48,49,50,
                  42,43,44,24,25,26,15,16,17,51,52,53,
                  27,28,29,30,31,32,33,34,35] # sequence of facelets in cubecolors or cubecode to print
    facelet=0 # set starting facelet
    print("        |-------|") # upper line
    for row in range(9): # iterate over the 9 rows to display the cube
        if row==3 or row ==6: # print line above and below the middle block (L, F, R, B)
            print("|-------|-------|-------|-------|")
        if row < 3 or row > 5: # For the U and D Face
            rowcolors="        |-" # create spaces
            for col in range(3): # for the 3 rows in U and D Face
               rowcolors+=colors[facesequence[facelet]]
               facelet+=1
               rowcolors+="-"
        else:
            rowcolors="|-"
            for col in range(12):   # for the L, F, R and B Face
                rowcolors+=colors[facesequence[facelet]]
                facelet+=1
                rowcolors+="-"
                if col==2 or col==5 or col==8: # Create a line between the faces
                    rowcolors+="|-"
        rowcolors+="|"  # end row with a line
        print(rowcolors) 
    print("        |-------|") # lower line

def sendsolution(cubesolution): 
    ''' transmit the solutioncode as a string to the hub'''
    ser.write(bytes(cubesolution+"\n\r", 'utf-8'))
    return

##################################################
########### main program #########################
##################################################

while True:
    print("Try to connect to Hub")
    try:
        ser=serial.Serial(HUBADDRESS, timeout=1)
        print("Connection to Hub successful!")
        break
    except:
        print("Connection not established!")
        print("Make sure the Hub is switched on and not connected to any other device")
        input("Hit enter to try again...")
        print("...")


while True: # run program till window is closed
    cubecolors = readcube() # wait for scanned cube to be send from hub
    drawcube(cubecolors) # grafical representation of cube
    cubecode = convertcube(cubecolors) # convert cubecolors from colors WYORBG into URFDLB
    try:
        solutioncode=solver.solve(cubecode) # solve the cube with max 20 movs and max. 20 sec calculation time
        print("Solution: ",solutioncode) # print the solution on the PC-screen
    except:
        solutioncode="error"
        print("Cube was scanned incorrectly - scan should be repeated")
    sendsolution(solutioncode) #send solution string to PC