#pragma once

/*
 * Copyright (C) 2020-2022 LEIDOS.
 *
 * 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.
 */

#include <rclcpp/rclcpp.hpp>
#include <stdexcept>

namespace route {

    /**
    * Possible events for the RouteStateWorker to respond to:
    * ROUTE_LOADED - Route worker received all necessary parameters and loaded route file path
    * ROUTE_SELECTED - A route file has been selected by user
    * ROUTE_STARTED - A route is generated by provided route file
    * ROUTE_COMPLETED - Reached the destination of current route
    * ROUTE_DEPARTED - Vehicle has deviated from the route
    * ROUTE_ABORTED - User decides to stop following the current route
    * ROUTE_GEN_FAILED - No route can be generated based on provided route file
    * ROUTE_INVALIDATION - Certain portion of the route is invalidated based on situation.
    */
    enum RouteEvent {
        ROUTE_LOADED = 0,
        ROUTE_SELECTED = 1,
        ROUTE_STARTED = 2,
        ROUTE_COMPLETED = 3,
        ROUTE_DEPARTED = 4,
        ROUTE_ABORTED = 5,
        ROUTE_GEN_FAILED = 6,
        ROUTE_INVALIDATION = 7,
    };

    // Stream operator for the RouteEvent enum
    inline std::ostream &operator<<(std::ostream &output, const RouteEvent &e)
    {
        switch(e) {
        case ROUTE_LOADED:
            output << "ROUTE_LOADED";
            break;
        case ROUTE_SELECTED:
            output << "ROUTE_SELECTED";
            break;
        case ROUTE_STARTED:
            output << "ROUTE_STARTED";
            break;
        case ROUTE_COMPLETED:
            output << "ROUTE_COMPLETED";
            break;
        case ROUTE_DEPARTED:
            output << "ROUTE_DEPARTED";
            break;
        case ROUTE_ABORTED:
            output << "ROUTE_ABORTED";
            break;
        case ROUTE_GEN_FAILED:
            output << "ROUTE_GEN_FAILED";
            break;
        case ROUTE_INVALIDATION:
            output << "ROUTE_INVALIDATION";
            break;
        default:
            output << "UNKNOWN_ROUTE_EVENT(" << static_cast<int>(e) << ")";
            break;
        }
        return output;
    }
    /**
    * Possible states of the RouteStateWorker:
    * LOADING - RouteState worker's initial state, waiting for all necessary parameters to be set
    * SELECTION - RouteState worker is waiting on user to select a route
    * ROUTING - Calling lanelet2 library to generate a route based on selected route file
    * FOLLOWING - Following a route and tracking its downtrack and crosstrack distance
    */
    enum RouteState {
        LOADING = 0,
        SELECTION = 1,
        ROUTING = 2,
        FOLLOWING = 3,
    };

    // Stream operator for the RouteState enum
    inline std::ostream &operator<<(std::ostream &output, const RouteState &s)
    {
        switch(s) {
        case LOADING:
            output << "LOADING";
            break;
        case SELECTION:
            output << "SELECTION";
            break;
        case ROUTING:
            output << "ROUTING";
            break;
        case FOLLOWING:
            output << "FOLLOWING";
            break;
        default:
            output << "UNKNOWN_ROUTE_STATE(" << static_cast<int>(s) << ")";
            break;
        }
        return output;
    }


    class RouteStateWorker {

        public:

            RouteStateWorker() = default;

            /**
             * \brief Process route event based on designed state machine diagram
             * \param event Incoming route event
             */
            void onRouteEvent(RouteEvent event);

            /**
             * \brief Get current route state machine state
             */
            RouteState getRouteState() const;

            void setLoggerInterface(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger);

        private:

            // private local variable tracks the current route satte
            RouteState state_ = RouteState::LOADING;

            // Logger interface
            rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_;
    };

}