#!/usr/bin/env python
"""
 Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved.
 Permission is hereby granted, free of charge, to any person obtaining a copy of this
 software and associated documentation files (the "Software"), to deal in the Software
 without restriction, including without limitation the rights to use, copy, modify,
 merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
 permit persons to whom the Software is furnished to do so.
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
 INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
 PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
"""
import rospy
import boto3
import time

from std_msgs.msg import String
from sensor_msgs.msg import CompressedImage, Joy


MIN_CAPTURE_PAUSE = 3 # Min seconds between image captures, no need to spam services here
MIN_CONFIDENCE = 50  # 0 to 100
MAX_LABEL_COUNT = 10


class ObjectDetector():

    def __init__(self):
        rospy.loginfo('(ObjectDetector): initializing')
        self.pub = rospy.Publisher('/detected_objects', String, queue_size=1)
        self.joysub = rospy.Subscriber("/joy", Joy, self.joystick_input)  # Kept this to show how to hook joystick if one is available
        
        self.aws_region = rospy.get_param('~/object_detector/aws_region')
        self.camera_topic = rospy.get_param('~/object_detector/camera_topic')
        self.auto_capture = rospy.get_param('~/object_detector/auto_capture')
        self.rekognition = boto3.client('rekognition', self.aws_region)
        self.last_capture_time = 0

    def joystick_input(self, data):
        dpad = data.buttons[0:]
        button_pressed = any(dpad)
        if button_pressed:
            rospy.loginfo('Joystick input detected.  Button pressed: {}'.format((button_pressed)))
            self.process_image()

    def process_image(self):
        
        if time.time() < self.last_capture_time + MIN_CAPTURE_PAUSE:
            return
        
        rospy.loginfo('Getting image from camera')
        img = self.get_image()

        if img is None:
            return
            
        rospy.loginfo('Sending image to Rekognition')
        try:
            response = self.rekognition.detect_labels(
                Image={'Bytes': img},
                MinConfidence=MIN_CONFIDENCE,
                MaxLabels=MAX_LABEL_COUNT
            )
            labels = self.sort_labels(response)

        except Exception as e:
            rospy.logerr('Error calling Rekognition: {}'.format(e))
        else:
            if labels:
                rospy.logdebug('(ObjectDetector) Detected: {}'.format(labels))
                self.pub.publish(",".join(labels))
                
        # Reset the capture time
        self.last_capture_time = time.time()
    
    def get_image(self):
        try:
            img = rospy.wait_for_message(self.camera_topic, CompressedImage, timeout=3).data
        except Exception as e:
            rospy.logerr('Error getting image from camera: {}'.format(e))
            return None
        else:
            return img
            
    def sort_labels(self, rekognition_response):
        '''Return labels from the Rekognition response sorted by confidence in descending order'''
        labels = rekognition_response['Labels']
        return [l['Name'] for l in sorted(labels, key=lambda k: k['Confidence'])]


def main():
    rospy.init_node('object_detector')
    detector = ObjectDetector()
    
    rate = rospy.Rate(1)  # For simplicity, check every second, unrelated to frequency of Rekognition calls
    while not rospy.is_shutdown():
        if detector.auto_capture:
            detector.process_image()
        rate.sleep()


if __name__ == '__main__':
    main()