#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import numpy as np
from enum import Enum
from std_msgs.msg import UInt8
class CoreModeDecider():
 def __init__(self):
  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)
  self.pub_decided_mode = rospy.Publisher('/core/decided_mode', UInt8, queue_size=1)
  self.InvokedObject = Enum('InvokedObject', 'traffic_sign traffic_light')
  self.TrafficSign = Enum('TrafficSign', 'divide stop parking tunnel')
  self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light parking_lot level_crossing tunnel intersection construction')
  self.fnInitMode()
 def cbInvokedByTrafficSign(self, traffic_sign_type_msg):
  self.fnDecideMode(self.InvokedObject.traffic_sign.value, traffic_sign_type_msg)
  rospy.loginfo("Traffic sign detected")
 def cbReturnedMode(self, mode):
  rospy.loginfo("Init Mode")
  self.fnInitMode()
 def fnInitMode(self):
  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
  if self.current_mode == self.CurrentMode.lane_following.value:
   if invoked_object == self.InvokedObject.traffic_sign.value:   # Any Sign detected
    if msg_data.data == self.TrafficSign.stop.value:   # Stop Sign detected
     self.current_mode = self.CurrentMode.level_crossing.value
    elif msg_data.data == self.TrafficSign.parking.value:    # Parking Sign detected
     self.current_mode = self.CurrentMode.parking_lot.value
    elif msg_data.data == self.TrafficSign.tunnel.value:  # Tunnel Sign detected
     self.current_mode = self.CurrentMode.tunnel.value
   else:
    pass
   self.fnPublishMode()
  else:
   pass
 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()