#!/usr/bin/env python3

# Copyright 2021 Open Rise Robotics
#
# 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.

import rclpy
import rclpy.logging
import rclpy.node

from clearpath_config_live.clearpath_config_handler import (
    ClearpathConfigHandler
)
from clearpath_config_live.clearpath_config_watcher import (
    ClearpathConfigWatcher
)
from clearpath_config_live.robot_description_client import (
    RobotDescriptionClient
)


def main():
    # Node
    rclpy.init()
    node = rclpy.create_node("clearpath_config_live")
    node.declare_parameter("config_file", "/etc/clearpath/robot.yaml")
    logger = rclpy.logging.get_logger("clearpath_config_live")

    # Get File
    config_param = node.get_parameter("config_file")
    config_file = config_param.get_parameter_value().string_value

    # Watcher, Handler, and Client
    client = RobotDescriptionClient(
        node,
        "robot_state_publisher",
        "robot_description"
    )
    watcher = ClearpathConfigWatcher(config_file, logger)
    handler = ClearpathConfigHandler(watcher, client, logger)

    # Start Handler
    try:
        watcher.start(handler)
    except Exception as ex:
        logger.error("Live updater failed: \n")
        logger.error(ex.args)

    # Spin
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        logger.info("Live updater exited with keyboard interrupt.")
    finally:
        watcher.stop()


if __name__ == "__main__":
    main()
