#!/usr/bin/env python
# -*- coding: utf-8 -*-

################################################################################
# Copyright 2018 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
################################################################################

# Author: Leon Jung, Gilbert, Ashe Kim

import rospy
import numpy as np
from enum import Enum
from std_msgs.msg import UInt8
from turtlebot3_autorace_msgs.msg import MovingParam

class CoreModeDecider():
    def __init__(self):
        # subscribes : invoking object detected
        self.sub_traffic_light = rospy.Subscriber('/detect/traffic_light', UInt8, self.cbInvokedByTrafficLight, queue_size=1)
        self.sub_traffic_sign = rospy.Subscriber('/detect/traffic_sign', UInt8, self.cbInvokedByTrafficSign, queue_size=1)
        self.sub_returned_mode = rospy.Subscriber('/core/returned_mode', UInt8, self.cbReturnedMode, queue_size=1)
        # publishes : decided mode
        self.pub_decided_mode = rospy.Publisher('/core/decided_mode', UInt8, queue_size=1)
        self.InvokedObject = Enum('InvokedObject', 'traffic_light traffic_sign')
        self.TrafficSign   = Enum('TrafficSign', 'intersection left right construction stop parking tunnel')
        self.CurrentMode   = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking_lot level_crossing tunnel')
        self.fnInitMode()
      # Invoke if traffic light is detected
    def cbInvokedByTrafficLight(self, invoke_type_msg):
        rospy.loginfo("Invoke Traffic light detected")
        self.fnDecideMode(self.InvokedObject.traffic_light.value, invoke_type_msg)
    def cbInvokedByTrafficSign(self, traffic_sign_msg):
        rospy.loginfo("Invoke Traffic sign detected")
        self.fnDecideMode(self.InvokedObject.traffic_sign.value, traffic_sign_msg)
    def cbReturnedMode(self, mode):
        rospy.loginfo("Init Mode")
        self.fnInitMode()
    def fnInitMode(self):                               # starts only when the program is started initially or any mission is completed
        self.current_mode = self.CurrentMode.lane_following.value
        self.fnPublishMode()
    def fnDecideMode(self, invoked_object, msg_data):   # starts only when the traffic sign / traffic light is detected & current_mode is lane_following
        rospy.loginfo("mode --all_core_mode_decider-----------------------------")
        rospy.loginfo("mode current_mode=%d",self.current_mode)
        rospy.loginfo("mode invoked_object=%d",invoked_object)
        rospy.loginfo("mode msg_data=%d",msg_data.data)
        rospy.loginfo("mode-------------------------------")
        if self.current_mode == self.CurrentMode.lane_following.value:
            rospy.loginfo("mode current_mode %d",self.current_mode)
            if invoked_object == self.InvokedObject.traffic_light.value:    # Traffic Light detected
                rospy.loginfo("mode invoked object %d",invoked_object)
                if msg_data.data  == self.CurrentMode.intersection.value:
                    rospy.loginfo("mode detect traffic_light and chenge mode")
                    self.current_mode = self.CurrentMode.intersection.value
            elif invoked_object == self.InvokedObject.traffic_sign.value:   # Any Sign detected
                rospy.loginfo("mode Any Sign detected")
                if msg_data.data == self.TrafficSign.intersection.value:
                    rospy.loginfo("detect sign : intersection")
                    self.current_mode = self.CurrentMode.intersection.value
                elif msg_data.data == self.TrafficSign.left.value:
                    rospy.loginfo("detect sign : intersection left")
                    self.current_mode = self.CurrentMode.intersection.value
                elif msg_data.data == self.TrafficSign.right.value:
                    rospy.loginfo("detect sign : inteersection right")
                    self.current_mode = self.CurrentMode.intersection.value
                elif msg_data.data == self.TrafficSign.construction.value:
                    rospy.loginfo("detect sign : construction")
                    self.current_mode = self.CurrentMode.construction.value
                elif msg_data.data == self.TrafficSign.parking_lot.value:
                    rospy.loginfo("detect sign : parking_log")
                    self.current_mode = self.CurrentMode.parking_lot.value
                elif msg_data.data == self.TrafficSign.level_crossing.value:
                    rospy.loginfo("detect sign : level_crossing")
                    self.current_mode = self.CurrentMode.level_crossing.value
                elif msg_data.data == self.TrafficSign.tunnel.value:
                    rospy.loginfo("detect sign : tunnel")
                    self.current_mode = self.CurrentMode.tunnel.value
            else:
                pass
        elif self.current_mode == self.CurrentMode.traffic_light.value:
            if invoked_object == self.InvokedObject.traffic_light.value:    # Traffic Light detected
                rospy.loginfo("traffic_light to lane_following")
                self.current_mode = self.CurrentMode.lane_follwoing.value
            else:
                pass
        else:
            pass
        self.fnPublishMode()

    def fnPublishMode(self):
        decided_mode = UInt8()
        decided_mode.data = self.current_mode
        self.pub_decided_mode.publish(decided_mode)

    def main(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('core_mode_decider')
    node = CoreModeDecider()
    node.main()