#!/usr/bin/env python

import rospy
import smach
import smach_ros
from actionlib import *
#from actionlib_msgs.msg import *

from std_srvs.srv import Empty

from .states.InitialisationState import *
from .states.CheckingQKStatusState import *

from .states.QKDState import *

from .states.ConnectionToRobotState import *
from .states.ManualControlModeState import *


def main():
    rospy.init_node('quantum_command_center', log_level=rospy.DEBUG)

    youbot_base = YoubotBaseControl()

    sm_root = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    # coordinates of visual marker
    sm_root.userdata.marker= [
        [0,0],
        [0,0],
        [0,0],
        [0,0]
    ]

    with sm_root:
        """
            Initialisation
                'succeeded'
                    CheckingQKStatusState (quary to DB)
                        'succeeded'
                            ConnectionToRobot
                                'succeeded'
                                    ManualControlModeState
                                        'outofqk'
                                        'aborted'
                        'failed'
                            Waiting while Robot find BS and get some quantum keys
                                'succeeded'            
                                    QKD session (or in this case, just update database)
                                        'succeeded'     
                <------------------------
        """
        
        
    
        smach.StateMachine.add('INITIALISATION', InitialisationState(), transitions={'succeeded': 'CHECKING_QK_STATUS', 'aborted': 'aborted'})
        smach.StateMachine.add('CHECKING_QK_STATUS', CheckingQKStatusState(), transitions={'succeeded': 'CONNECTION_TO_COMAND_CENTER', 'failed': 'FINDING_BASE_STATION'})

        smach.StateMachine.add('CONNECTION_TO_ROBOT', ConnectionToCommandCenterState(), transitions={'succeeded': 'MANUAL_MODE', 'failed': 'aborted'})
        smach.StateMachine.add('MANUAL_MODE', ManualControlModeState(), transitions={'aborted': 'WAITING', 'outoftheqk': 'CHECKING_QK_STATUS'})
        smach.StateMachine.add('WAITING', ManualControlModeState(), transitions={'succeeded': 'QKD'})
        smach.StateMachine.add('QKD', QKDState(), transitions={'succeeded': 'CHECKING_QK_STATUS'})

    sis = smach_ros.IntrospectionServer('state_machine', sm_root, '/SM_QCC_ROOT')
    sis.start()
    outcome = sm_root.execute()

    rospy.spin()
    sis.stop()
    rospy.signal_shutdown('All done.')


if __name__ == '__main__':
    main()
