import copy

'''
Generates the G-code to drive a CNC machine to render the solution
to a Sudoku puzzle.

It is IMPORTANT to note that the G-code generated assumes that
the proper state for execution of the code has already been
set (mm, absolute, XY), and that the workspace origin has
been established.
'''
'''
The workspace origin for the CNC machine is the lower left corner of the
Puzzle Grid; the PG is 129x129 mm. The Number Pad lower left is @
(139, 1) and is 53x41 mm.

The PG cell size is 129/9 x 129/9 (~14.33x14.33). The centroid of a
cell in the PG is @ (for both coordinates)
    PGC = origin of cell + centroid offset
        = (129/9 * PGc) + ((129/9) / 2) =  (129/9 * PGc) + (129/18)

    PGCx = (129/9 * PGcx) + (129/18)
    PGCy = (129/9 * PGcy) + (129/18)
        where 'cx' is the PC cell x coordinate and
              'cy' is the PC cell y coordinate

The NP cell size is 53/3 x 41/3 (~17.67x13.67). The centroid for a
number in the NP is
    NPCx = origin of number + centroid offset
         = 139 + (53/3 * NPnx) + ((53/3) / 2) = (53/3 * NPnx) + (139 + 53/6)

    NPCy = origin of number + centroid offset
         = 1 + (41/3 * NPny) + ((41/3) / 2) = (41/3 * NPny) + (1 + 41/6)
        where 'nx' is the NP number x coordinate and
              'ny' is the NP number y coordinate

The constants above will be injected into a dictionary for the
PG cell coordinates, mapping the empty cell coordinate tuple into
the centroid coordinates for that cell. Another dictionary maps the
NP number into the centroid coordinates for that number.
'''
class GenerateGcode:

    PG_CELL_SIZE = 129 / 9
    PG_CENTROID_OFFSET = 129 / 18

    NP_CELL_SIZE_X = 53 / 3
    NP_CELL_SIZE_Y = 41 / 3
    NP_CENTROID_OFFSET_X = 139 + 53 / 6
    NP_CENTROID_OFFSET_Y = 1 + 41 / 6

    def __init__(self, z_down_loc=0, z_up_loc=2, z_feed_speed=200, z_up_feed=True):
        """
        Initializes the class based on the geometry of the iPad. The
        parameters define the Z-axis behavior.
        :param z_down_loc: The Z down location in mm; default=0
        :param z_up_loc: The Z up location in mm; default=2
        :param z_feed_speed: The Z feed speed in mm/min; default=200
        :param z_up_feed: If Z up movement at feed speed, not fast; default=True
        """
        # stringify parameters
        self.z_down_loc = str(z_down_loc)
        self.z_up_loc = str(z_up_loc)
        self.z_feed_speed = str(z_feed_speed)
        self.z_up_feed = z_up_feed
 
        '''
        Configuration commands
        '''
        # perform homing
        self.go_home = '$H'
        # set execution state: use XY plane, use mm coordinates, use absolute positioning
        self.execution_state_set = 'G17 G21 G90'
        # set workspace origin
        self.work_origin_set = 'G92 X0 Y0 Z0'
        # set the machine position
        self.machine_pos_set = 'G10 P0 L20 X0 Y0 Z0'
        # set feed speed
        self.z_feed_speed_set = 'F' + self.z_feed_speed
        # move to work origin
        self.fix_work_origin_1 = 'G00 X50'
        self.fix_work_origin_2 = 'G00 Y-153 Z-20'
        self.fix_work_origin_3 = 'G01 Z-25'

        # set up some instance variables
        self.puzzle = None
        self.puzzle_copied = False

        # create the PGC dictionary       
        self.pgc_dict: dict[tuple, tuple] = {} 
        for i in range(9):
            for j in range(9):
                y = self.PG_CELL_SIZE * (8 - i) + self.PG_CENTROID_OFFSET
                x = self.PG_CELL_SIZE * j + self.PG_CENTROID_OFFSET
                self.pgc_dict[(i, j)] = (round(y, 2), round(x, 2))

        # create NPC dictionary, transposing coordinate in Y
        self.npc_dict: dict[int, tuple] = {}
        self.npc_dict[1] = (2, 0)
        self.npc_dict[2] = (2, 1)
        self.npc_dict[3] = (2, 2)
        self.npc_dict[4] = (1, 0)
        self.npc_dict[5] = (1, 1)
        self.npc_dict[6] = (1, 2)
        self.npc_dict[7] = (0, 0)
        self.npc_dict[8] = (0, 1)
        self.npc_dict[9] = (0, 2)

        # apply the geometric constants to dictionary
        for i in range(1, 10):
            y, x = self.npc_dict[i]
            # (41 / 3 * NPny) + (1 + 41 / 6)
            y = (self.NP_CELL_SIZE_Y * y) + self.NP_CENTROID_OFFSET_Y
            x = (self.NP_CELL_SIZE_X * x) + self.NP_CENTROID_OFFSET_X
            self.npc_dict[i] = (round(y, 2), round(x, 2))

    def copy_puzzle(self, puzzle_string):
        """
        The Mac represents the original puzzle with a byte string for
        transmission to the Pi. The Sudoku solver wants a 9x9 array.
        So, this method creates that array, a.k.a. the grid.
        Since the Sudoku solver describes the solution in the original
        puzzle, the G-code generator needs a copy of the original,
        and this method makes that copy.
        :param puzzle_string: defines the Sudoko puzzle
        :return: a 2D list representing the puzzle
        """
        # decode the puzzle string 
        char_string = puzzle_string.decode("utf-8")

        # put string in 9x9 grid
        rows, cols = (9, 9)
        grid = [[0 for i in range(cols)] for j in range(rows)]

        k = 0
        for i in range(9):
            for j in range(9):
                asr = int(char_string[k])
                grid[i][j] = int(asr)
                k += 1

        self.puzzle = copy.deepcopy(grid)
        self.puzzle_copied = True

        return grid

    def generate_solution_gcode(self, solution):
        """
        Generates the G-code to drive a CNC machine to solve a
        Sudoku puzzle based on the solution provided.

        It uses the unsolved puzzle to assist, so the method
        copy_puzzle must be called prior to solving the puzzle
        and calling this method. If the copy does not exist,
        returns an empty list; if copy exists, returns an 2D list of
        G-code strings.
        :param solution: a 2D list defining the solution
        :return: a list of G-code command strings
        """
        if not self.puzzle_copied:
            print("ERROR: Puzzle NOT copied!")
            return []
        else:
            # assume single use
            self.puzzle_copied = False
            # place the solution in a grid
            the_grid = [[]]
            for row in solution:
                the_grid = row

            # self._print_grid(the_grid)
            # self._print_what_todo(the_grid)

            '''
            Create the G-code for solving the puzzle
            '''
            g_code = []
            '''
            The following pattern repeats
            G00 move, with stylus up, to the puzzle cell centroid
            G01 stylus down
            G00 stylus up
            G00 move, with stylus up, to the number pad centroid
            G01 stylus down
            G00 stylus up
            '''
            for i in range(len(self.puzzle)):
                for j in range(len(self.puzzle[i])):
                    if self.puzzle[i][j] == 0:
                        # move to cell centroid
                        self.move_to_cell(g_code, (i, j))
                        # stylus down/up
                        self.tap(g_code)
                        # move to number centroid
                        self.move_to_num(g_code, the_grid[i][j])
                        # stylus down/up
                        self.tap(g_code)

            return g_code

    def generate_new_game_gcode(self):
        """
        Generates the G-code to drive a CNC machine to start a
        Sudoku puzzle by poking the New Game button.
        :return: a list of G-code command strings
        """
        g_code = []
        '''
        G00 move, with stylus up, to the button centroid
        G01 stylus down
        G00 stylus up
        '''
        
        # move to button centroid
        g_code.append('G00 X98 Y12.5')

        # stylus down/up
        self.tap(g_code)

        return g_code

    def move_to_cell(self, g_code, coord: tuple):
        """
        Generates the G-code for moving to a puzzle cell. Assumes
        absolute coordinates. Appends that G-code to the
        list indicated by the g_code parameter.
        :param g_code: the list of G-code commands
        :param coord: the (y,x) position of a cell (origin top left)
        """
        # get coordinates of cell from dictionary
        cell_centroid = self.pgc_dict[coord]
        # move to cell centroid
        g_code.append("G00 X" + str(cell_centroid[1]) + " Y" + str(cell_centroid[0]))
        
    def move_to_num(self, g_code, num: int):
        """
        Generates the G-code for moving to a pad number. Assumes
        absolute coordinates. Appends that G-code to the
        list indicated by the g_code parameter.
        :param g_code: the list of G-code commands
        :param num: the number in the number pad
        """
        # get coordinates of number from dictionary
        number_centroid = self.npc_dict[num]
        # move to number centroid
        g_code.append("G00 X" + str(number_centroid[1]) + " Y" + str(number_centroid[0]))

    def tap(self, g_code):
        """
        Generates the G-code for moving the stylus down,
        to touch the iPad and then up. Assumes
        absolute coordinates. Appends that G-code to the
        list indicated by the g_code parameter.
        :param g_code: the list of G-code commands
        """
        # stylus down at feed speed
        g_code.append("G01 Z" + self.z_down_loc)
        # stylus up at desired speed
        up_code = ''
        if self.z_up_feed:
            up_code = 'G01 Z'
        else:        
            up_code = 'G00 Z'
        g_code.append(up_code + self.z_up_loc)
        

    @staticmethod
    def _print_grid(grid):
        """
        Prints the grid.
        :param grid: is the 2D list (grid) to print
        """
        print()

        for i in range(len(grid)):
            print(grid[i])

    def _print_what_todo(self, the_grid):
        """
        Prints what should be done.
        :param: grid is the 2D list (grid) to print
        """
        print()
        # print what need to do
        for i in range(len(self.puzzle)):
            for j in range(len(self.puzzle[i])):
                if self.puzzle[i][j] == 0:
                    print("empty cell at [", i, "][", j, "]; solution = ", the_grid[i][j])

                    # get coordinates of cell and number centroids from dictionaries
                    cell_centroid = self.pgc_dict[(i,j)]
                    number_centroid = self.npc_dict[the_grid[i][j]]

                    print("cell centroid ", cell_centroid, " number centroid ", number_centroid)


'''
For testing.
'''
def main():
    from solve_sudoku import solve_sudoku

    puzzle_str = b'008009460700564038600138200879002306034607520050003874421000050087300000060200701'

    gen_c = GenerateGcode()

    grid = gen_c.copy_puzzle(puzzle_str)

    solution = solve_sudoku((3, 3), grid)

    g_code = gen_c.generate_solution_gcode(solution)

    if not g_code:
        print('Call copy_puzzle first, you twit!')
    else:
        # print the G-code
        for i in range(len(g_code)):
            print(g_code[i])

if __name__ == '__main__':
    main()