#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (C) 2013, Alexander Tiderko
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of the author nor of other contributors may be
#    used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#

import math
import sys
import threading

from geographic_msgs.msg import GeoPoint
from geographic_msgs.msg import RouteNetwork
from geographic_msgs.msg import RoutePath
from geographic_msgs.srv import GetGeoPath
from geographic_msgs.srv import GetGeoPathResponse
from geographic_msgs.srv import GetRoutePlan
from geographic_msgs.srv import GetRoutePlanResponse
from geometry_msgs.msg import Point
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Vector3
from route_network import planner
from sensor_msgs.msg import NavSatFix
from std_msgs.msg import ColorRGBA
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import geodesy
import rospy
import tf


class RvizGoalListener():
    """
    ROS node to handle the goal and init positions from rviz and calculate the route.
    """

    def __init__(self):
        rospy.init_node('rviz_goal')
        self._lock = threading.RLock()
        self.graph = None
        rospy.loginfo("Waiting for 'get_geo_path' service...")
        rospy.wait_for_service('get_geo_path')
        self.get_geo_path = rospy.ServiceProxy('get_geo_path', GetGeoPath)

        # Advertise visualization marker topic (display the map on RVIZ)
        self.viz_pub = rospy.Publisher('visualization_marker_array', MarkerArray, latch=True, queue_size=1)

        # Subscribe to sat fix
        self._sub_fix = rospy.Subscriber('fix', NavSatFix, self.gps_fix_callback)
        
        # Subscribe to initialpose: Listen for starting position to plan a route.
        self._sub_init = rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.initpose_callback)

        # Subscribe to goal: Listen for goal position to plan a route.
        self._sub_goal = rospy.Subscriber('goal', PoseStamped, self.goal_callback)

        self._own_geo_point = None
        self._own_utmpoint = None
        self.listener = tf.TransformListener()
        self.gridZone = None

        # Subscribe to route network (Graph on which the planning is carried out).
        self._sub_route = rospy.Subscriber('route_network', RouteNetwork, self.graph_callback)

    def graph_callback(self, graph):
        """ Callback to initialize to graph to plan on.

        :param graph: The required graph. 
        :type graph: geographic_msgs/RouteNetwork
        """
        if len(graph.points) > 0:
            graph.points[0].position
            utm = geodesy.utm.fromLatLong(graph.points[0].position.latitude, graph.points[0].position.longitude, graph.points[0].position.altitude)
            self.gridZone = utm.gridZone()
            self.points = geodesy.wu_point.WuPointSet(graph.points)
            self.segment_ids = {}   # segments symbol table
            for sid in xrange(len(graph.segments)):
                self.segment_ids[graph.segments[sid].id.uuid] = sid
            self.graph = graph

    def initpose_callback(self, msg):
        """ Callback to aquire the initial position from RVIZ.
        
        :param msg: The initial position.
        :type msg: geometry_msgs/PoseWithCovarianceStamped
        """
        with self._lock:
            rospy.loginfo("Init pose received.")
            if not self.gridZone is None:
                (map_trans, map_rot) = self.listener.lookupTransform(msg.header.frame_id, "/world", rospy.Time(0))
                self._own_utmpoint = geodesy.utm.UTMPoint(easting=msg.pose.pose.position.x-map_trans[0], 
                                                northing=msg.pose.pose.position.y-map_trans[1], 
                                                altitude=msg.pose.pose.position.z-map_trans[2], 
                                                zone=self.gridZone[0], 
                                                band=self.gridZone[1])
                self._own_geo_point = self._own_utmpoint.toMsg()
            else:
                rospy.logerr("Unknown UMT grid zone!")
    
    def gps_fix_callback(self, msg):
        """ Callback to acquire the GPS Fix message from a robot.

        :param msg: The GPS position.
        :type msg: geometry_msgs/PoseWithCovarianceStamped
        """
        with self._lock:
            self._own_utmpoint = geodesy.utm.fromLatLong(msg.latitude, msg.longitude, msg.altitude)
            self._own_geo_point = GeoPoint(msg.latitude, msg.longitude, msg.altitude) 

    def goal_callback(self, msg):
        """ Callback to acquire the goal position, and calculate the plan towards it.
        
        :param msg: The goal position.
        :type msg: geometry_msgs/PoseStamped
        """
        with self._lock:
            rospy.loginfo("Goal pose received.")
            if not self._own_geo_point is None:
                try:
                    (map_trans, map_rot) = self.listener.lookupTransform(msg.header.frame_id, "/world", rospy.Time(0))
                    gridZone = self._own_utmpoint.gridZone()
                    goal_utm = geodesy.utm.UTMPoint(easting=msg.pose.position.x-map_trans[0], 
                                                    northing=msg.pose.position.y-map_trans[1], 
                                                    altitude=msg.pose.position.z-map_trans[2], 
                                                    zone=gridZone[0], 
                                                    band=gridZone[1])
                    geo_goal = goal_utm.toMsg()
                    path_plan = self.get_geo_path(self._own_geo_point, geo_goal)
                    self._pub_viz_path([pose.pose.position for pose in path_plan.plan.poses], self._own_geo_point, geo_goal, path_plan.start_seg, path_plan.goal_seg, path_plan.distance)
                except (tf.Exception, tf.LookupException, tf.ConnectivityException):
                    import traceback
                    print traceback.format_exc()
            else:
                print "no init pose"

    def _pub_viz_path(self, path, start, goal, start_seg, end_seg, distance):
        """ Publish visualization markers for a RoutePath.

        :param path: The planned path to be visualized.
        :type path: geographic_msgs/GeoPoint[]
        :param start: The start position.
        :type start: geographic_msgs/GeoPoint
        :param goal: The goal position.
        :type goal: geographic_msgs/GeoPoint
        :param start_seg: The ID of the nearest graph-segment to the starting way point.
        :type start_seg: uuid_msgs/UniqueID
        :param end_seg: The ID of the nearest segment to the goal way point.
        :type end_seg: uuid_msgs/UniqueID
        :param distance: Length of the planned route.
        :type distance: float64
        """
        if self.viz_pub.get_num_connections() > 0 and not self.graph is None:
            life_time = rospy.Duration(120)
            marks = MarkerArray()
            hdr = self.graph.header
            hdr.stamp = rospy.Time.now()
            index = 0
            # add nearest segments
            marker = Marker(header = hdr,
                            ns = 'start segment',
                            id = index,
                            type = Marker.LINE_STRIP,
                            action = Marker.ADD,
                            scale=Vector3(x=3.0),
                            color = ColorRGBA(r=0.3, g=0.3, b=1.0, a=0.8),
                            lifetime = life_time)
            segment = self.graph.segments[self.segment_ids[start_seg.uuid]]
            marker.points.append(self.points[segment.start.uuid].toPointXY())
            marker.points.append(self.points[segment.end.uuid].toPointXY())
            marks.markers.append(marker)
            marker = Marker(header = hdr,
                            ns = 'goal segment',
                            id = index,
                            type = Marker.LINE_STRIP,
                            action = Marker.ADD,
                            scale=Vector3(x=3.0),
                            color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.8),
                            lifetime = life_time)
            segment = self.graph.segments[self.segment_ids[end_seg.uuid]]
            marker.points.append(self.points[segment.start.uuid].toPointXY())
            marker.points.append(self.points[segment.end.uuid].toPointXY())
            marks.markers.append(marker)

            # add start point
            marker = Marker(header = hdr,
                            ns = 'start',
                            id = index,
                            type = Marker.CYLINDER,
                            action = Marker.ADD,
                            scale = Vector3(x=6.0, y=4., z=10.0),
                            color = ColorRGBA(r=0.3, g=0.3, b=1.0, a=0.8),
                            lifetime = life_time)
            utm_start = geodesy.utm.fromMsg(start)
            marker.pose.position.x = utm_start.easting
            marker.pose.position.y = utm_start.northing
            marks.markers.append(marker)
            # add the route
            index += 1
            path_marker = Marker(header = hdr,
                                  ns = 'path',
                                  id = index,
                                  type = Marker.LINE_STRIP,
                                  action = Marker.ADD,
                                  scale=Vector3(x=3.0),
                                  color = ColorRGBA(r=1.0, g=1.0, b=1.0, a=0.8),
                                  lifetime = life_time)
            for geo_point in path:
                utm_point = geodesy.utm.fromMsg(geo_point)
                path_marker.points.append(Point(utm_point.easting, utm_point.northing, 0.))
            marks.markers.append(path_marker)
            index += 1
            # add goal point
            marker = Marker(header = hdr,
                            ns = 'goal',
                            id = index,
                            type = Marker.CYLINDER,
                            action = Marker.ADD,
                            scale = Vector3(x=6.0, y=4., z=10.0),
                            color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.8),
                            lifetime = life_time)
            utm_goal = geodesy.utm.fromMsg(goal)
            marker.pose.position.x = utm_goal.easting
            marker.pose.position.y = utm_goal.northing
            marks.markers.append(marker)
            
            # add distance to the goal
            marker = Marker(header = hdr,
                            ns = 'goal distance',
                            id = index,
                            text = '{0:.2f}m'.format(distance),
                            type = Marker.TEXT_VIEW_FACING,
                            action = Marker.ADD,
                            scale = Vector3(x=6.0, y=4., z=10.0),
                            color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.8),
                            lifetime = life_time)
            utm_goal = geodesy.utm.fromMsg(goal)
            marker.pose.position.x = utm_goal.easting
            marker.pose.position.y = utm_goal.northing
            marker.pose.position.z = 15
            marks.markers.append(marker)
            self.viz_pub.publish(marks)

def main():
  node_class = RvizGoalListener()
  try:
      rospy.spin()            # wait for messages
  except rospy.ROSInterruptException: pass
  return 0

if __name__ == '__main__':
    # run main function and exit
    sys.exit(main())
