#!/usr/bin/env python

from __future__ import print_function

from sensor_msgs.msg import JointState
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

import rospy
import actionlib
from control_msgs.msg import FollowJointTrajectoryAction
from control_msgs.msg import FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint

from sensor_msgs.msg import PointCloud2

from control_msgs.msg import GripperCommandAction
from control_msgs.msg import GripperCommandGoal

from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse

import math
import time
import threading
import sys
import os
import tf2_ros
import argparse as ap

import hello_helpers.hello_misc as hm
import stretch_funmap.navigate as nv
import stretch_funmap.manipulation_planning as mp


class CleanSurfaceNode(hm.HelloNode):

    def __init__(self):
        hm.HelloNode.__init__(self)
        self.rate = 10.0
        self.joint_states = None
        self.joint_states_lock = threading.Lock()
        self.move_base = nv.MoveBase(self)
        self.letter_height_m = 0.2
        self.wrist_position = None
        self.lift_position = None
        self.manipulation_view = None
        self.debug_directory = None
        
    def joint_states_callback(self, joint_states):
        with self.joint_states_lock: 
            self.joint_states = joint_states
        wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(joint_states)
        self.wrist_position = wrist_position
        lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states)
        self.lift_position = lift_position
        
    def lower_tool_until_contact(self):
        rospy.loginfo('lower_tool_until_contact')
        trigger_request = TriggerRequest() 
        trigger_result = self.trigger_lower_until_contact_service(trigger_request)
        rospy.loginfo('trigger_result = {0}'.format(trigger_result))

    def wipe_in(self):
        rospy.loginfo('wipe_in')
        if self.wrist_position is not None:
            wrist_target_m = self.wrist_position - 0.2
            pose = {'wrist_extension': wrist_target_m}
            self.move_to_pose(pose)
            return True
        else:
            rospy.logerr('wipe_in: self.wrist_position is None!')
            return False

    def wipe_out(self):
        rospy.loginfo('wipe_out')
        if self.wrist_position is not None:
            wrist_target_m = self.wrist_position + 0.22
            pose = {'wrist_extension': wrist_target_m}
            self.move_to_pose(pose)
            return True
        else:
            rospy.logerr('wipe_out: self.wrist_position is None!')
            return False

    def look_at_surface(self):
        self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory)
        manip = self.manipulation_view
        manip.move_head(self.move_to_pose)
        manip.update(self.point_cloud, self.tf2_buffer)
        if self.debug_directory is not None:
            dirname = self.debug_directory + 'clean_surface/'
            # If the directory does not already exist, create it.
            if not os.path.exists(dirname):
                os.makedirs(dirname)
            filename = 'look_at_surface_' + hm.create_time_string()
            manip.save_scan(dirname + filename)
        else:
            rospy.loginfo('CleanSurfaceNode: No debug directory provided, so debugging data will not be saved.')
        
    def trigger_clean_surface_callback(self, request):

        self.look_at_surface()
        strokes, simple_plan, lift_to_surface_m = self.manipulation_view.get_surface_wiping_plan(self.tf2_buffer)

        print('********* lift_to_surface_m = {0} **************'.format(lift_to_surface_m))

        tool_width_m = 0.03
        safety_from_edge = 0.03
        tool_length_m = 0.03

        if True and (strokes is not None) and (len(strokes) > 5):

            if (self.lift_position is not None) and (self.wrist_position is not None): 

                lift_above_surface_m = self.lift_position + lift_to_surface_m + 0.1 
                pose = {'joint_lift': lift_above_surface_m}
                rospy.loginfo('Raise tool above surface.')
                self.move_to_pose(pose)

                start = simple_plan[0]
                forward_m = start['mobile_base_forward_m']
                pose = {'translate_mobile_base': forward_m}
                rospy.loginfo('Drive to the start of the cleaning region.')
                self.move_to_pose(pose)

                initial_wrist_position = self.wrist_position 
                extension_m = start['start_wrist_extension_m']

                start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0)
                pose = {'wrist_extension': start_extension_m}
                rospy.loginfo('Extend tool above surface.')
                self.move_to_pose(pose)
                
                self.lower_tool_until_contact()

                extension_m = start['end_wrist_extension_m']
                end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0) - safety_from_edge
                pose = {'wrist_extension': end_extension_m}
                rospy.loginfo('Extend tool to end of surface.')
                self.move_to_pose(pose)

                extension_m = start['start_wrist_extension_m']
                pose = {'wrist_extension': start_extension_m}
                rospy.loginfo('Retract tool to beginning.')
                self.move_to_pose(pose)

                for m in simple_plan[1:]:
                    forward_m = m['mobile_base_forward_m']
                    pose = {'translate_mobile_base': forward_m}
                    rospy.loginfo('Drive to the start of the cleaning region.')
                    self.move_to_pose(pose)

                    extension_m = m['start_wrist_extension_m']
                    start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0)
                    pose = {'wrist_extension': start_extension_m}
                    rospy.loginfo('Extend tool above surface.')
                    self.move_to_pose(pose)

                    extension_m = m['end_wrist_extension_m']
                    end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0) - safety_from_edge
                    pose = {'wrist_extension': end_extension_m}
                    rospy.loginfo('Extend tool to end of surface.')
                    self.move_to_pose(pose)

                    extension_m = m['start_wrist_extension_m']
                    pose = {'wrist_extension': start_extension_m}
                    rospy.loginfo('Retract tool to beginning.')
                    self.move_to_pose(pose)

                pose = {'joint_lift': lift_above_surface_m}
                rospy.loginfo('Raise tool above surface.')
                self.move_to_pose(pose)

                pose = {'wrist_extension': initial_wrist_position}
                rospy.loginfo('Retract tool to initial position.')
                self.move_to_pose(pose)

        return TriggerResponse(
            success=True,
            message='Completed surface cleaning!'
            )

    
    def main(self):
        hm.HelloNode.main(self, 'clean_surface', 'clean_surface', wait_for_first_pointcloud=False)
        
        self.debug_directory = rospy.get_param('~debug_directory')
        rospy.loginfo('Using the following directory for debugging files: {0}'.format(self.debug_directory))

        self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
        
        self.trigger_clean_surface_service = rospy.Service('/clean_surface/trigger_clean_surface',
                                                           Trigger,
                                                           self.trigger_clean_surface_callback)

        rospy.wait_for_service('/funmap/trigger_reach_until_contact')
        rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.')
        self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', Trigger)

        rospy.wait_for_service('/funmap/trigger_lower_until_contact')
        rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.')
        self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger)

        default_service = '/camera/switch_to_default_mode'
        high_accuracy_service = '/camera/switch_to_high_accuracy_mode'
        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service)
        rospy.wait_for_service(default_service)
        rospy.loginfo('Node ' + self.node_name + ' connected to ' + default_service)
        self.trigger_d435i_default_mode_service = rospy.ServiceProxy(default_service, Trigger)
        rospy.wait_for_service(high_accuracy_service)
        rospy.loginfo('Node ' + self.node_name + ' connected to'  + high_accuracy_service)
        self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger)
        
        rate = rospy.Rate(self.rate)
        while not rospy.is_shutdown():
            rate.sleep()

        
if __name__ == '__main__':
    try:
        parser = ap.ArgumentParser(description='Clean Surface behavior for stretch.')
        args, unknown = parser.parse_known_args()
        node = CleanSurfaceNode()
        node.main()
    except KeyboardInterrupt:
        rospy.loginfo('interrupt received, so shutting down')
