#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
# Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
# SPDX-License-Identifier: MIT-0
#
# 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.
#

#
# This node enable you to control robot moving velocity from cloud using AWS IoT.
#

import json
import time

import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import String

# set speed parameter
LINERAR_SPEED = 0.1
LINEAR_STEP = 0.1 / 3
ANGULER_SPEED = 0.5
ANGULER_STEP = 0.5 / 3


class RemoteController():
    def __init__(self):
        # set topic for publication and subscription
        # set other parameters
        self._cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self._remote_sub = rospy.Subscriber('/awsiot_to_ros', String, self.awsiot_to_ros_cb, queue_size=1)
        self.twist = Twist()
        self.next_synctime = time.time()
        self.is_moving = False
        self.action = ""

    def main(self):
        while not rospy.is_shutdown():
            # if turtlebot is not moving, sleep for 0.3 seconds
            if not self.is_moving:
                time.sleep(0.3)
                continue

            # if next_synctime hasn't come yet, wait until the next_synctime
            while not rospy.is_shutdown():
                d = self.next_synctime - time.time()
                if d <= 0:
                    break
                time.sleep(d)

            # if turtlebot is moving, execute sync_action
            if self.is_moving:
                self.sync_action()

    # devide into cases according to self.action and put velocity into self.twist
    def sync_action(self):
        if self.action == 'forward':
            self.twist.linear.x = min([LINERAR_SPEED, self.twist.linear.x + LINEAR_STEP])
            self.is_moving = True
        elif self.action == 'backward':
            self.twist.linear.x = max([-LINERAR_SPEED, self.twist.linear.x - LINEAR_STEP])
            self.is_moving = True
        elif self.action == 'right':
            self.twist.angular.z = max([-ANGULER_SPEED, self.twist.angular.z - ANGULER_STEP])
            self.is_moving = True
        elif self.action == 'left':
            self.twist.angular.z = min([ANGULER_SPEED, self.twist.angular.z + ANGULER_STEP])
            self.is_moving = True
        if self.action == 'stop' or self.action == 'right' or self.action == 'left':
            if self.twist.linear.x > 0:
                self.twist.linear.x = max([0, self.twist.linear.x - LINEAR_STEP])
            elif self.twist.linear.x < 0:
                self.twist.linear.x = min([0, self.twist.linear.x + LINEAR_STEP])
        if self.action == 'stop' or self.action == 'forward' or self.action == 'backward':
            if self.twist.angular.z > 0:
                self.twist.angular.z = max([0, self.twist.angular.z - ANGULER_STEP])
            elif self.twist.angular.z < 0:
                self.twist.angular.z = min([0, self.twist.angular.z + ANGULER_STEP])

        # set next_synctime to 0.3 seconds away
        self.next_synctime = time.time() + 0.3
        # publish the velocity to /cmd_vel topic
        self._cmd_pub.publish(self.twist)

        # if velocity is equal to 0, then set self.is_moving to false
        if self.twist.linear.x == 0 and self.twist.angular.z == 0:
            self.is_moving = False

    def awsiot_to_ros_cb(self, message):
        # subscribe controll comands from aws iot
        try:
            message_json = json.loads(message.data)
            payload = message_json['payload']
            if 'command' in payload and payload['command'] == "move":
                rospy.loginfo(message)
                rospy.loginfo("remote_controller Move {}".format(payload['action']))
                self.action = payload['action']
                self.sync_action()
        except Exception as e:
            rospy.logwarn(e)


def main():
    rospy.init_node('remote_controller')
    remote_controller = RemoteController()
    remote_controller.main()


if __name__ == '__main__':
    main()