#!/usr/bin/env python
from rospy import get_param
from numpy import sqrt

import include.yaml_data_handling as yaml_handle

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

# Definition of players
HUMAN = 0
COMPUTER = 1

class Piece:
    pos = (0, 0)
    real_pos = (0, 0)
    center_dist = (0, 0)
    square = (0, 0)
    rank = "unassigned"

    # Constructor to define piece position and wheter its pawn, or king
    def __init__(self, pos, rank):
        # Set inner variables, if real_pos is true, save provided pos into real_pos
        self.pos = pos
        self.rank = rank

    def __str__(self):
        msg = "Rank: {}\n".format(self.rank)
        # Print only non-zero data
        if sum(self.pos):
            msg = msg + "Position [px]:\n  x: {}\n  y: {}\n".format(self.pos[0],
                                                                    self.pos[1])
        if sum(self.center_dist):
            msg = msg + "Distance from center [px]:\n  x: {}\n  y: {}\n".format(self.center_dist[0],
                                                                                self.center_dist[1])
        if sum(self.real_pos):
            msg = msg + "Position [mm]:\n  x: {}\n  y: {}\n".format(self.real_pos[0],
                                                                  self.real_pos[1])
        if sum(self.square):
            msg = msg + "Square:\n  x: {}\n  y: {}".format(self.square[0],
                                                           self.square[1])

        return msg

    def calc_dist(self, center):
        """
        Calculate the distance between a piece and the center of camera screen
        """
        # Calculate the distance in both axes
        tmp = list(self.center_dist)
        tmp[0] = abs(self.pos[0] - center[0])
        tmp[1] = abs(self.pos[1] - center[1])
        self.center_dist = tuple(tmp)

    def get_square(self):
        """
        Calculate the square position of the piece
        """
        chessboard = yaml_handle.load_dimensions_data(yaml_handle.RESIZE_TO_PX)
        chessboard = chessboard.get('chessboard')

        x = float(self.pos[0])/chessboard*8+1
        y = float(self.pos[1])/chessboard*8+1

        self.square = (int(x), int(y))

    def normalize_by_center(self):
        """
        Remap the piece position by the distance from the center point of view
        """
        # Remap position to real world
        tmp = list(self.pos)

        # Remap by the distance from the center point of veiw
        tmp[0] = int(self.pos[0] + (-0.0259522 * float(self.pos[0]) + 6.2))
        tmp[1] = int(self.pos[1] + (-0.0238963 * float(self.pos[1]) + 4.4))
        #tmp[0] = int(self.pos[0] + (-0.0259522 * float(self.pos[0]) + 4.38586))
        #tmp[1] = int(self.pos[1] + (-0.0238963 * float(self.pos[1]) + 2.46914))

        self.pos = tuple(tmp)

    def remap_to_real_world(self):
        """
        Remap the piece position by the real world coordinates
        """
        # Load dimension data
        pose_shift = yaml_handle.load_dimensions_data(yaml_handle.POSE_SHIFT)
        coef = pose_shift.get('coef')
        offx = pose_shift.get('offset_x')
        offy = pose_shift.get('offset_y')

        # Remap position to real world
        tmp = list(self.real_pos)
        # real_x = cam_y * coef + off_real_x
        tmp[0] = round(self.pos[1] * coef + offx, 3)
        # real_y = cam_x * coef + off_real_y
        tmp[1] = round(self.pos[0] * coef + offy, 3)

        self.real_pos = tuple(tmp)


class Player:
    def __init__(self, player_type=-1):
        self.pieces = []
        if player_type in [HUMAN, COMPUTER]:
            self.player_type = player_type
        else:
            self.player_type = -1

    def __str__(self):
        ret_str = ""
        for i, piece in enumerate(self.pieces):
            ret_str += "=== Piece No. {} ===\n".format(i) + str(piece) + "\n"

        if ret_str == "":
            return "Empty"
        return ret_str

    def player_str(self):
        if self.player_type == 0:
            return "Human"
        if self.player_type == 1:
            return "Computer"
        return ""

    def empty(self):
        self.pieces = []

    def convert(self, color):
        msg = []

        # Creating header of the message carryiong information about player
        if color == "blue":
            msg.append("B")
        elif color == "orange":
            msg.append("O")
        else:
            print("ERROR: Invalid player color!")
            return -1

        # Creating message content in format (posX, posY), rank
        for piece in self.pieces:
            if piece.rank == "king":
                msg.append(1)
            else:
                msg.append(0)
            msg.append(round(piece.pos[0], 3))
            msg.append(round(piece.pos[1], 3))

        return msg

    def publish(self, pub, color):
        msg = ','.join(str(e) for e in self.convert(color))
        if DEBUG: print(msg)

        pub.publish(msg)

    def get_piece_data(self, keypoints, mark_keypoints, offset=0):
        """
        Fill the instance of the player class with piece data
        :param kepoints: Keypoints from found blobs
        :param offset: Offset in image for piece position recalculation
        """
        # Empty the instance of this class and prepare some temp vars
        self.empty()
        pieces = []
        marks = []

        # Fill var keypoints with positions of pieces
        for item in keypoints:
            pieces.append((round(item.pt[0]-offset, 3), round(item.pt[1]-offset, 3)))

        # Fill var mark_keypoints with positions of marks
        for item in mark_keypoints:
            marks.append((round(item.pt[0]-offset, 3), round(item.pt[1]-offset, 3)))

        # Cycle through pieces and add them with respective ranks
        for piece in pieces:
            for mark in marks:
                # If green mark near piece, set mark to king and break and add it
                if sqrt((piece[0]-mark[0])**2 + (piece[1]-mark[1])**2) < 32:
                    self.add(Piece(piece, 'king'))
                    break
            # If piece not added as king, add it as pawn
            if Piece(piece, 'king') not in pieces:
                self.add(Piece(piece, 'pawn'))

    def calc_dists(self, center):
        """
        Calculate the distances between the pieces and the center of camera screen
        """
        for i, _ in enumerate(self.pieces):
            self.pieces[i].calc_dist(center)

    def get_squares(self):
        """
        Calculate the square positions of the pieces
        """
        for i, _ in enumerate(self.pieces):
            self.pieces[i].get_square()

    def normalize_pieces(self):
        """
        Remap the pieces by the distance from the center point of view
        """
        for i, _ in enumerate(self.pieces):
            self.pieces[i].normalize_by_center()

    def remap_pieces(self):
        """
        Remap the pieces by the real coordinates
        """
        for i, _ in enumerate(self.pieces):
            self.pieces[i].remap_to_real_world()

    def get_positions(self, square=False):
        pos_list = []
        
        if square:
            for piece in self.pieces:
                pos_list.append(piece.square)
        else:
            for piece in self.pieces:
                pos_list.append(piece.pos)

        return pos_list

    # Function for adding pieces to player
    def add(self, piece):
        if not piece.pos in self.get_positions():
            self.pieces.append(piece)

    def fill(self, msg):
        # Checking if list is empty
        if len(msg) <= 1:
            return False

        # Removing first number from list
        msg.pop(0)

        for i in range(0, len(msg), 3):
            if msg[i] == 1:
                self.add(Piece((msg[i+1], msg[i+2]), 'king'))
            else:
                self.add(Piece((msg[i+1], msg[i+2]), 'pawn'))

        return True

    def check_start_state(self):
        # Checking the correct number of pieces
        if len(self.pieces) != 12:
            print self.player_str(), "ERROR: Not exactly 12 pieces!"
            return False

        # Defining human starting positions
        human_pos = [(2, 1), (4, 1), (6, 1), (8, 1),
                     (1, 2), (3, 2), (5, 2), (7, 2),
                     (2, 3), (4, 3), (6, 3), (8, 3)]
        # Defining computer starting positions
        computer_pos = [(1, 6), (3, 6), (5, 6), (7, 6),
                        (2, 7), (4, 7), (6, 7), (8, 7),
                        (1, 8), (3, 8), (5, 8), (7, 8)]

        for piece in self.pieces:
            # Checking if all the pieces are pawns
            if piece.rank == "king":
                print self.player_str(), "ERROR: Not all pieces are pawns!"
                return False

            # Checking if all pieces are in right positions
            if self.player_type == HUMAN:
                if piece.square not in human_pos:
                    print self.player_str(), "ERROR: Bad piece position!"
                    print "Piece position:", piece.square
                    return False
            elif self.player_type == COMPUTER:
                if piece.square not in computer_pos:
                    print self.player_str(), "ERROR: Bad piece position!"
                    print "Piece position:", piece.square
                    return False
            else:
                return False

        return True
