#! /usr/bin/env python

"""Sends mock cone detections based on robot location and waypoints."""

from math import sqrt, tan, atan2, pi
import rospy
import message_filters
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import PoseStamped, TransformStamped
from robo_magellan.msg import LocalWaypointList as LocalWaypointList
from robo_magellan.msg import LocalWaypoint as LocalWaypoint
from robo_magellan.msg import location_msgs as location_data
from robo_magellan.msg import pose_data as pose_data

class Topics:
    LOCAL_POSITION = '/mavros/local_position/pose'
    CONE_LOCATIONS = '~cone_locations'
    LOCAL_WAYPOINTS = '~waypoints/local'

class MockConeDetector:

    def run(self):
        rospy.init_node('mock_cone_detector')

        self.detection_distance = rospy.get_param('~detection_distance', 8.0)
        self.camera_horz_fov = rospy.get_param('~horz_fov', 70.0) / 180.0 * pi
        self.camera_horz_pixels = rospy.get_param('~horz_pixels', 640)
        self.detect_cones = rospy.get_param("~detect_cones", True)

        self.cone_detection_patterns = eval(rospy.get_param(
            '~cone_detection_patterns', "[]"))
        self.detection_positions = [0 for p in self.cone_detection_patterns]
        self.detection_counts = [0 for p in self.cone_detection_patterns]
        rospy.loginfo('%d cone patterns', len(self.cone_detection_patterns))

        self.waypoint_list = LocalWaypointList()
        self.waypoint_list.waypoints = []

        rospy.Subscriber(Topics.LOCAL_WAYPOINTS, LocalWaypointList,
                         self.on_waypoints_changed)
        rospy.Subscriber(Topics.LOCAL_POSITION, PoseStamped,
                         self.on_robot_position_changed)

        self.cone_pub = rospy.Publisher(Topics.CONE_LOCATIONS, location_data,
                                        queue_size=1)

        rospy.spin()

    def on_waypoints_changed(self, waypoint_list):
        self.waypoint_list = waypoint_list

    def on_robot_position_changed(self, pose):
        cone_poses = []

        xr, yr = pose.pose.position.x, pose.pose.position.y
        q = [pose.pose.orientation.x,
             pose.pose.orientation.y,
             pose.pose.orientation.z,
             pose.pose.orientation.w]
        (roll, pitch, theta) = euler_from_quaternion(q)

        # A pseudo-distance used to determine the pixel width of a cone
        # heading angle.
        fov_dist = self.camera_horz_pixels/2 / tan(self.camera_horz_fov/2)

        cone_index = 0
        for wp in self.waypoint_list.waypoints:
            # Skip waypoints that are not cones.
            if wp.point.z < 1000:
                continue

            # Cone location.
            xc, yc = wp.point.x, wp.point.y
            d = sqrt((xc-xr)**2 + (yc-yr)**2)

            # Heading from robot to cone.
            cone_direction = atan2(yc-yr, xc-xr)
            heading = self.normalize_angle(cone_direction - theta)

            if d <= self.detection_distance \
               and abs(heading) <= self.camera_horz_fov/2:

                # Determine view x, y, w, h, d, area as seen by the camera
                # on the robot.
                pose = pose_data()
                pose.d = d
                pose.x = int(round(fov_dist * tan(heading)))
                pose.y = 0 # Just put all cones in the middle, vertically.
                pose.z = 0 # Assume minimum possible depth is not available.
                pose.w, pose.h, pose.area = self.get_cone_area(d)

                if self.detect_cones and self.is_cone_detected(cone_index):
                    cone_poses.append(pose)
                    rospy.loginfo('[Detector] cone %d: d=%f heading=%f x=%d',
                                  cone_index, d, heading*180/pi, pose.x)

            cone_index += 1

        msg = location_data()
        msg.header.stamp = rospy.Time.now()
        msg.poses = cone_poses
        self.cone_pub.publish(msg)

    def is_cone_detected(self, cone_index):
        # If we don't have a detection pattern for the cone, we always
        # see it.
        if cone_index >= len(self.cone_detection_patterns):
            rospy.loginfo('No detection pattern for cone %d', cone_index)
            return True

        pattern = self.cone_detection_patterns[cone_index]
        position = self.detection_positions[cone_index]
        count = self.detection_counts[cone_index]

        # If we have exhausted the pattern, we always see the cone.
        if position >= len(pattern):
            rospy.loginfo('No more patterns for cone %d', cone_index)
            return True

        # If we have exhausted the count for the current position, move
        # to the next position.
        if count >= pattern[position]:
            position += 1
            count = 0
            rospy.loginfo('Moving to pattern %d for cone %d',
                          position, cone_index)

        count += 1
        # We detect the cone at even numbered positions in the pattern or
        # if we have exhausted the pattern.
        detected = (position >= len(pattern)) or (position%2 == 0)
        rospy.loginfo('Cone detection: cone=%d position=%d count=%d detect=%s',
                      cone_index, position, count, detected)

        # Update count and position.
        self.detection_positions[cone_index] = position
        self.detection_counts[cone_index] = count

        return detected


    def get_cone_area(self, d):
        # Area should be proportional to the square of distance.
        # Assume approx. 3900 pixels at 3 meters and a 2.0
        # aspect ratio.
        ratio = 3.0**2 / d**2
        area = ratio * 3900
        aspect = 18.0/8.0
        w = int(sqrt(area/aspect))
        h = int(w * aspect)
        return (w, h, area)

    def normalize_angle(self, theta):
        """Gets an equivalent angle to a specified angle within the
           range plus pi to minus pi."""
        while theta >= 2*pi:
            theta -= 2*pi
        while theta < 0:
            theta += 2*pi

        # Now 0 <= theta < 2*pi. Convert to -pi < theta <= pi
        if theta > pi:
            return theta - 2*pi
        else:
            return theta

if __name__ == "__main__":
    try:
        MockConeDetector().run()

    except rospy.ROSInterruptException:
        pass
