import rclpy
from rclpy.node import Node

from std_msgs.msg import Header
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion

from defs.srv import HighLvlCmds
from high_lvl_cmd_srv.cmd_constants import *

import transforms3d

# To add a new voice command, item, etc.:
# 1. Add to Enum in cmd_constants.py
# 2. Add words to word list in voice_listener.py
# 3. Add function to HighLvlCmdSrv
# 4. Add that function to to HighLvlCmdSrv.CmdFunc

class VoiceCommandException(Exception):
    pass
class OpenError(VoiceCommandException):
    pass
class MoveError(VoiceCommandException):
    pass
class TurnError(VoiceCommandException):
    pass
class PickUpError(VoiceCommandException):
    pass
class SetDownError(VoiceCommandException):
    pass
class GiveError(VoiceCommandException):
    pass
class NotRecognizedError(VoiceCommandException):
    pass


# Temp for demo -- position presets
Position = [    [ 0.0, 0.0, 0.0 ],
                [ 0.5, 0.0, 0.0 ],
                [ 0.0, -0.5, 0.4 ],
                [ 0.0, 0.0, 0.5 ],
                [ 0.2, 0.7, 0.1 ]   ]


class HighLvlCmdSrv(Node):
    
    def __init__(self):

        super().__init__('high_lvl_cmd_srv')

        self.logger = self.get_logger()

        self.srv = self.create_service(HighLvlCmds, 'high_lvl_cmds', self._high_lvl_cmd_callback)

        self.fake_action_server_pub = self.create_publisher(PoseStamped, 'fake_action_server_topic', 10) # for testing
        self.pose_pub = self.create_publisher(PoseStamped, 'move_pose', 10)

        # # Subscribe to hand for self.holdingItem
        # self.subscription = self.create_subscription(
        #     ???,
        #     '???',
        #     self._hand_callback,
        #     10)

        self.holdingItem = Item.NONE   # Item currently being held; won't be necessary when hand/arbiter knows this

        self.CmdFunc = {    Cmd.NONE     : self.none,
                            Cmd.IGNORE   : self.ignore,
                            Cmd.OPEN     : self.open,
                            Cmd.CLOSE    : self.close,
                            Cmd.MOVE     : self.move,
                            Cmd.TURN     : self.turn,
                            Cmd.PICKUP   : self.pickup,
                            Cmd.SETDOWN  : self.setdown,
                            Cmd.GIVE     : self.give    }


    def _hand_callback(self, msg):
        # Receive info on which object is currently being held
        self.holdingItem = msg.data


    def _high_lvl_cmd_callback(self, request, response):

        self.requestedCommand       = Cmd(request.command)
        self.requestedItem          = Item(request.item)
        self.requestedDirection     = Direction(request.direction)
        self.requestedPosition      = request.position
        self.requestedDegrees       = request.degrees

        if not (    self.requestedCommand == Cmd.NONE       or  \
                    self.requestedCommand == Cmd.PICKUP     or  \
                    self.requestedCommand == Cmd.SETDOWN    or  \
                    self.requestedCommand == Cmd.GIVE               ):
            self.tts('This command does not yet interface with the arm. Try pickup, setdown, or give')

        response.success = False
        try:
            self.CmdFunc[self.requestedCommand]()
            response.success = True
        except Exception as e:
            self.tts(f"Error: {e}") # Send to some tts node?

        self.requestedCommand       = Cmd.NONE
        self.requestedItem          = Item.NONE
        self.requestedDirection     = Direction.NONE
        self.requestedPosition      = -1
        self.requestedDegrees       = -1

        return response


    def move_pose(self, point, level=False):

        poseStamped = PoseStamped()

        # Default header for now
        poseStamped.header = Header()

        poseStamped.pose.position = Point(x=point[0],
                                          y=point[1],
                                          z=point[2])

        vector = [point[0], point[1], point[2]]
        if level:
            vector[2] = 0

        quat = transforms3d.quaternions.axangle2quat(vector, 0)
        
        #poseStamped.pose.orientation = Quaternion(w=quat[0],
        #                                          x=quat[1],
        #                                          y=quat[2],
        #                                          z=quat[3])
        poseStamped.pose.orientation = Quaternion(w=0.7071069,
                                                  x=0.70710667,
                                                  y=0.0,
                                                  z=0.0)

        self.pose_pub.publish(poseStamped)
        self.fake_action_server_pub.publish(poseStamped)


    def open(self):

        # requestedItem = self.parseWords(WordsType.ITEM)

        if self.requestedItem != Item.NONE:
            if self.requestedItem != self.holdingItem:
                raise OpenError(f"Can't drop {self.requestedItem} because currently holding {self.holdingItem}")
            
        if self.holdingItem == Item.NONE:
            self.tts('Attempting to open the claw')
        else:
            self.tts(f'Attempting to drop the {self.holdingItem}')

        # Send command to action server to open the claw
        
        # self._send_goal_future = self._action_client.send_goal_async(
        #     goal_msg,
        #     feedback_callback=self.feedback_callback)

        self.holdingItem = Item.NONE # Delete this line once hand/arbiter is publishing this info


    def close(self):

        self.tts('Attempting to close the claw')

        # Send command to action server to close the claw

        self.holdingItem = Item.UNKNOWN # Delete this line once hand/arbiter is publishing this info


    def move(self):
        
        if self.requestedDirection == Direction.NONE:
            raise TurnError('A direction to move in must be specified')

        # if not (1 <= self.requestedPosition or self.requestedPosition <= len(Position)):
        #     raise TurnError(f'Please state a position from 1 to {len(Position)}')

        self.tts (f'Attempting to move')
        # self.tts(f'Attempting to move to a preset position {self.requestedPosition} {self.requestedDirection}')

        # self.move_pose(Position[self.requestedPosition-1])


    def turn(self):

        # self.requestedDegrees = self.parseWords(WordsType.DEGREES)
        # self.requestedDirection = self.parseWords(WordsType.DIRECTION)
        
        if self.requestedDirection == Direction.NONE:
            if self.requestedDegrees % 180 == 0:
                # Left/right does not matter if doing a 180
                self.requestedDirection = Direction.LEFT
            else:
                raise TurnError('A direction to turn must be specified')

        self.tts(f'Attempting to turn {self.requestedDegrees} degrees {self.requestedDirection}')

        # Calculate new position from current position + degrees to rotate

        # Send command to action server to move to new position


    def pickup(self):

        # requestedItem = self.parseWords(WordsType.ITEM)

        # Make sure hand is currently empty
        if self.holdingItem != Item.NONE:
            raise PickUpError(f"Can't pick up {self.requestedItem}, already holding {self.holdingItem}")

        if self.requestedItem == Item.NONE:
            raise PickUpError(f'Did not hear a valid item specified to pick up')

        self.tts(f'Attempting to pick up {self.requestedItem}')

        # Temp
        self.move_pose(Position[2])

        # Send command to action server to open the claw

        # Use video processing service client to ask for position of target
        # if !targetFound:
            # raise PickUpError(f'Unable to find the requested item')

        # Move to current item position, checking if it has moved

        # prevTargetPos = None
        # targetPos = None
        # firstTime = True
        # While (robot hand hasn't yet reached the target) or (firstTime)

            # targetPos = video_processing_service_request(target)
            
            # if targetPos == None:  # If the target wasn't found by video processing serivce
                # if firstTime:
                    # raise PickUpError(f'Unable to find the requested item')
                # targetPos = prevTargetPos

            # if firstTime or distance(targetPos, prevTargetPos) > some_threshold_distance
                # Send goal to action server to move to that position

        # Send command to action server to close the claw

        self.holdingItem = self.requestedItem # Delete this line once hand/arbiter is publishing this info


    def setdown(self):
    
        # Make sure hand is currently holding something
        if self.holdingItem == Item.NONE:
            raise SetDownError(f'Not currently holding anything to set down')

        # requestedItem = self.parseWords(WordsType.ITEM)

        # If an item was mentioned
        if self.requestedItem != Item.NONE:
            # Make sure it is the currently held item
            if self.requestedItem != self.holdingItem:
                raise SetDownError(f"Can't set down {self.requestedItem}, not currently holding it")

        self.tts(f'Attempting to set down {self.requestedItem}')


        # Temp
        self.move_pose(Position[3])


        # Send command to action server to go down to the table/surface (just current position but with Z = 0?)

        # Send command to action server to open the claw

        self.holdingItem = Item.NONE # Delete this line once hand/arbiter is publishing this info


    def give(self):
        
        # If the claw is not currently holding anything
        if self.holdingItem == Item.NONE:

            # Pick up the item first
            self.pickup()

        else:

            # requestedItem = self.parseWords(WordsType.ITEM)

            # If a specific item was requested
            if self.requestedItem != Item.NONE:

                # Make sure the requested item is currently being held
                if self.requestedItem != self.holdingItem:
                    raise GiveError(f"Can't give you {self.requestedItem}, already holding {self.holdingItem}")


        self.tts(f'Attempting to give {self.holdingItem} to the operator')


        # Temp
        self.move_pose(Position[5])

        # prevHandPos = None
        # handPos = None
        # firstTime = True
        # While (robot hand hasn't yet reached operator hand) or (firstTime)

            # handPos = video_processing_service_request(opHand)
            
            # if handPos == None:  # If operator hand wasn't found by video processing serivce
                # if firstTime:
                    # raise GiveError('Unable to find the requested item')
                # handPos = prevHandPos

            # if firstTime or distance(handPos, prevHandPos) > some_threshold_distance
                # Send goal to action server to move to that position


        # isGrabbed = hand_service_client(something) to block until the hand determines that the operator is holding the target
        # if isGrabbed:
            # Send command to action server to open the claw
        # else:
            # raise GrabError(f'Couldn't tell whether the {self.holdingItem} was being grabbed')

        self.holdingItem = Item.NONE # Delete this line once hand/arbiter is publishing this info


    def ignore(self):
        self.tts('Ignoring')


    def none(self):
        raise NotRecognizedError("Sorry, I didn't get that.")


    def tts(self, phrase):
        # Send to some text-to-speech node, maybe the arbiter?
        self.logger.info(phrase)



def main(args=None):

    rclpy.init(args=args)
    high_lvl_cmd_service = HighLvlCmdSrv()
    rclpy.spin(high_lvl_cmd_service)

    high_lvl_cmd_service.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
