# from picamera2 import Picamera2
import time
from cnc_machine import CncMachine
import imagezmq
from generate_gcode import GenerateGcode
from solve_sudoku import solve_sudoku
import numpy as np
from sudoku_camera import SCamera

#################################
#   MAIN
#################################

'''
Orchestrates looking at a Sudoku puzzle on an iPad, solving the puzzle,
and rendering of the solution using a CNC machine.
'''

def send_gcode_to_cnc(cnc, g_code):
    """
    Sends G-code to a CNC machine.
    :param cnc: the CNC machine
    :param g_code: a G-code list
    """
    for i in range(len(g_code)):
        cmd = g_code[i]
        if not cnc.send_cmd_check_status(cmd):
            print('G-code error of some sort at line ', i+1)
            
def position_for_photo(cnc):
    """
    Move to position the iPad for a photo of the puzzle
    :param cnc: the CNC machine
    """
    # send G-code to move to the position
    try: 
        cnc.send_cmd_check_status('G00 Z10', True) # move stylus up first
        cnc.send_cmd_check_status('G00 X-30 Y60', True) 
    except Exception as ex:
        print('Some problem with G-code or machine itself')
        print(ex)
        exit(33)
        
    # wait for machine to reach the desired position
    cnc.wait_for_idle()
            
def is_game_state_new(cnc, camera):
    """
    Determines if the game shows the New Game screen or
    a puzzle.
    :param cnc: the CNC machine
    :param camera: the camera
    :return: True if New Game, False if puzzle
    """
    # move to position the iPad for a photo of the game
    position_for_photo(cnc)

    # take photo
    puzzle_photo = camera.take_photo(False, False)

    # calculate the average color
    average_color = np.mean(puzzle_photo, axis=(0, 1))

    # calculate the ratios of green and red to blue
    green_blue_ratio = average_color[1]/average_color[0]
    red_blue_ratio = average_color[2] / average_color[0]

    # if New Game, return True
    if red_blue_ratio > 0.6 and green_blue_ratio > 0.6:  # threshold empirically determined
        return False
    else:
        return True

#=================   MAIN =======================        
'''
Ask human to confirm necessary initial pre-conditions.
'''
print("\n\nPlease confirm the following:")
print("    * The iPad is clamped to the CNC")
print("    * The camera is aligned properly to the iPad ")
print("    * Nothing (cables, etc.) will interfere with CNC movement")
print("    * The Mac puzzle server is started")
print("    * The CNC machine is plugged in, turned on, and connected to the Pi")
print("    * The New Game button is visible, or an unsolved puzzle is visible\n")
input("Move away from the CNC and press ENTER to confirm")

'''
Initialize the camera, serial communication with the CNC, the CNC itself,
and network communication with the Mac server that interprets the 
unsolved puzzle, the G-code generator.
'''
# initialize communication with the Mac
sender = imagezmq.ImageSender(connect_to='tcp://192.168.1.144:5555')
# due to an imagezmq restriction, no way to know if server
# connection successful without trying it; if not alive, then
# must kill this script -- sadly
print('Checking if Image Server alive ...')
print('If no confirmation message, kill program,\nstart image server, and try again.')
# create fake image
fake_image = np.zeros((3,3,3), dtype=np.uint8)
# send fake image
puzzle_str = sender.send_image('test', fake_image)
print('Puzzle Server alive!')

# initialize CNC machine communication
cnc = None
try:
    cnc = CncMachine()
except Exception as ex:
    print(ex)
    print('Since no CNC machine available, ending program')
    exit(99)

# initialize G-code generator
# the parameters mean a poke is Z -1.7 mm down, and 1.5 mm up, down at
# a feed rate of 200; Z up speed=fast
gcode_generator = GenerateGcode(-1.7, 1.5, 200, False)

# initialize camera to take full resolution photos
camera = SCamera()
print("Camera initialized")

# configure CNC machine for rendering
cnc.configure(gcode_generator)
try:
    print('CNC configured')
except Exception as ex:
    print(ex)
    print('Some problem with G-code or machine itself while initializing')
    exit(55)

'''
System initialized and ready to solve a puzzle
'''
print('System initialized')

'''
Determine whether New game or puzzle visible
'''
new_game = is_game_state_new(cnc, camera)
if new_game:
    print('Found the New game screen')
else:
    print('Found puzzle screen')

'''
Loop playing until tired or bored
'''
while True:
    # prompt to start a game
    command = input('\nStart a New puzzle? YES: type Enter; NO: type anything and Enter: ')
    if not command == '':
        break
    else:
        '''
        At this point the stylus is positioned for a photo
        '''
        print("Working!!!")
        start= time.time()

        '''
        If New game, hit button to start
        '''
        if new_game: # already have a puzzle screen
            # generate the G-code to hit the button
            g_code = gcode_generator.generate_new_game_gcode()

            # send the G-code to the CNC
            send_gcode_to_cnc(cnc, g_code)

            # move to position the iPad for a photo of the puzzle
            position_for_photo(cnc)
        
        '''
        Loop to solve the puzzle
        '''
        solved = False
        while not solved:
            '''
            Take photo and crop it. Send the photo to the Mac and receive the 
            puzzle.
            '''
            # take photo
            puzzle_photo = camera.take_photo(True, False)

            # send the photo to the puzzle server and get the puzzle
            puzzle_str = sender.send_image('image', puzzle_photo)
     
            '''
            Solve the puzzle and generate the G-code; must copy puzzle
            before getting the solution.
            '''
            puzzle_grid = gcode_generator.copy_puzzle(puzzle_str)
            solution = solve_sudoku((3, 3), puzzle_grid)
            g_code = gcode_generator.generate_solution_gcode(solution)

            # send the G-code to the CNC to render the solution
            send_gcode_to_cnc(cnc, g_code)
          
            '''
            Determine if really solved
            '''
            solved = is_game_state_new(cnc, camera)

        # end single puzzle solve loop
        
        # indicate that will now see the New Game screen
        new_game = True 

        # calculate time to solve
        end = time.time()
        total_time_sec = end - start
        total_time_min = int(total_time_sec / 60)
        total_time_sec = total_time_sec - (total_time_min * 60)
        
        print("Total time: ", total_time_min, ":", int(total_time_sec), sep='')
        
        pass

# end major while loop (multiple games)

# move so that iPad can be charged
if not cnc.send_cmd_check_status('G00 Y100'):
    print('G-code error of some sort on send to Y=100')

'''
Close the system components
'''
# close the puzzle server
sender.send_image('end', fake_image)
# close imagezmq
sender.close()
# close CNC
cnc.close()
# close camera
camera.close()

print("\nFinished!")