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

import include.move_group_lib as mg


class RobotControl:
    def __init__(self):
        print("")
        print("--------------------------------")
        print(" Welcome to the Robot Move 3.0! ")
        print("--------------------------------")
        print("Press Ctrl+D to exit at any time")
        print("")

        # Definition of global instance of MoveGroup Class
        print("============ Initializing ...")
        self.movement = mg.MoveGroup()

        print("============ Adding collision walls")
        self.movement.addCollisionObjects()

        # Define default and drop positions, go to default
        self.movement.initStaticPositions()
        self.movement.goToStaticPosition("default")

        # Open gripper
        self.movement.set_gripper(mg.io.OFF)

        self.robot_busy = rospy.Publisher('/robocheckers/robot_busy', Bool, queue_size=10)
        
        #rospy.Subscriber('/robot_status', String, self.receive_robot_status)

        rospy.Subscriber('/robocheckers/coordinates', String, self.receive_coordinates)
        rospy.spin()


    def convertData(self, data):
        # Defining move list and word string
        pos_list = []
        word = ""
        # Add coma to the end of data string to save last value
        data += ','
        
        # Converting data separated by coma from string to list
        for char in data:
            # If coma detected, append word to player list
            if char == ',':
                # Try adding it as float, if not possilbe, add it as string
                try:
                    pos_list.append(float(word)/1000)
                except ValueError:
                    pos_list.append(word)
                    pos_list.append(0)
                # Clear the word string
                word = ""
            # If coma not detected, just add the char to word string
            else:
                word += char

        move = []
        for i in range(0, len(pos_list), 2):
            if isinstance(pos_list[i], (float, int)):
                move.append((pos_list[i], pos_list[i+1]))
            elif isinstance(pos_list[i], str):
                move.append(pos_list[i])
            else:
                print "ERROR: Invalid message!"

        move_list = []
        for i in range(0, len(move), 2):
            move_list.append([move[i], move[i+1]])

        return move_list


    #def receive_robot_status(self, data):
    #    print "In error", data.data.in_error.val


    def receive_coordinates(self, data):
        move = self.convertData(data.data)
        print "Received data for move:", move
        # Send the message, that the robot is busy
        self.robot_busy.publish(True)

        try:
            # Moving via path
            for move_seq in move:
                print "Move sequence:", move_seq
                piece, dest = move_seq

                if piece == "promote":
                    self.movement.pickKing()
                else:
                    self.movement.moveAboveBoard(piece)
            
                    self.movement.pickAndPlace("pick")
            
                if dest == "drop":
                    self.movement.dropPiece()
                else:
                    self.movement.moveAboveBoard(dest)
                
                    self.movement.pickAndPlace("place")

            self.movement.goToStaticPosition("default")

            print("============ Movement complete! ============")
        except rospy.ROSInterruptException:
            return
        except KeyboardInterrupt:
            print("============ Press `Enter` to execute a movement to default position ...")
            raw_input()
            self.movement.goToStaticPosition("default")

            print("============ Press `Enter` to remove collision walls ...")
            raw_input()
            self.movement.scene.remove_world_object()
            return

        # Send the message, that the robot is not busy anymore
        self.robot_busy.publish(False)


def main():
    move = RobotControl()


if __name__ == '__main__':
    main()