#!/usr/bin/python
# coding: utf-8

"""
#############################################################################################################
# Andrea Favero 10 April 2024
# 
# Script to virtually solve random generated Rubik's cube status, by the by CUBOTino_T
#
# The random cube status are generated by the Kociemba solver.
# Afterward, the solver is asked to provide the solution for that status.
# The solver solution is sent to the robot solver: The robot solver estimates the servos solving time.
# 
# The robot solution is sent to a virtual manipulator, that applies the CUBOTino movements.
# The cube status is updated after each virtual cube manipulation (Flip, Spin and/or Rotate).
# The final cube status, after last manipulation, is verified if resembling a solved cube.
#
#############################################################################################################
"""



################  setting argparser for robot remote usage, and other settings  #################
import argparse

# argument parser object creation
parser = argparse.ArgumentParser(description='Test random cube status via a virtual cube manipulator')


# --debug argument is added to the parser
parser.add_argument("-d", "--debug", action='store_true',
                    help="Activates printout of settings, variables and info for debug purpose")

# --print argument is added to the parser
parser.add_argument("-p", "--print", action='store_true',
                    help="Print to terminals some info related to the simulation")

# --runs argument is added to the parser
parser.add_argument("-r", "--runs", type=int,
                    help="Input the number of random cubes to test")

# --plot argument is added to the parser
parser.add_argument("--plot", action='store_true',
                    help="Enables a graphical animation")

# --status argument is added to the parser
parser.add_argument("-s", "--status", type=str,
                    help="Enter the cube status and test the solver")

args = parser.parse_args()   # argument parsed assignement
# ###############################################################################################





def imports(plot):
    global np, math, time, rm, servo, dt, os, path, pathlib
    
    import math                                   # math library
    import time                                   # time check
    import numpy as np                            # arrays
    import datetime as dt                         # used for timestamp
    import os.path, pathlib                       # folder names management
    import Cubotino_T_moves as rm                 # custom library, traslates the cuber solution string in robot movements string
    import Cubotino_T_servos as servo             # custom library for the servos control
    if plot:                                      # case plot is set True (cube status sketch plotting to screen)
        global cv2                                # openCV library is set as global variable
        print("Loading OpenCV")                   # feedback is printed to terminal to manage thewaiting time         
        import cv2                                # openCV library is imported





def clear_terminal():
    """ Clears the terminal and positions the cursors on top left; Still possible to scroll up in case of need"""
    print("\x1b[H\x1b[2J")                        # escape sequence





def introduction():
    row = "#"*95                                  # string of characters used as separator
    print(row)                                    # separator is printed to the terminal
    print(" Script to virtually test CUBOTino_T solver, with random Rubik's cube status.")
    print(" The random cube status are generated by the Kociemba solver.")
    print(" The permutation is sent to the Kociemba solver, that returns one solution.")
    print(" The solution is sent to the robot solver, that returns the moves and othe info.")
    print(" The robot solution is applied to a virtual manipulator with the CUBOTino movements.")
    print(" The cube status is updated after each virtual cube manipulation (Flip, Spin or Rotate).")
    print(" The final cube status, after last manipulation, is verified if resembling a solved cube.")
    print(" The script has several arguments, type --help to check")
    print(row,"\n")                               # separator is printed to the terminal





def import_solver():
    """Importing Kociemba solver."""
    
    global sv, cubie
    
    # this import takes some time to be uploaded
    # there are three import attempts, that considers different solver installation methods
    try:                                                  # attempt
        import solver as sv                               # import Kociemba solver copied in /home/pi/cubotino/src
        import cubie as cubie                             # import cubie Kociemba solver library part
        solver_found = True                               # boolean to track no exception on import the copied solver
        if debug:                                         # case debug variable is set True            
            print('Found Kociemba solver copied in active folder')   # feedback is printed to the terminal
    except:                                               # exception is raised if no library in folder or other issues
        solver_found = False                              # boolean to track exception on importing the copied solver
       
    if not solver_found:                                  # case the solver library is not in active folder
        import os.path, pathlib                           # library to check file presence            
        folder = pathlib.Path().resolve()                 # active folder (should be home/pi/cubotino/src)  
        fname = os.path.join(folder,'twophase','solver.py')   # active folder + twophase' + solver name
        if os.path.exists(fname):                         # case the solver exists in 'twophase' subfolder
            try:                                          # attempt
                import twophase.solver as sv              # import Kociemba solver copied in twophase sub-folder
                import twophase.cubie as cubie            # import cubie Kociemba solver library part
                solver_found = True                       # boolean to track the  import the copied solver in sub-folder
                if debug:                                 # case debug variable is set True            
                    print('Found Kociemba solver copied in subfolder')  # feedback is printed to the terminal
            except:                                       # exception is raised if no library in folder or other issues
                pass                                      # pass, to keep solver_found = False as it was before
        
    if not solver_found:                                  # case the library is not in folder or twophase' sub-folder
        try:                                              # attempt
            import twophase.solver as sv                  # import Kociemba solver installed in venv
            import twophase.cubie as cubie                # import cubie Kociemba solver library part
            twophase_solver_found = True                  # boolean to track no exception on import the installed solver
            if debug:                                     # case debug variable is set True
                print('Found Kociemba solver installed')  # feedback is printed to the terminal
        except:                                           # exception is raised if no library in venv or other issues
            twophase_solver_found = False                 # boolean to track exception on importing the installed solver
    

    if not solver_found and not twophase_solver_found:    # case no one solver has been imported
        print('\nNot found Kociemba solver')              # feedback is printed to the terminal
        return False
    else:
        return True





def scramble():
    """Random cubegenerator, from Kociemba solver library. """
    cc = cubie.CubieCube()                        # cube in cubie reppresentation
    cc.randomize()                                # randomized cube in cubie reppresentation 
    return str(cc.to_facelet_cube())              # returns a randomized cube in facelets string reppresentation





def cube_facelets_permutation(cube_status, move_type, direction):
    """Function that updates the cube status, according to the move type the robot does
       The 'ref' tuples provide the facelet current reference position to be used on the updated position.
       As example, in case of flip, the resulting facelet 0 is the one currently in position 23 (ref[0]).
       The initial cube orientation is not the one the robos has after the scanning process."""

    if move_type == 'F':      # case the robot move is a cube flip (complete cube rotation around L-R horizontal axis) 
        ref=(53,52,51,50,49,48,47,46,45,11,14,17,10,13,16,9,12,15,0,1,2,3,4,5,6,7,8,18,
             19,20,21,22,23,24,25,26,42,39,36,43,40,37,44,41,38,35,34,33,32,31,30,29,28,27)
    
    elif move_type == 'S':    # case the robot move is a spin (complete cube rotation around vertical axis)
        if direction == '1':  # case spin is CW
            ref=(2,5,8,1,4,7,0,3,6,18,19,20,21,22,23,24,25,26,36,37,38,39,40,41,42,43,44,
                 33,30,27,34,31,28,35,32,29,45,46,47,48,49,50,51,52,53,9,10,11,12,13,14,15,16,17)
        elif direction == '3':   # case spin is CCW
            ref=(6,3,0,7,4,1,8,5,2,45,46,47,48,49,50,51,52,53,9,10,11,12,13,14,15,16,17,
                 29,32,35,28,31,34,27,30,33,18,19,20,21,22,23,24,25,26,36,37,38,39,40,41,42,43,44)   
    
    elif move_type == 'R':    # case the robot move is a rotation (lowest layer rotation versus mid and top ones) 
        if direction == '1':  # case 1st layer rotation is CW
            ref=(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,24,25,26,18,19,20,21,22,23,42,43,44,
                 33,30,27,34,31,28,35,32,29,36,37,38,39,40,41,51,52,53,45,46,47,48,49,50,15,16,17)
        elif direction == '3':   # case 1st layer rotation is CCW
            ref=(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,51,52,53,18,19,20,21,22,23,15,16,17,
                 29,32,35,28,31,34,27,30,33,36,37,38,39,40,41,24,25,26,45,46,47,48,49,50,42,43,44)
    
    new_status = ""              # empty string to generate the cube status, updated according to move_type and direction
    for i in range(54):          # iteration over the 54 facelets
        new_status+=str(cube_status[ref[i]])  # updated cube status takes the facelet from previous status at ref location
    
    return new_status            # updated cube status is returned





def cube_sketch_coordinates(x_start, y_start, d, gap=0):
    """ Generates a list and a dict with the top-left coordinates of each facelet, as per the URFDLB order.
    These coordinates are later used to draw a cube sketch
    The cube sketch (overall a single rectangle with all the facelets in it) starts at x_start, y_start
    Each facelet on the sketch has a square side dimention = edge, defined at start_up() function."""
    
    square_start_pt=[]                            # lits of all the top-left vertex coordinate for the 54 facelets
    
    g = abs(int(gap))                                      # gap (in pixels) between the cube faces
    
    # the six cube faces are noted from 0 to 5.
    # the top left corner coordinate is defined as function of x_start, y_start and d (=distance)
    starts={0:(x_start+3*d+g   , y_start),
            1:(x_start+6*d+2*g , y_start+3*d+g),
            2:(x_start+3*d+g   , y_start+3*d+g),
            3:(x_start+3*d+g   , y_start+6*d+2*g),
            4:(x_start         , y_start+3*d+g),
            5:(x_start+9*d+3*g , y_start+3*d+g)}   # dict with the top-left coordinate of each face (not facelets !)
    
    for value in starts.values():                 # iteration over the 6 faces
        x_start=value[0]                          # x coordinate fo the face top left corner
        y_start=value[1]                          # y coordinate fo the face top left corner
        y = y_start                               # y coordinate value for the first 3 facelets
        for i in range(3):                        # iteration over rows
            x = x_start                           # x coordinate value for the first facelet
            for j in range(3):                    # iteration over columns
                square_start_pt.append([x, y])    # x and y coordinate, as list, for the top left vertex of the facelet is appendended
                x = x+d                           # x coordinate is increased by square side
                if j == 2: y = y+d                # once at the third column the row is incremented
    
    facelets_start={}                             # empty dictionary to store the facelets starting coordinates
    inner_points={}                               # empty dictionary to store the array with 4 inner verteces
    d=d-2                                         # square edge is reduced by 2 pixels 
    for i in range(54):                           # iteration over the 54 facelets
        coordinate = tuple(square_start_pt[i])    # starting coordinate is taken from the square_start_pt dictionary
        x = coordinate[0]+1                       # top left x coordinate is shifted by 1 pixel to the right
        y = coordinate[1]+1                       # top left y coordinate is shifted by 1 pixel to the bottom
        pts = np.array([(x,y),(x+d,y),(x+d,y+d),(x,y+d)])  # array with tupples of square coordinates, shifted by 1 pixel toward the inside
        facelets_start[i] = coordinate            # starting coordinate is added to the facelets_start dictionary
        inner_points[i] = pts                     # array with the 4 square inner vertex coordinates is added to inner_points dict
    
    return facelets_start, inner_points





def plot_interpreted_colors(wait, cube_status, test, startup=False, kill=False):
    """ Based on the detected cube status, a sketch of the cube is plot with bright colors on the pictures collage."""
    
    if startup:
        global assigned_colors, sketch, frame, start_points, inner_points, x_start, y_start, d
        global font, fontScale, fontColor, lineType
        
        font = cv2.FONT_HERSHEY_SIMPLEX           # type of cv2 font used
        fontScale = 1                             # font size used
        fontColor = (255,255,255)                 # font color
        lineType = 2                              # font thickness
        
        assigned_colors = {'U':(255, 255, 255), 'R':(29, 32, 185), 'F':(35, 144, 0),
                           'D':(50, 255, 255), 'L':(0, 128, 255), 'B':(100, 65, 0)}  # BGR colors for the facelets
        
        cv2.namedWindow('cube')                   # create the cube window
        cv2.moveWindow('cube', 0,0)               # move the window to (0,0)
        
        x_start=20                                # top lef corner of the rectangle from where all the cube's faces are referring to
        y_start=20                                # top lef corner of the rectangle from where all the cube's faces are referring to
        d = 40                                     # edge lenght for each facelet reppresentation
        g = d//10                                 # gap between cube faces
        w = 2*x_start + 12*d + 3*g                # image width for the animation
        h = 2*y_start + 9*d  + 2*g                # image height for the animation
        sketch = np.zeros([h, w, 3],dtype=np.uint8)  # empty array (image dimension of h_a times w_a times 3 colors)
        sketch.fill(230)                          # array is filled with light gray
        
        # text is added to the sketch
        cv2.putText(sketch, 'TEST', (x_start+int(6.6*d), y_start+int(0.6*d)), font, fontScale*1.2,(0,0,0),lineType)
        
        facelets_start, inner_points = cube_sketch_coordinates(x_start, y_start, d, g)  # dict with the top-left coordinates for each of the 54 facelets
        for i in range(54):                       # iteration over the 54 facelets interpreted colors
            cv2.rectangle(sketch, tuple(facelets_start[i]), (facelets_start[i][0]+d, facelets_start[i][1]+d), (0, 0, 0), 1) # square black frame
        
        cv2.imshow("cube", sketch)                # sketch is plot to screen
        cv2.waitKey(500)                          # refresh time is limited
        frame = sketch.copy()                     # the sketch is copied to frame, which is the image being updated by colors
    
    
    ######### facelet filling with colors and test number #################    
    cv2.putText(frame, str(test), (x_start+int(6.6*d), y_start+int(1.6*d)), font, fontScale*1.2,(0,0,0),lineType)
    for i, color in enumerate(cube_status):       # iteration over the 54 facelets interpreted colors
        B,G,R = assigned_colors[color]            # BGR values of the assigned colors for the corresponding detected color
        cv2.fillPoly(frame, pts = [inner_points[i]], color=(B,G,R))  # inner square is colored with bright color of the interpreted one      
    
    cv2.imshow("cube", frame)                     # frame is plot to screen
    cv2.waitKey(wait)                             # refresh time
    
    if kill:                                      # case kill variable is set True
        cv2.destroyAllWindows()                   # all the windows are closed





def cube_solution(cube_string, printout, informative):
    """ Calls the Hegbert Kociemba solver, and returns the solution's moves
    from: https://github.com/hkociemba/RubiksCube-TwophaseSolver 
    (Solve Rubik's Cube in less than 20 moves on average with Python)
    The returned string is slightly manipulated to have the moves amount at the start."""
    
    simulation = True                             # simulation is set True (cube oriented as per URF)
    
    sv_max_moves = 20           # solver parameter: max 20 moves or best at timeout
    sv_max_time = 2             # solver parameter: timeout of 2 seconds, if not solution within max moves
    s = sv.solve(cube_string, sv_max_moves, sv_max_time)  # solver is called
    s = s[:s.find('(')]         # solution capture the sequence of manoeuvres
    
    # solution_text places the amount of moves first, and the solution (sequence of manoeuvres) afterward
    solution_Text = s[s.find('(')+1:s.find(')')-1]+' moves  '+ s[:s.find('(')] 
    if s[:5] =='Error':         # solution could start with "Error" in case of incoherent cusbe string sent to the solver
        solution_Text = 'Error'                   # in that case a short error string is returned
     
    
    _, robot_moves, total_moves, opt = rm.robot_required_moves(s, solution_Text, simulation=simulation, informative=informative)
    est_time = servo.estimate_time(robot_moves, timer, slow_time=0)   # estimated time for the robot moves in argument
    depth = len(s.replace(" ","")) // 2           # cube status depth
  
    if debug and solution_Text != 'Error':        # case debug variable is set True
        print("Estimated time for the servos:", est_time, "secs")  # feedback is printed to Terminal
    
    return s, solution_Text, robot_moves, total_moves, est_time, opt, depth







def solved_status_check(cube_status):
    """Checks is a cube status string is a solved cube.
        The test is performed on 5 faces, that if ok it also means the 6th face is ok.
        Funtion returns 1 for a solved cube, otherwise 0.
    """
    # facelets identification number by face: U R F D L B
    cube = ((0,1,2,3,4,5,6,7,8),(9,10,11,12,13,14,15,16,17),(18,19,20,21,22,23,24,25,26),\
            (27,28,29,30,31,32,33,34,35),(36,37,38,39,40,41,42,43,44))
    
    if len(cube_status) == 54:  # case the cube status is complete (54 facelets)
        for face in cube:       # iteration over the faces
            for i in range(8):  # iteration over 8 facelets
                
                # case the facelets at location i+1 differs from facelet at location 0 (ref)
                if cube_status[face[i+1]] != cube_status[face[0]]:
                    return 0   # returns zero (means cube is not solved)
        return 1               # returns 1 (means 5 faces are correct, therefore also the 6th one)
    else:
        return 0







def test_random_permutations(runs, cube_status, timer, plot, debug, printout):
    """Generates random cube status (permutations) of a Rubik's cube.
    Each random permutation is analysed from the Cubotino_T_moves.
    Each set of robot movements is virtually applied.
    After each of the (virtual) cube manipulation, the cube status is updated to reflect the new status.
    The last cube status is analyzed if conforming to the solved cube.
    The simulation can be limited to a predefined quantity of tests.
    """
    
    start = time.time()                           # initial time reference 
    
    if len(cube_status.strip().replace(" ","")) == 54: # case the script was launched with a cube status as argument
        random_status = False                     # random_status variable is set False
        runs = 1                                  # number of test is limited to one
    else:                                         # script not launched with a cube status as argument
        random_status = True                      # random_status variable is set True

    if plot:                                      # case plot variable is set True (sketch with the cube status on screen)
        if random_status:                         # case random_cube is set True (random cube status generation)
            t1 = 600                              # variable t1 in ms (plot time for initial and final cube status on the sketch)
            t2 = 50                               # variable t2 in ms(plot time for cube status while moving the cube)
        else:                                     # case random_cube is set False (passed a specific cube_status)
            t1 = 5000                             # variable t1 in ms (plot time for initial and final cube status on the sketch)
            t2 = 500                              # variable t2 in ms(plot time for cube status while moving the cube)
            
    test_ok_number, opt1, opt2 = 0,0,0            # initialized counters
    cube_depth, tot_robot_moves, estimated_time = [], [], []  # initialized empty lists to store values for stats
    
    
    for test in range(1,runs+1):                  # iteration over the test runs
        if runs>1:
            if not debug and not printout:            # case debug and printout are set False
                print(f"  Test: {test:,d} (of {runs:,d})  Errors: {test-1-test_ok_number:,d}", end='\r', flush=True) # feedback is write and overwrite to terminal
            if printout or debug:                     # case printout is set True                      
                print()                               # empty line separation
                print(row,"\n")                       # print a separation string
                print(f"Test number: {test:,d}")      # feedback is printed to terminal
            
        if random_status:                         # case random_cube is set True (random cube status generation)
            cube_status = scramble()              # a random cube_status is generated

        if printout:                              # case printout is set True
            if random_status:                     # case the cube status is random generated
                print("Random cube_status generated:", cube_status)  # feedback is printed to terminal
            else:                                 # case the cube status is entered by user
                print("Entered cube_status:", cube_status)  # feedback is printed to terminal
        
        a,b,c,d,e,f,g = cube_solution(cube_status, printout,informative=informative) # Kociemba solver is called to have the solution string
        solution, solution_Text, robot_moves, total_robot_moves, est_time, opt, depth = a,b,c,d,e,f,g
            
        if solution_Text != 'Error':              # case no errors returned by the Kociemba solver
            cube_depth.append(depth)              # cube_status depth is appended to cube_depth list
            tot_robot_moves.append(total_robot_moves) # quantity of robot movements is appended to tot_robot_moves
            estimated_time.append(est_time)       # the estimated time for this run is appended to estimated_time list
            opt1 += opt[0]                        # counter is incremented by the optimization type 1 that were applied
            opt2 += opt[1]                        # counter is incremented by the optimization type 3 that were applied
                
            if printout:                          # case printout is set True
                print('\nRobot_moves returned by the robot solver:', robot_moves)  # feedback is printed to terminal
                print("Estimated solving time:",est_time)  # feedback is printed to terminal
            
            if plot:                              # case plot variable is set True (sketch with the cube status on screen)
                if test == 1:                     # case is the test beginning
                    startup=True                  # startup variable is set True (preparing the fix graphical part for the cube sketch)
                show_ms = t1                      # sketch showing time as per t1
                plot_interpreted_colors(show_ms, cube_status, test, startup=True) # initial cube status is plot to the screen
            
            for i in range(0, len(robot_moves),2): # iteration over the robot movements
                if plot:                          # case plot variable is set True (sketch with the cube status on screen)
                    show_ms = t2                  # sketch showing time as per t2
                move_type = robot_moves[i:i+1]    # robot move type is retrieved from robot_moves string
                direction = robot_moves[i+1:i+2]  # robot move direction/repeats is retrieved from robot_moves string
                if move_type == 'F':              # case the robot move is F (flip)
                    for i in range(int(direction)):  # iteration over the quantity of flips
                        cube_status = cube_facelets_permutation(cube_status, move_type, direction)  # facelets permutation assigned to new_cube_status
                        if plot:                  # case plot variable is set True (sketch with the cube status on screen)
                            plot_interpreted_colors(show_ms, cube_status, test)  # the new cube status is plot to the screen
                else:                             # case the robot move is not F (not flip means spin or rotate)
                    if direction == '0':
                        cube_status = cube_facelets_permutation(cube_status, move_type, '1') # facelets permutation assigned to new_cube_status
                        cube_status = cube_facelets_permutation(cube_status, move_type, '1') # facelets permutation assigned to new_cube_status
                        if plot:                  # case plot variable is set True (sketch with the cube status on screen)
                            plot_interpreted_colors(show_ms, cube_status, test)  # the new cube status is plot to the screen
                    elif direction == '4':
                        cube_status = cube_facelets_permutation(cube_status, move_type, '3') # facelets permutation assigned to new_cube_status
                        cube_status = cube_facelets_permutation(cube_status, move_type, '3') # facelets permutation assigned to new_cube_status
                        if plot:                  # case plot variable is set True (sketch with the cube status on screen)
                            plot_interpreted_colors(show_ms, cube_status, test)  # the new cube status is plot to the screen
                    else:  
                        cube_status = cube_facelets_permutation(cube_status, move_type, direction) # facelets permutation assigned to new_cube_status
                        if plot:                  # case plot variable is set True (sketch with the cube status on screen)
                            plot_interpreted_colors(show_ms, cube_status, test)  # the new cube status is plot to the screen
            
            if plot:                              # case plot variable is set True (sketch with the cube status on screen)
                show_ms = t1                      # sketch showing time as per t1
                if test != runs:                  # case the current test is not the last one
                    plot_interpreted_colors(show_ms, cube_status, test)  # the final cube status is plot to the screen
                else:                             # case the current test is the last one  
                    plot_interpreted_colors(show_ms, cube_status, test, kill=True)  # the final cube status is plot to the screen, with kill instruction
                
                
            # test if the cube status reppresents a solved cube
            test_ok = solved_status_check(cube_status)  # solved_status_check returns 1 if ok
            test_ok_number += test_ok             # test_ok_counter is incremented by 1  
            
            if printout or debug:                 # case printout or debug is set True                      
                if test_ok:                       # case the final cube status resembles a solved cube
                    print("Virtual robot solver has solved the cube: OK")  # feedback is printed to terminal
                else:                             # case the final cube status does not resemble a solved cube
                    print("Virtual robot solver did not solve the cube: Test NOT OK")  # feedback is printed to terminal
        else:                                     # case of errors returned by the Kociemba solver
            test_ok_number=0                      # test_ok_number is set to False
    
    if solution_Text != 'Error':                  # case no errors returned by the Kociemba solver
        print()                                   # print empty line as separation
        print()                                   # print empty line as separation
        print(row)                                # print a separation line
        print(row)                                # print a separation line
        
        if runs>1:                                # case were tested more than one run
            print(f"Tested {runs} random cube's status with {runs-test_ok_number} failures")
        else:                                     # case was tested one run only
            if runs-test_ok_number == 0:          # case of no errors
                print(f"The random cube status was solved without any failure")
            else:                                 # case of errors
                print(f"The random cube status failed to be solved")
        
        tot_time = time.time()-start              # total analysis time is calculated
        
        if plot:                                  # case plot variable is set True (sketch with the cube status on screen) 
            print(f"The test took {round(tot_time,1)} seconds, being slowed down by the graphical animation")
        else:                                     # case plot variable is set False
            print(f"The test took {round(time.time()-start,2)} seconds")
        
        # printing out some little stats
        if runs>1:                                # case tested more than a single test (runs)
            cube_depth = np.array(cube_depth)     # an array is generated from the list of tested cube depths
            tot_robot_moves = np.array(tot_robot_moves)  # an array is generated from the list of tot_robot_moves
            estimated_time = np.array(estimated_time) # an array is generated from the list of estimated servos time
            avg_robot_moves = round(np.mean(tot_robot_moves),1)  # average of robot movements
            avg_servo_time = round(np.mean(estimated_time),1)  # average servos time
            std_servo_time = round(np.std(estimated_time),3)  # standard deviation of the average value
            min_servo_time = round(np.min(estimated_time),1)  # minimum servos time
            max_servo_time = round(np.max(estimated_time),1)  # maximum servos time
            
            print("Average time to analyze each cube_status:", round(1000*tot_time/runs,1), "ms")
            print()                               # print empty line as separation
            print("Average quantity of robot movements:", avg_robot_moves)
            print("Average estimated solving time (servos):", avg_servo_time)
            print("std on estimated solving time (servos):", std_servo_time)
            print("Min estimated solving time (servos):", min_servo_time)
            print("Max estimated solving time (servos):", max_servo_time)
            print()                               # print empty line as separation
            print()                               # print empty line as separation

        # printing out info related to the optimizations (cube solution, and robot moves)
        print("Optimization type 1 being used:", opt1)
        print("Optimization type 2 being used:", opt2)
        print()                                   # print empty line as separation
        
        # saving some data to a text file
        if runs>1:                                # case were tested more than one run
            datetime = dt.datetime.now().strftime('%Y%m%d_%H%M%S')  # date_time variable is assigned, for file name
            folder = pathlib.Path().resolve()     # active folder (should be home/pi/cubotino)
            fname = "TestRandom_" + str(runs) + "cubes_" + datetime + '.txt'  # filename constructor
            fname = os.path.join(folder, fname)   # folder+filename for the cube data
            np.savetxt(fname, np.c_[cube_depth, tot_robot_moves, estimated_time],
                       fmt='%10.1f', delimiter='\t', comments='',
                       header='Cube_depth\tTot_robot_moves\tEstimated_time',)   # data is saved to the text file
            print(f"Saved the cube depth and estimated servo time, of the {runs} runs")  # feedback is printed to terminal
            print(f"File saved at: {fname}")      # feedback is printed to terminal
            print()
            print()





####################################################################################################
####################################################################################################
if __name__ == "__main__":
    
    debug = False                    # flag to enable/disable the debug related prints
    if args.debug != None:           # case 'debug' argument exists
        if args.debug:               # case the script has been launched with 'debug' argument
            debug = True             # flag to enable/disable the debug related prints is set True

    printout = False                 # flag to enable/disable the prints related to the simulation prints
    if args.print != None:           # case 'debug' argument exists
        if args.print:               # case the script has been launched with 'printout' argument
            printout = True          # flag to enable/disable the simulation related prints is set True
    
    cube_status = ''                 # cubestatus is initially set as eempty string
    if args.status != None:          # case 'status' argument exists
        cube_status = args.status    # 'status' argument is assigned to cube_status
    
    plot = False                     # flag to enable/disable the graphical animation
    if args.plot != None:            # case 'plot' argument exists
        if args.plot:                # case the script has been launched with 'plot' argument
            plot = True              # flag to enable/disable the graphical animation
    
    runs = 100                       # arbitrary amount of simulation runs, when not overwritten by the argument
    if args.runs != None:            # case 'runs' argument exists
        runs = args.runs             # case the script has been launched with 'runs' argument

    if printout:                     # case printout is set True
        informative = True           # informative is set True (additional printout from the Cubotino_moves.py)
    else:                            # case printout is set False
        informative = False          # informative is set False (nodditional printout from the Cubotino_moves.py)
    
    
    clear_terminal()                 # cleares the terminal
    introduction()                   # prints to terminal the main info about this script
    imports(plot)                    # imports the needed libraries

    print("Loading Kociemba solver:")
    ret = import_solver()            # cube solver to get the solution per each permutation
    print("Solver imported:", ret)   # feedback is printed to the terminal
    print()
    
    timer = []                       # empty list to store the timer values
    timer = servo.load_servos_parameters(False)  # servos timers, necessary for searching the fastest robot solution
    if len(timer)>0:                 # case the timer list is not empty
        ret=True                     # return variable is set True
    else:                            # case the timer list is empty
        ret=False                    # return variable is set False
    print("Timers returned by the robot:", ret)  # feedback is prionted to terminal
    
    if runs > 20000:                 # case runs is too big (data is tored in list and later saved to a text file)
        runs = 20000                 # runs is limited to 20000
        print("Test limited to 20K runs")  # feedback is printed
    
    row = "#"*95                     # string of characters used as separator
    print(row,'\n')                  # a row separation is printed to the terminal
    
    # command to generate, and solve, random cube status
    test_random_permutations(runs, cube_status, timer, plot, debug, printout)

####################################################################################################
####################################################################################################
