import copy
import dataclasses

import twophase.solver as sv
# import kociemba as ksv
# import serial
import math
import time
import requests
import threading
import pygame
import sys
import kociemba
import pygame
import sys
import sched

# serialPort = "/dev/ttyACM0"
# arduino = serial.Serial(serialPort, 115200, timeout=1)
# translator dictionaries
npTr = {
    "n": 1,
    "p": -1
}
pnTr = {
    "n": "p",
    "p": "n"
}
# dictionary which returns a face which is guaranteed to not be connected to a servo for a specific rotation
upDownDict = {0: 'U', 1: 'R', 2: 'B'}


class Cube:
    faceLocation = {
        "F": 0,
        "L": 1,
        "B": 2,
        "R": 3,
        "U": 4,
        "D": 5,
    }
    servoAngles = [[1, 1, 1, 1]]  # modify these to fit the final servo angles
    currentRotation = 0  # and cube rotations after scanning the cube.

    def rotate_fb(self, invert):
        values = list(self.faceLocation.values())
        keys = list(self.faceLocation.keys())
        temp = dict(zip(values, keys))
        # D -> L -> U -> R
        if not invert:
            keys = temp[0], temp[5], temp[2], temp[4], temp[1], temp[3]
        else:
            keys = temp[0], temp[4], temp[2], temp[5], temp[3], temp[1]
        return dict(zip(keys, values))

    def rotate_rl(self, invert):
        values = list(self.faceLocation.values())
        keys = list(self.faceLocation.keys())
        temp = dict(zip(values, keys))
        # D -> F -> U -> B
        if invert:
            keys = temp[4], temp[1], temp[5], temp[3], temp[2], temp[0]
        else:
            keys = temp[5], temp[1], temp[4], temp[3], temp[0], temp[2]
        return dict(zip(keys, values))
    # def rotate42(self, invert):


class Move:
    face = 'U'  # the face on which the move happens
    servoNum = 0  # the servo on which the move happens
    angle = 0  # the degree of rotation of the move (1, 2 or 3).
    # In the case of rotations, this is the rotation of servo 0 or 1
    index = 0  # the index within the total moves where this move happens

    def __init__(self, face=None, servo_num=None, angle=None, index=None):
        self.face = face
        self.servoNum = servo_num
        self.angle = angle
        self.index = index

    def __repr__(self):
        return str(self.index) + ": F:" + self.face + " A:" + str(self.angle) + " S:" + str(self.servoNum)


def set_servo(servo_num, angle):
    pass
    # requests.post(url, data={
    #     "servo": servo_num,
    #     "position": position,
    # })


class Servos:
    counter = 0
    servoAngles = [1, 1, 1, 1]
    rotator_servo_angles = {
        0: [[176], [175], [178], [173]],
        1: [[123], [123], [123], [115]],
        2: [[63], [60], [63], [56]],
        3: [[10], [0], [7], [2]],
    }
    retraction_servo_angles = {
        'extend': [3, 7, 12, 57],
        'retract': [25, 30, 35, 80],
    }
    # how long to wait after each length of move before turning the servo back
    moveTimings = [0.1, 0.2, 0.35]

    def __init__(self):
        x = True

    def tick(self):
        self.counter += 1

    def goto(self, index, position):
        if self.servoAngles[index] > position:
            set_servo(index * 2, self.rotator_servo_angles[position][index][0] + 5)
        elif self.servoAngles[index] < position:
            set_servo(index * 2, self.rotator_servo_angles[position][index][-1] - 5)
        diff = abs(self.servoAngles[index] - position)
        scheduler.enter(self.moveTimings[diff - 1], 1,
                        set_servo, (index * 2, self.rotator_servo_angles[position][index][-1],))
        self.servoAngles[index] = position

    def move(self, index, move_by):
        if (self.servoAngles[index] + move_by) % 4 != self.servoAngles[index] + move_by:
            raise "servo overflowed"
        else:
            move_to = (self.servoAngles[index] + move_by) % 4
            self.goto(index, move_to)

    def retract(self, index):
        set_servo((index * 2) + 1, self.retraction_servo_angles['retract'][index])

    def extend(self, index):
        set_servo((index * 2) + 1, self.retraction_servo_angles['extend'][index])


# ___VARIABLES___
# timing stuff
scheduler = sched.scheduler(time.time, time.sleep)
scheduler.run()

# Network Stuff
pi_ip = '192.168.0.102'

url = "http://{0}:5000/receive".format(pi_ip)



# cube stuff
# cubeString = 'FFFFUFFFFUUUURUUUURRRRFRRRRBBBBDBBBBDDDDLDDDDLLLLBLLLL'
# strSolution = sv.solve(cubeString)
strSolution = 'L2 B2 U2 B2 R1 D2 L3 B3 L1 R2 B1 D3 F2 L2 U1 R3 U3 F3 L1 (19f)'
#strSolution = 'F2 U2 D1 B3 L1 (19f)'

if __name__ == '__main__':
    cube = Cube()

    # first, convert the solution to a list. Split it by whitespace and remove the final item
    solution = str.split(strSolution)[0:-1]
    new_solution = []
    for i, move in enumerate(solution):
        new_solution.append(Move(face=move[0], angle=int(move[1])))
    solution = new_solution
    # the structure of [1, 1, 1] is Up/down, Sides, Front/back
    rotationCompatDict = {'U': [0, 1, 1], 'D': [0, 1, 1], 'L': [1, 0, 1],
                          'R': [1, 0, 1], 'F': [1, 1, 0], 'B': [1, 1, 0], }
    compatibilities = [rotationCompatDict[move.face] for move in solution]
    # 0 is orientations for each move, performed before the move. The list is offset by one by the initial cube
    # position of 0. 1 is for the number of orientation changes
    rotationLists = [[[0], 0]]
    for move in solution:
        new_moveLists = []
        for mlist in rotationLists:
            # if the current move list doesn't match the current move, spawn two more and pick both to fix it.
            if rotationCompatDict[move.face][mlist[0][-1]] == 0:
                new_moveLists.append([mlist[0] + [(mlist[0][-1] + 1) % 3], mlist[1] + 1])
                new_moveLists.append([mlist[0] + [(mlist[0][-1] - 1) % 3], mlist[1] + 1])
            # else, keep the cube orientation the same
            else:
                new_moveLists.append([mlist[0] + [mlist[0][-1]], mlist[1]])
        # sort the new rotationLists in terms of fewest rotations each time we add new items
        new_moveLists.sort(key=lambda item: item[1])

        # loop through the rotationLists and cull all of them with two or more rotations than the lowest moveList
        # probably not the most efficient way of culling, but it reduces the computation from 1000s to 100s.
        minMoves = new_moveLists[0][1]
        rotationLists = []
        for mlist in new_moveLists:
            if mlist[1] <= minMoves + 1:
                rotationLists.append(mlist)

    # find a list of only the move-lists with the fewest number of rotations.
    minRotations = rotationLists[0][1]
    bestRotations = []
    for mlist in rotationLists:
        if mlist[1] == minRotations:
            # remove the 0th item we used earlier
            mlist[0].pop(0)
            bestRotations.append(mlist)
        else:
            break
    print("best: " + str(bestRotations))
    # next, find the string of servo moves based on the rotation moves. Loop through the original solution
    allMoves = []
    moveIndex = 0  # stores the index of the move - useful for stitching everything back together later
    for i, move in enumerate(solution):
        if cube.currentRotation != bestRotations[0][0][i]:
            # rotate the cube
            faceLocations1 = cube.rotate_rl(False)
            # if the index is greater than 3, then it must be either 4 or 5. This is good.
            if faceLocations1[upDownDict[bestRotations[0][0][i]]] > 3:
                cube.faceLocation = faceLocations1
                allMoves.append([Move(face='TRL', index=moveIndex, angle=1)])
                moveIndex += 1
            else:
                cube.faceLocation = cube.rotate_fb(False)
                allMoves.append([Move(face='TFB', index=moveIndex, angle=1)])
                moveIndex += 1

            cube.currentRotation = bestRotations[0][0][i]
        # try to combine opposing moves into one move
        try:
            if cube.faceLocation[allMoves[-1][0].face] == ((cube.faceLocation[move.face] + 2) % 4):
                allMoves[-1].append(move)
                moveIndex -= 1  # subtract one from the move index so when finally adds one, it stays the same
                # allMoves[-1] = [allMoves[-1], str(cube.faceLocation[move[0]]) + move[1]]
            else:
                allMoves.append([move])
                # allMoves.append(str(cube.faceLocation[move[0]]) + move[1])
        except KeyError:  # accessing the cube dictionary with a rotation move (e.g. tfb) yields an error))
            allMoves.append([move])

        except IndexError:
            allMoves.append([move])
        finally:
            move.index = moveIndex
            moveIndex += 1
    print("moves: " + str(allMoves))

    # first, split up the moves based on where rotations occur, and create a list of potential tweaks.
    cube2 = Cube()
    splitAllMoves02, splitAllMoves13 = [[]], [[]]
    for moveList in allMoves:
        for j, move in enumerate(moveList):
            if move.face[0] == 'T':  # check if the move is a rotation
                if move.face == 'TRL':  # branch for both types of rotation
                    splitAllMoves13[-1].append(move)
                    splitAllMoves02.append([])
                    cube2.faceLocation = cube2.rotate_rl(False)
                else:
                    splitAllMoves02[-1].append(move)
                    splitAllMoves13.append([])
                    cube2.faceLocation = cube2.rotate_fb(False)
            else:
                move.servoNum = cube2.faceLocation[move.face]
                if move.servoNum == 0 or move.servoNum == 2:
                    splitAllMoves02[-1].append(move)
                else:
                    splitAllMoves13[-1].append(move)

    # finally, loop through all splitAllMoves lists again, and combine moves with the same indices
    lastIndex = -1
    for splitAllMoves in (splitAllMoves02, splitAllMoves13):
        for i, splitList in enumerate(splitAllMoves):
            new_splitList = []
            for move in splitList:
                if lastIndex == move.index:
                    new_splitList[-1] += [move]
                else:
                    new_splitList.append([move])
                lastIndex = move.index
            splitAllMoves[i] = new_splitList

    # print(tweaks)
    print("Split::(230)")
    print(splitAllMoves02)
    print(splitAllMoves13)


    # tries to calculate the solution as far as it can.
    # recursively calls itself and accepts the longest solution
    # returns: the move list, the servo it failed on, how far it got.
    def run_solution(moves, servo_angles):
        result_optimal = 0
        for i, move1 in enumerate(moves):
            for move in move1:  # for each move, check what type of move it is.
                if move.face[0] == 'T':
                    # for rotation moves, spawn two new functions for each direction of possible rotation
                    rotator_servos = []
                    if move.face == 'TFB':
                        rotator_servos = [0, 2]
                    else:  # move.face == 'TRL':
                        rotator_servos = [1, 3]
                    # if neither rotation is possible, call add_retraction
                    # I'm brute forcing this without being clever because I'm lazy
                    clockwise_possible = True
                    anticlockwise_possible = True
                    if servo_angles[rotator_servos[0]] == 0:
                        anticlockwise_possible = False
                    elif servo_angles[rotator_servos[0]] == 3:
                        clockwise_possible = False

                    if servo_angles[rotator_servos[1]] == 3:
                        anticlockwise_possible = False
                    elif servo_angles[rotator_servos[1]] == 0:
                        clockwise_possible = False

                    possible_solutions = []
                    next_moves = copy.copy(moves[i + 1::])
                    insert_moves = []
                    if clockwise_possible or anticlockwise_possible:
                        if clockwise_possible:
                            insert_moves.append([
                            [
                                Move('retract', servo_num=(rotator_servos[0]+1) % 4,
                                     angle=0, index=move.index-0.1),
                                Move('retract', servo_num=(rotator_servos[1]+1) % 4,
                                     angle=0, index=move.index-0.1)
                            ],
                                [Move(move.face, servo_num=rotator_servos[0],
                                     angle=1, index=move.index),
                                Move(move.face, servo_num=rotator_servos[1],
                                     angle=-1, index=move.index)],
                            [
                                Move('extend', servo_num=(rotator_servos[0]+1) % 4,
                                     angle=0, index=move.index+0.1),
                                Move('extend', servo_num=(rotator_servos[1]+1) % 4,
                                     angle=0, index=move.index+0.1)
                            ],
                            ])
                            next_servo_angles = copy.copy(servo_angles)
                            next_servo_angles[rotator_servos[0]] += 1
                            next_servo_angles[rotator_servos[1]] -= 1
                            possible_solutions.append(run_solution(next_moves, next_servo_angles))
                        if anticlockwise_possible:
                            insert_moves.append([
                            [
                                Move('retract', servo_num=(rotator_servos[0]+1) % 4,
                                     angle=0, index=move.index-0.1),
                                Move('retract', servo_num=(rotator_servos[1]+1) % 4,
                                     angle=0, index=move.index-0.1)
                            ],
                            [
                                Move(move.face, servo_num=rotator_servos[0],
                                     angle=-1, index=move.index),
                                Move(move.face, servo_num=rotator_servos[1],
                                     angle=1, index=move.index)
                            ],
                            [
                                Move('extend', servo_num=(rotator_servos[0]+1) % 4,
                                     angle=0, index=move.index-0.1),
                                Move('extend', servo_num=(rotator_servos[1]+1) % 4,
                                     angle=0, index=move.index-0.1)
                            ],
                            ])
                            next_servo_angles = copy.copy(servo_angles)
                            next_servo_angles[rotator_servos[0]] += 1
                            next_servo_angles[rotator_servos[1]] -= 1
                            possible_solutions.append(run_solution(next_moves, next_servo_angles))
                    else:
                        result_optimal -= 100
                        # for now, brute force a retraction move which shifts the first servo
                        # into a position usable by the second
                        # there are only 2 cases where both rotation directions are impossible:
                        #   servo 1 == 0 and servo 2 == 0
                        #   servo 1 == 3 and servo 2 == 0
                        next_servo_angles = copy.copy(servo_angles)
                        if servo_angles[rotator_servos[1]] == 3:
                            insert_moves.append([[Move(face='Retraction', servo_num=rotator_servos[0],
                                                      angle=-1, index=move.index + .1)]])
                            next_servo_angles[rotator_servos[1]] -= 1
                        elif servo_angles[rotator_servos[1]] == 0:
                            insert_moves.append([[Move(face='Retraction', servo_num=rotator_servos[0],
                                                      angle=1, index=move.index + .1)]])
                            next_servo_angles[rotator_servos[1]] += 1
                        possible_solutions.append(run_solution(next_moves, next_servo_angles))

                    max_score = -2147483648
                    best_index = 1
                    # evaluate the possible solutions and pick the one with the highest optimization score
                    for x, tried_solution in enumerate(possible_solutions):
                        if tried_solution[1] > max_score:
                            best_index = x
                            max_score = tried_solution[1]

                    return [moves[0:i] + insert_moves[best_index] + possible_solutions[best_index][0],
                            result_optimal + possible_solutions[best_index][1]]
                else:
                    if move.position == 2:
                        # double moves are always possible. all we need to do is check which way the double move can go,
                        # and if needed, set 2 to -2
                        if servo_angles[move.servoNum] > 1:
                            move.position = -2
                    else:
                        # single moves are again always possible.
                        # However, sometimes we need to do an inverted 270-degree rotation
                        # this can be simplified by finding the final position by adding the position to the current position,
                        # and taking modulo 4
                        final_position = (servo_angles[move.servoNum] + move.position) % 4
                        # then we can just calculate the position by the change in current position and final position
                        move.position = final_position - servo_angles[move.servoNum]
                    # finally, apply the move to the servo_angles
                    servo_angles[move.servoNum] += move.position
                    # reduce optimal score if the move position is 3
                    if abs(move.position) == 3:
                        result_optimal -= 1
        return [moves, result_optimal]


    print("")


    def solve_moves(all_split_moves, which_servos):
        result = []
        rotation_info = []
        result += run_solution(all_split_moves[0], [1, 1, 1, 1])[0]
        # solve the rest, inputting every possible combination of servo angles (since there are only 9)
        for splitMoves in all_split_moves[1::]:
            calculated_solutions = []
            for s1 in range(3):
                for s2 in range(3):
                    if which_servos == [0, 2]:
                        servos_start = [s1, 0, s2, 0]
                    else:
                        servos_start = [0, s1, 0, s2]
                    calculated_solutions.append(run_solution(copy.copy(splitMoves), servos_start) + [[s1, s2]])
            # find the best calculated_solution
            max_optimal = -2147483648
            best_solution = []
            for _solution in calculated_solutions:
                if _solution[1] > max_optimal:
                    max_optimal = _solution[1]
                    best_solution = _solution
            print(best_solution)
            result += best_solution[0]
            rotation_info.append(best_solution[2])
        return result, rotation_info

    splitAllMoves02, rotationInfo02 = solve_moves(splitAllMoves02, [0, 2])
    splitAllMoves13, rotationInfo13 = solve_moves(splitAllMoves13, [1, 3])
    # combine the splitmoves into two lists. Handle the different rotation directions
    allMoves = []
    # combinedMoves02 = []
    # combinedMoves13 = []
    # stores the position in both splitMoves lists
    c_02 = 0
    c_13 = 0
    # whether we flip over the servos for each thing
    _02Reversed = False
    _13Reversed = False
    # the currently stored move index
    index = -1
    while True:
        # bit of a janky way of doing this. The first half is for handling ending the loop
        if c_02 != len(splitAllMoves02) and (c_13 == len(splitAllMoves13) or
                                             splitAllMoves02[c_02][0].index < splitAllMoves13[c_13][0].index):
            if _02Reversed and not splitAllMoves02[c_02][0].face[0] == 'T':  # do not reverse rotation moves
                for move in splitAllMoves02[c_02]:
                    move.servoNum = (move.servoNum + 2) % 4
            allMoves.append(splitAllMoves02[c_02])
            # if the move is a rotation, check whether it is an inverse rotation
            if splitAllMoves02[c_02][0].face[0] == 'T' and splitAllMoves02[c_02][0].position == -1:
                _13Reversed = not _13Reversed
            c_02 += 1
        elif c_13 != len(splitAllMoves13):  # splitAllMoves13[c_02][0].index < splitAllMoves02[c_13][0].index:
            if _13Reversed and not splitAllMoves13[c_13][0].face[0] == 'T':  # do not reverse rotation moves
                for move in splitAllMoves13[c_13]:
                    move.servoNum = (move.servoNum + 2) % 4
            allMoves.append(splitAllMoves13[c_13])
            # if the move is a rotation, check whether it is an inverse rotation
            if splitAllMoves13[c_13][0].face[0] == 'T' and splitAllMoves13[c_13][0] == -1:
                _02Reversed = not _02Reversed
            c_13 += 1
        else:
            break

    for i in allMoves:
        for j in i:
            print(j.face + ", S: ", str(j.servoNum) + ", A: ", j.angle)
    # Servos Class
    servos = Servos()
    cube = Cube()

    # self check whether moves work
    rotationCounter02 = 0
    rotationCounter13 = 0
    for j in allMoves:
        output = ""
        if j[0].face[0] == 'T':
            # handle servos opposite to the rotation. Set them to the starting angles based on 'rotationInfo'
            if j[0].face == 'TFB':
                servos.goto(1, rotationInfo02[rotationCounter02][0])
                servos.goto(3, rotationInfo02[rotationCounter02][1])
                cube.rotate_fb(True)
                rotationCounter02 += 1
            elif j[0].face == 'TRL':
                servos.goto(0, rotationInfo13[rotationCounter13][0])
                servos.goto(2, rotationInfo13[rotationCounter13][1])
                cube.rotate_rl(True)
                rotationCounter13 += 1
        for k, move in enumerate(j):
            if move.face == "retract":
                servos.retract(move.servoNum)
            elif move.face == "extend":
                servos.extend(move.servoNum)
            elif move.face == "retraction":
                raise "No worky"
            else:
                servos.move(move.servoNum, move.position)
            # output += str(i.servoNum) + " " + str(i.position) + " | "
        scheduler.run(True)
        #time.sleep(0.25)
        print(output)

    for i in range(4):
        servos.retract(i)
        #time.sleep(0.2)
        servos.goto(i, 0)
        servos.goto(i, 1)
        #time.sleep(0.2)
    servos.extend(2)
    #time.sleep(.5)
    servos.extend(0)
    #time.sleep(0.25)
    servos.extend(1)
    servos.extend(3)

    #input()
    rotationCounter02 = 0
    rotationCounter13 = 0
    for j in allMoves:
        output = ""
        if j[0].face[0] == 'T':
            # handle servos opposite to the rotation. Set them to the starting angles based on 'rotationInfo'
            if j[0].face == 'TFB':
                servos.goto(1, rotationInfo02[rotationCounter02][0])
                servos.goto(3, rotationInfo02[rotationCounter02][1])
                rotationCounter02 += 1
            elif j[0].face == 'TRL':
                servos.goto(0, rotationInfo13[rotationCounter13][0])
                servos.goto(2, rotationInfo13[rotationCounter13][1])
                rotationCounter13 += 1
        for k, move in enumerate(j):
            if move.face == "retract":
                servos.retract(move.servoNum)
            elif move.face == "extend":
                servos.extend(move.servoNum)
            elif move.face == "retraction":
                raise "No worky"
            else:
                servos.move(move.servoNum, move.position)
            # output += str(i.servoNum) + " " + str(i.position) + " | "
        scheduler.run(True)
        #time.sleep(0.25)
        print(output)

    # for splitMoves0 in splitAllMoves02:
    #     for splitMoves in splitMoves0:
    #         x = 0
    #     # first, add an item to combinedMoves02

    # todo:
    # add in retractions where other solutions are not possible
    # change the initial position of servos (except for at the start of the solution)
    # def run_solution(moves, servo_start, which_servos, move_index_start):
    #     servo_angles = [servo_start]
    #     counter = 0
    #     #result_moves = []
    #     move_index = move_index_start
    #     while True:
    #         servo_angles.append(copy.copy(servo_angles[-1]))
    #         for k, f_move in enumerate(moves[counter]):
    #             # first, check for rotations
    #             if f_move[0] == 'T':
    #                 # create a list of remaining moves to pass higher up the stack
    #                 new_moves = copy.copy(moves[counter:])
    #                 # pop out the first item from the first moves
    #                 # (prevents the move from being handled twice higher up the stack)
    #                 new_moves[0].pop(k)
    #                 # inverted rotation: false.
    #
    #                 # create new variables to pass to the next function
    #                 new_servo_angles = copy.copy(servo_angles[-1])
    #                 new_servo_angles[which_servos[0]] += 1
    #                 new_servo_angles[which_servos[1]] -= 1
    #                 val = validate_servos(new_servo_angles)
    #                 # not sure but it seems important
    #                 try_ = []  # [[result_moves, counter, val[1]]]
    #                 if val[0]:
    #                     # append to the try_ list the result_moves from the next function,
    #                     # and whether the rotation is inverted (false)
    #                     try_.append([run_solution(new_moves, new_servo_angles, which_servos), False])
    #                 # create new variables to pass to the next function
    #                 new_servo_angles = copy.copy(servo_angles[-1])
    #                 new_servo_angles[which_servos[0]] -= 1
    #                 new_servo_angles[which_servos[1]] += 1
    #                 if validate_servos(new_servo_angles)[0]:
    #                     # append to the try_ list the result_moves from the next function,
    #                     # and whether the rotation is inverted (true)
    #                     try_.append([run_solution(new_moves, new_servo_angles, which_servos), True])
    #                 # find the longest of the three lists
    #                 max_ = -1
    #                 max_try_result = None
    #                 for y, try_result in enumerate(try_):
    #                     if int(try_result[0][1]) > max_:
    #                         max_try_result = try_result
    #
    #                 # look for what the try_result indicates whether the rotation is inverted or not.
    #                 if max_try_result is None:
    #                     return "aaaaaaa"
    #                 elif not max_try_result[1]:
    #                     result_moves[-1].append(str(which_servos[0]) + "+1")
    #                     result_moves[-1].append(str(which_servos[1]) + "-1")
    #                 elif max_try_result[1]:
    #                     result_moves[-1].append(str(which_servos[0]) + "-1")
    #                     result_moves[-1].append(str(which_servos[1]) + "+1")
    #                 result_moves += max_try_result[0][0]
    #                 return [result_moves, max_try_result[0][1], max_try_result[0][2]]
    #                 # return try_[max_index]
    #
    #             # if the move is a double move, try to pick a direction which works.
    #             # if the direction doesn't work, return the current result_moves.
    #             elif f_move[2] == 2:
    #                 servo_num = int(f_move[0])
    #                 if servo_angles[servo_num] == 1:
    #                     # spawn two new instances for each possible retraction move
    #                     return [result_moves, counter, servo_num]
    #                 elif servo_angles[servo_num] == 0:
    #                     servo_angles[servo_num] = 2
    #                     result_moves[-1].append(str(servo_num) + "+2")
    #                 else:
    #                     servo_angles[servo_num] = 0
    #                     result_moves[-1].append(str(servo_num) + "-2")
    #             # for all regular moves, apply them and then check if the servo is still valid
    #             else:
    #                 servo_num = int(f_move[0])
    #                 # for forward moves
    #                 if f_move[1] == '+':
    #                     servo_angles[-1][servo_num] += 1
    #                     if servo_angles[-1][servo_num > 2]:
    #                         # spawn two new instances for each possible retraction move
    #                         # new_result = copy.copy(result_moves)
    #                         # result_moves.append(str(servo_num) + "+1")
    #                         return [result_moves, counter, servo_num]
    #                     else:
    #                         result_moves[-1].append(str(servo_num) + "+1")
    #                 # for reverse moves
    #                 else:
    #                     servo_angles[-1][servo_num] -= 1
    #                     if servo_angles[-1][servo_num < 0]:
    #                         return [result_moves, counter, servo_num]
    #                     else:
    #                         result_moves[-1].append(str(servo_num) + "-1")
    #         counter += 1
    #         if counter == len(moves) - 1:
    #             return [result_moves, counter, -2]
    #
    # def validate_servos(servo_angles):
    #     valid = True
    #     failed = -1
    #     num = 0
    #     for a, ang in enumerate(servo_angles):
    #         if ang > 2 or ang < 0:
    #             valid = False
    #             failed = a
    #             num = ang
    #     return valid, failed, num
    # solve first two lists where we can't change the servo angles

    # #print(
    # #print(run_solution(splitAllMoves13[0], [1, 1, 1, 1]))
    # splitSolutions02 = []
    # splitSolution12 = []
    # # solve the rest, inputting every possible combination of servo angles (since there are only 9)
    # for splitMoves in splitAllMoves02[1::]:
    #     calculated_solutions = []
    #     for s1 in range(3):
    #         for s2 in range(3):
    #             calculated_solutions.append(run_solution(copy.copy(splitMoves), [s1, 0, s2, 0]))
    #     # find the best calculated_solution
    #     maxOptimal = -2147483648
    #     best_solution = []
    #     for solution in calculated_solutions:
    #         if solution[1] > maxOptimal:
    #             maxOptimal = solution[1]
    #             best_solution = solution
    #     print(best_solution)
    # for splitMoves in splitAllMoves13[1::]:
    #     calculated_solutions = []
    #     for s1 in range(3):
    #         for s2 in range(3):
    #             calculated_solutions.append(run_solution(splitMoves, [0, s1, 0, s2]))
    #     # find the best calculated_solution
    #     maxOptimal = -2147483648
    #     best_solution = []
    #     for solution in calculated_solutions:
    #         if solution[1] > maxOptimal:
    #             maxOptimal = solution[1]
    #             best_solution = solution
    #     moves13.append(best_solution)
    #     #print(best_solution)
    #
    # next, stitch all of the lists back together
    # def apply_move(move, servo_angles, inverted):
    #     if move

    # # first, count the number of tweaks.
    # tweaksCounter = 0
    # for mList in allMoves:
    #     for move in mList:
    #         if move[1] == '2' or move[0] == 'T':
    #             tweaksCounter += 1
    # print(tweaksCounter)
    #
    # tried = []
    # for tweaksTweaker in range(int(math.pow(2, tweaksCounter))):
    #     tried.append(try_moves(format(tweaksTweaker, ('0' + str(tweaksCounter) + 'b'))))
    #
    # tried.sort(key=lambda item: item[1], reverse=True)
    # print(tried[0])
    # print(servoAngles)
    # print(moveList)
    #     if j == len(move_list):
    #         return [moves, millis]11
    #     elif move_list[j][0] == 'T':
    #         break
    #     cube.servoAngles.append(copy.copy(cube.servoAngles[-1]))
    #     try:
    #         r = add_move(move_list[j], cube)
    #     # if ValueError is thrown, then the move item contains a list of multiple parallel moves.
    #     # handle as such.
    #     except ValueError:
    #         r = [[], []]
    #         for m in move_list[j]:
    #             r1 = add_move(m, cube)
    #             r[0].append(r1[0])
    #             r[1].append(r1[1])
    #     # before we append the moves, check if the servoList is in range.
    #     for k, ang in enumerate(cube.servoAngles[-1]):
    #         if ang > 2:
    #             # loop back through the doubles list to find if we can invert a double to fix things
    #             for servo_ang in cube.servoAngles:
    #         elif ang < 0:
    #     moves.append(r[0])
    #     doubles.append(r[1])
    #     j += 1
    # # spawn two new solveMoves and pick whichever one completes in less time
    # new_cube = copy.copy(f_cube)
    # if move_list[j] == 'TRL':
    #     new_cube.faceLocation = f_cube.rotateRL(True)
    #     result1 = solve_moves(move_list[j + 1:], new_cube)
    #     new_cube.faceLocation = f_cube.rotateRL(False)
    #     result2 = solve_moves(move_list[j + 1:], new_cube)
    #     rotate_move = 'RL'
    # else:
    #     new_cube.faceLocation = f_cube.rotateFB(True)
    #     result1 = solve_moves(move_list[j + 1:], new_cube)
    #     new_cube.faceLocation = f_cube.rotateFB(False)
    #     result2 = solve_moves(move_list[j + 1:], new_cube)
    #     rotate_move = 'FB'
    # # pick the lowest time solution
    # if result1[1] < result2[1]:
    #     result = [moves + ["I" + rotate_move] + result1[0], millis + result1[1]]
    # else:
    #     result = [moves + ["N" + rotate_move] + result1[0], millis + result1[1]]
    # return result

    # # I'm putting the function here because even though i'm not supposed to , it makes the code easier to read.
    # def solve_moves(move_list, f_cube: Cube):
    #     moves = []
    #     # stores the servo on which a double move occurred. Stores -1 when there is no double move.
    #     doubles = []
    #     j = 0
    #     millis = 0
    #     while True:
    #         if j == len(move_list):
    #             return [moves, millis]
    #         elif move_list[j][0] == 'T':
    #             break
    #         cube.servoAngles.append(copy.copy(cube.servoAngles[-1]))
    #         try:
    #             r = add_move(move_list[j], f_cube)
    #         # if ValueError is thrown, then the move item contains a list of multiple parallel moves.
    #         # handle as such.
    #         except ValueError:
    #             r = [[], []]
    #             for m in move_list[j]:
    #                 r1 = add_move(m, f_cube)
    #                 r[0].append(r1[0])
    #                 r[1].append(r1[1])
    #         # before we append the moves, check if the servoList is in range.
    #         for k, ang in enumerate(cube.servoAngles[-1]):
    #             if ang > 2:
    #                 # loop back through the doubles list to find if we can invert a double to fix things
    #                 for servo_ang in cube.servoAngles:
    #
    #             elif ang < 0:
    #         moves.append(r[0])
    #         doubles.append(r[1])
    #         j += 1
    #     # spawn two new solveMoves and pick whichever one completes in less time
    #     new_cube = copy.copy(f_cube)
    #     if move_list[j] == 'TRL':
    #         new_cube.faceLocation = f_cube.rotateRL(True)
    #         result1 = solve_moves(move_list[j + 1:], new_cube)
    #         new_cube.faceLocation = f_cube.rotateRL(False)
    #         result2 = solve_moves(move_list[j + 1:], new_cube)
    #         rotate_move = 'RL'
    #     else:
    #         new_cube.faceLocation = f_cube.rotateFB(True)
    #         result1 = solve_moves(move_list[j + 1:], new_cube)
    #         new_cube.faceLocation = f_cube.rotateFB(False)
    #         result2 = solve_moves(move_list[j + 1:], new_cube)
    #         rotate_move = 'FB'
    #     # pick the lowest time solution
    #     if result1[1] < result2[1]:
    #         result = [moves + ["I" + rotate_move] + result1[0], millis + result1[1]]
    #     else:
    #         result = [moves + ["N" + rotate_move] + result1[0], millis + result1[1]]
    #     return result
    #
    # # modifies cube and returns the new move
    # def add_move(_move, f_cube: Cube):
    #     num = int(_move[1])
    #     servo = f_cube.faceLocation[_move[0]]
    #     if num == 2:
    #         doubles = servo
    #     else:
    #         doubles = -1
    #     if num != 3:
    #         f_cube.servoAngles[-1][servo] += num
    #     else:
    #         f_cube.servoAngles[-1][servo] -= 1
    #     return _move, doubles

    # cube = Cube()
    # final_solution = solve_moves(allMoves, cube)
    # print(cube.servoAngles)
    # print(final_solution)

    # print(servoMoves)
    # print(servoAngles)
    #
    # # write to serial port
    # print(num)
    # print(len(servoMoves))
    #
    # servoSend = ""
    # for signal in servoMoves:
    #     if len(signal) == 2:
    #         servoSend = servoSend + signal + "zz"
    #
    #     else:
    #         servoSend = servoSend + signal
    # print(servoSend)
    # arduino.write(bytes(servoSend, 'utf-8'))
    # for signal in servoMoves:
    #      arduino.write(bytes(signal, 'utf-8'))
    #      time.sleep(1.2)
# See PyCharm help at https://www.jetbrains.com/help/pycharm/
