#!/usr/bin/env python
import rospy
from std_msgs.msg import String, Bool

from include.game_lib import Player, HUMAN, COMPUTER
import include.yaml_data_handling as yaml_handle
import include.io_control_lib as io

# Debug mode, when True, show a lot of useful messages
DEBUG = rospy.get_param("robocheckers/debug")

# Defining human move identifiers
MOVE = 1
JUMP = 2
MULTI_JUMP = 3

# Definining info panel LED masks
RED = 0b001
WHITE = 0b010
GREEN = 0b100


class Controller:
    def __init__(self):
        # Node initialization
        rospy.init_node('game_interface')
        self.coords_pub = rospy.Publisher('/robocheckers/coordinates', String, queue_size=10)
        self.human_move_pub = rospy.Publisher('/robocheckers/human_turn', String, queue_size=10)
        self.io_control = rospy.Publisher('/robocheckers/io_message', String, queue_size=10)
        rospy.Subscriber('/robocheckers/pieces', String, self.receive_players)
        rospy.Subscriber('/robocheckers/ai_move', String, self.receive_ai_move)
        rospy.Subscriber('/robocheckers/robot_busy', Bool, self.receive_robot_busy)

        # Defining Player instancies in global
        self.Human = Player(HUMAN)
        self.Computer = Player(COMPUTER)
        self.Human_tmp = Player(HUMAN)
        self.Computer_tmp = Player(COMPUTER)

        # Defining Human player pieces color
        self.human_color = "B"
        self.computer_color = "O"

        # Defining tuple and list for data from game logic node
        self.ai_move = []
        self.options = []

        # Defining boolean flag to determine, if robot is moving, True = moving
        self.robot_busy = False


    def send_robot_position(self, move):
        msg = ''
        for i in range(0, len(move), 2):
            piece, dest = move[i], move[i+1]
            data = []
            for pos in [piece, dest]:
                if isinstance(pos, str):
                    data.append(pos)
                else:
                    data += pos

            if i != 0:
                msg += ','

            # Mozna tento radek posunout o tab doleva
            msg += ','.join(str(e) for e in data)

        self.coords_pub.publish(msg)


    def send_human_move(self, move):
        if isinstance(move, tuple):
            move = [move]

        self.human_move_pub.publish(str(move))


    def receive_players(self, data):
        # Defining player pieces list and word string
        player = []
        word = ""
        # Add coma to the end of data string to save last value
        data.data += ','

        # Converting data separated by coma from string to list
        for char in data.data:
            # If coma detected, append word to player list
            if char == ',':
                # Try adding it as float, if not possilbe, add it as char
                try:
                    player.append(float(word))
                except ValueError:
                    player.append(word)
                    # Clear the word string
                word = ""
            # If coma not detected, just add the char to word string
            else:
                word += char

        # Clearing old pieces data and assigning new pieces to players
        if player[0] == self.human_color:
            self.Human_tmp.empty()
            self.Human_tmp.fill(player)
        elif player[0] == self.computer_color:
            self.Computer_tmp.empty()
            self.Computer_tmp.fill(player)
        else:
            print("Game Logic ERROR: Invalid player selection!")


    def receive_ai_move(self, data):
        divide = data.data.find("|")
        ai_move = eval(data.data[:divide])
        if DEBUG: print "This just came in!", ai_move
        if ai_move:
            if ai_move not in self.ai_move:
                self.ai_move.append(ai_move)
        if DEBUG: print "Whole AI turn", self.ai_move

        self.options = eval(data.data[divide+1:])
        if DEBUG: print "Human options:", self.options


    def receive_robot_busy(self, data):
        self.robot_busy = data.data

        if self.robot_busy:
            if DEBUG: print "Manipulator is moving! TAKE COVER!"
        else:
            if DEBUG: print "Manipulator is not moving, chill out, dude."


    def wait_for_game_logic(self):
        """
            Waits for data from game_logic node and then returns
        """
        while not self.options and not self.ai_move:
            continue
        rospy.sleep(0.001)
        return


    def get_ai_move(self):
        """
            Gets single ai_move from self.ai_move buffer and returns it
        """
        # Save ai_move values and clear their original variable
        ai_move = self.ai_move.pop(0)

        return ai_move


    def get_options(self):
        """
            Gets options from self.options buffer, clear the buffer, and returns them
        """
        # Save options values and clear their original variable
        options, self.options = self.options, []

        return options


    def wait_for_robot(self):
        # Wait few miliseconds for robot_move node to send the busy signal
        rospy.sleep(0.1)
        # Wait till the busy signal is OK
        while self.robot_busy:
            continue


    def get_real_pos(self, piece, dest):
        if isinstance(piece, tuple):
            # Find the real location of the selected piece
            pieces = self.Computer.pieces + self.Human.pieces
            for item in pieces:
                if piece == item.square:
                    item.remap_to_real_world()
                    piece = item.real_pos
                    break
                
                # If at last item and piece still not found
                if item == pieces[-1]:
                    if DEBUG: print "All pieces squares:\n"
                    for tmp in pieces:
                        print tmp.square
                    print "ERROR: No piece with the square value {} found!".format(piece)
                    return None

        elif isinstance(piece, str):
            if piece != "promote":
                return None

        else:
            return None

        if isinstance(dest, tuple):
            # Load the real world dimension a position of the chessboard
            dimensions = yaml_handle.load_dimensions_data(yaml_handle.REAL_MM)
            square = dimensions.get('chessboard')/8
            board_pos_x = dimensions.get('pos_x_max')
            board_pos_y = dimensions.get('pos_y_max')

            dest = list(dest)
            dest.reverse()
            dest[0] = board_pos_x - ((dest[0]-1) * square + square/2)
            dest[1] = board_pos_y - ((dest[1]-1) * square + square/2)

            return [piece, tuple(dest)]

        elif isinstance(dest, str):
            if dest == "drop":
                return [piece, dest]

        else:
            return None


    def capture_piece(self, move):
        piece, dest = move
        # Calculate distance, it should be 1 for normal move and 2 for jump
        distance = abs(float(piece[0] - dest[0]))/2 + abs(float(piece[1] - dest[1]))/2

        if distance == 1:
            # Normal move, not a jump
            return None

        if distance == 2:
            # This is a jump
            captured = ((piece[0] + dest[0])/2, (piece[1] + dest[1])/2)
            if DEBUG: print "Captured human piece:", captured
            return captured

        # This is error state, this shouldnt happen
        print "ERROR: Invalid move!"
        return None


    def check_promotion(self, move):
        # Get rank of the piece
        rank = ""
        for piece in self.Computer.pieces:
            if piece.square == move[0]:
                rank = piece.rank

        # If the piece is not pawn, we wont promote it
        if rank != "pawn":
            if DEBUG: print "Promote: Piece is not a pawn! Piece rank:", rank
            return None

        # If the destination is not in last line, we wont promote it
        if move[1][1] != 1:
            if DEBUG: print "Promote: Piece destination is not last line!"
            return None

        # If piece is pawn and destination is last line, promote it
        if rank == "pawn" and move[1][1] == 1:
            return self.get_real_pos(move[0], "drop") + self.get_real_pos("promote", move[1])

        # If it goes here, something is not quite right
        if DEBUG: print "Promote: Something is not quite right!", rank, move[1][1]
        return None


    def get_board_changes(self, player):
        '''
            Returns list of missing pieces and new pieces from old - new board situation comparison
        '''
        # Prepare instance of Player class and old_pieces list
        new_pieces = Player(player)
        old_pieces = Player(player)

        # Load new piece data and calculate squares
        if player == HUMAN:
            new_pieces.pieces = self.Human_tmp.pieces
            old_pieces.pieces = self.Human.pieces

        elif player == COMPUTER:
            new_pieces.pieces = self.Computer_tmp.pieces
            old_pieces.pieces = self.Computer.pieces

        else:
            return [], [], []

        # Calculate square for new pieces
        new_pieces.get_squares()

        # Get squares in new_squares and kings in new_kings lists
        new_squares = []
        new_kings = []
        for piece in new_pieces.pieces:
            new_squares.append(piece.square)
            if piece.rank == "king":
                new_kings.append(piece.square)

        # Get squares in old_squares and kings in old_kings lists
        old_squares = []
        old_kings = []
        for piece in old_pieces.pieces:
            old_squares.append(piece.square)
            if piece.rank == "king":
                old_kings.append(piece.square)

        # Get lists of added and miggins pieces
        missing = list(set(old_squares) - set(new_squares))
        added = list(set(new_squares) - set(old_squares))

        # Get lists of added and miggins kings
        missing_kings = list(set(old_kings) - set(new_kings))
        added_kings = list(set(new_kings) - set(old_kings))

        # If you are looking for a move
        if len(missing) == len(added) == 1:
            # If move with king executed, remove them
            if missing[0] in missing_kings:
                missing_kings.remove(missing[0])
                if added[0] in added_kings and added[0][1] != 8:
                    added_kings.remove(added[0])
            
            return missing + added, missing_kings, added_kings

        # Remove missing kings if the whole piece is missing
        for king in missing_kings:
            if king in missing:
                missing_kings.remove(king)
        
        # If you are looking for captured pieces
        if missing and not added:
            return missing, missing_kings, added_kings

        # If have no move or more than one move
        if not missing and not added:
            print "ERROR: Invalid move! No move executed."
        elif len(missing) == len(added):
            print "ERROR: Invalid move! More than one move executed."
        else:
            print "ERROR: Invalid move! You did something horribly wrong, you little sneaky cheater, return it. Now!"

        # If you are looking for something illegal
        return [], missing_kings, added_kings


    def get_human_midjumps(self, move, cpu_changes):
        '''
            Method for checking if the jumps are legal and if no computer pieces were illegaly
            removed
        '''
        if not move:
            return False, None

        # Get starting and ending position of the human move
        piece, dest = move
        distances = abs(piece[0]-dest[0]), abs(piece[1] - dest[1])

        if DEBUG: print "Captured computer pieces:", cpu_changes

        # Normal move
        if distances[0] == distances[1] == 1 and not cpu_changes:
            return MOVE, None

        dist = max(distances)

        # Only normal jump with no midjump positions occured
        if dist == 2 and len(cpu_changes) == 1:
            # Prepare jump list and load the only change as missing piece
            jump = [piece]
            missing = cpu_changes[0]

            changes_dists = (abs(piece[0] - missing[0]), abs(piece[1] - missing[1]))

            # If computer piece is not adjecent, return error
            if changes_dists != (1, 1):
                return False, None

            new_pos = (2*missing[0] - piece[0], 2*missing[1] - piece[1])

            # Check if calculated final pos is the same as the original
            if dest != new_pos:
                return False, None

            jump.append(new_pos)

            return JUMP, jump

        # Multi jump move with some midjump positions occured
        if dist % 2 == 0 and cpu_changes:
            if DEBUG: print "============ MULTIJUMP DEBUG INFO ============"
            # Prepare midjumps list
            midjumps = [piece]

            while cpu_changes:
                changes_dists = []
                for missing in cpu_changes:
                    changes_dists.append((abs(piece[0] - missing[0]), abs(piece[1] - missing[1])))

                if DEBUG: print "Distances:", changes_dists

                # Find the closest computer piece
                closest = min(changes_dists)

                if DEBUG: print "Closest distance:", closest

                # If next computer piece is not adjecent, return error
                if closest != (1, 1):
                    return False, None

                closest = changes_dists.index(closest)
                closest = (cpu_changes.pop(closest))

                midjump = (2*closest[0] - piece[0], 2*closest[1] - piece[1])

                if DEBUG: print "Midjump:", midjump

                midjumps.append(midjump)

                piece = midjumps[-1]

            if DEBUG: print "Midjumps:", midjumps, "\nChosen dest:", dest, "\nCalculated dest:", piece

            # Check if calculated final pos is the same as the original
            if dest != piece:
                return False, None

            if DEBUG: print "========== MULTIJUMP DEBUG INFO END =========="
            return MULTI_JUMP, midjumps

        # If anything else, error state, this should never happen
        return False, None

    # Function to operate the LED info panel
    def info_panel(self, color, state):
        if state not in [io.OFF, io.ON, io.BLINK]:
            print "IO ERROR: Invalid LED state"
            return False

        msg = []
        if color & 0b001:
            msg.append(io.get_msg(io.DOUT, 117, state))
        if color & 0b010:
            msg.append(io.get_msg(io.DOUT, 118, state))
        if color & 0b100:
            msg.append(io.get_msg(io.DOUT, 119, state))

        for string in msg:
            self.io_control.publish(string)


    # Function to determine if the received data are move or special message
    def check_ai_move(self, ai_move):
        # Check if coords are in boundaries
        if len(ai_move) == 2:
            coords = [-1, -1, -1, -1]
            [(coords[0], coords[1]), (coords[2], coords[3])] = ai_move

            for pos in coords:
                if pos not in range(1, 9):
                    self.info_panel(RED, io.ON)

            return False

        # Case of winning message
        elif len(ai_move) == 1:
            self.info_panel(RED + WHITE + GREEN, io.OFF)
            [(active, order)] = ai_move

            if (active + order)%2 == 1:
                print "Congratulation, Human, you have won!"
                self.info_panel(GREEN, io.BLINK)
            else:
                print "Dont worry, Human, you will win next time!"
                self.info_panel(WHITE, io.BLINK)

        # Other message is Error
        else:
            self.info_panel(RED, io.ON)

        return True


    def interface(self):
        # Start Computer move and wait for game_logic data
        false_move = False
        self.info_panel(WHITE, io.ON)
        self.wait_for_game_logic()

        if (-1, -1) in self.ai_move:
            self.ai_move.remove((-1, -1))
            rospy.sleep(1)
            false_move = True

        # Execute every buffered movement from game_logic
        while self.ai_move:
            # Here will be command to turn on the white light

            # Get Computer data from tmp var and calculate real world positions and squares for them
            self.Computer.pieces = self.Computer_tmp.pieces
            self.Computer.get_squares()

            # Load AI move
            ai_move = self.get_ai_move()

            # If AI move bad, or win occured, end program
            if self.check_ai_move(ai_move):
                return True

            print "AI move:", ai_move

            # Do as the AI guides
            if ai_move:
                # Calculate if there are any pieces jumped over
                captured_piece = self.capture_piece(ai_move)

                # Check for promotion
                promotion = self.check_promotion(ai_move)

                if promotion:
                    ai_move = promotion
                else:
                    ai_move = self.get_real_pos(ai_move[0], ai_move[1])

                if captured_piece:
                    self.Human.pieces = self.Human_tmp.pieces
                    self.Human.get_squares()
                    captured_pos = self.get_real_pos(captured_piece, "drop")
                    ai_move += captured_pos

                    if DEBUG: print "Captured move:", ai_move

                # Move the manipulator
                self.send_robot_position(ai_move)
                self.wait_for_robot()

        # Load Human player options
        options = self.get_options()

        if not false_move:
            # Save current Human pieces data as old for further comparison
            self.Human.pieces = self.Human_tmp.pieces
            self.Human.get_squares()

            # Save current Computer pieces data as old for further comparison
            self.Computer.pieces = self.Computer_tmp.pieces
            self.Computer.get_squares()

        # End Computer turn and start Human turn
        self.info_panel(RED + WHITE, io.OFF)
        self.info_panel(GREEN, io.ON)

        # Human move, wait for it
        while True:
            # Get Human move
            raw_input("Execute a move and press `Enter`.")
            human_turn, man_missing_kings, man_added_kings = self.get_board_changes(HUMAN)
            cpu_changes, cpu_missing_kings, cpu_added_kings = self.get_board_changes(COMPUTER)

            # Check if human added and removed only one of his pieces
            if len(human_turn) != 2:
                if len(human_turn) < 2:
                    print "ERROR: No move detected!"
                if len(human_turn) > 2:
                    print "ERROR: Multiple moves detected!"
                self.info_panel(RED, io.BLINK)
                continue

            # Check the kings
            if check_kings(human_turn, man_missing_kings, man_added_kings,
                           cpu_missing_kings, cpu_added_kings):
                self.info_panel(RED, io.BLINK)
                continue

            if DEBUG: print "Human move:", human_turn

            # Check for multiple jumps
            move_type, jumps = self.get_human_midjumps(human_turn, cpu_changes)

            if not move_type:
                print "ERROR: Captured and removed computer pieces are not the same!"
                self.info_panel(RED, io.BLINK)
                continue

            # Print what move human player just performed
            if DEBUG: print "Human player performed move ({}):".format(move_type)
            if move_type == MOVE:
                print "Regular move"
            elif move_type == JUMP:
                print "Regular jump"
                human_turn = jumps
            elif move_type == MULTI_JUMP:
                print "Multiple jump"
                human_turn = jumps

            # Check if executed move is valid
            if human_turn[:2] in options:
                self.send_human_move(human_turn)
                # Turn off green and red lights
                self.info_panel(GREEN + RED, io.OFF)
                break
            else:
                # Indicate invalidity of the Human turn
                self.info_panel(RED, io.BLINK)
                print "Move is illegal!"


def check_kings(move, man_missing, man_added, cpu_missing, cpu_added):
    # Create lists with all missing and added kings
    missing = man_missing + cpu_missing
    added = man_added + cpu_added

    # Check if there are any illegally missing king pieces
    if missing:
        print ("ERROR: You cannot just randomly remove king pieces! Return the illegaly " +
               "removed king(s) at position(s) {} back, please.".format(missing))
        return True

    # Check if there are any illegally added king pieces
    if len(man_added) > 1 or cpu_added:
        print ("ERROR: You cannot just randomly add king pieces! Return the illegaly " +
               "added king(s) at position(s) {} back, please.".format(added))
        return True

    # Check if there are more than one added piece by human
    if len(man_added) == 1:
        if man_added[0][1] != 8:
            print ("ERROR: You can promote your pawn to king only at the end of the " +
                   "chessboard! Demote king at position {} ".format(man_added) +
                   "back to pawn, please.")
            return True

    if move[1][1] == 8:
        try:
            if man_added[0] != move[1]:
                print ("ERROR: You must promote your piece at the end of the chessboard! Promote" +
                       "pawn piece at position {}, please.".format(move[1]))
                return True
        except IndexError:
            print ("ERROR: You must promote your piece at the end of the chessboard! Promote" +
                   "pawn piece at position {}, please.".format(move[1]))
            return True

    return False


def main():
    cont = Controller()

    # LED panel testing
    rospy.sleep(0.1)
    cont.info_panel(RED + WHITE + GREEN, io.ON)
    rospy.sleep(2)
    cont.info_panel(RED + WHITE + GREEN, io.OFF)

    # Check if the human color is orange
    if rospy.get_param("robocheckers/human_color").capitalize() == "Orange":
        cont.human_color = "O"
        cont.computer_color = "B"
    # If not, default is blue
    else:
        cont.human_color = "B"
        cont.computer_color = "O"

    print "Enter `0` to start or `1` to let the AI start: "
    player_order = -1
    while True:
        try:
            player_order = int(raw_input())
        except ValueError:
            print "ERROR: You have to enter `0` or `1` only!\n"
            continue

        if player_order in [0, 1]:
            break
        else:
            print "ERROR: You have to enter `0` or `1` only!"

    # Send the choice to game_logic node
    cont.send_human_move((0, player_order))

    while not DEBUG:
        # Load Human and Coputer pieces data
        cont.Human.pieces = cont.Human_tmp.pieces
        cont.Human.get_squares()
        cont.Computer.pieces = cont.Computer_tmp.pieces
        cont.Computer.get_squares()

        # If both starting position are ok, break the loop
        if cont.Human.check_start_state() and cont.Computer.check_start_state():
            break
        else:
            print("ERROR: Invalid starting board! " +
                  "Both players must have exactly 12 pawns in white squares.")
            raw_input("Fix the mistakes and pres `Enter`.")

    while True:
        rospy.sleep(1)
        if cont.interface():
            break


if __name__ == '__main__':
    main()
