#include <gtest/gtest.h>

#include <atomic>
#include <chrono>
#include <thread>

#include "mission_adapters/mission_adapters.h"

using namespace mission_adapters;

namespace
{

robot_geometry_msgs::PoseStamped MakeGoal(double x, double y)
{
    robot_geometry_msgs::PoseStamped g;
    g.pose.position.x = x;
    g.pose.position.y = y;
    return g;
}

robot_protocol_msgs::Action MakeAction(
    const std::string &id)
{
    robot_protocol_msgs::Action a;
    a.actionId = id;
    a.actionType = "TEST";
    return a;
}

robot_protocol_msgs::Node MakeNode(
    int seq,
    bool add_action = false)
{
    robot_protocol_msgs::Node n;
    n.sequenceId = seq;
    n.nodeId = "node_" + std::to_string(seq);

    n.nodePosition.x = seq;
    n.nodePosition.y = seq;

    if (add_action)
        n.actions.push_back(
            MakeAction("node_action_" + std::to_string(seq)));

    return n;
}

robot_protocol_msgs::Edge MakeEdge(
    int seq,
    bool add_action = false)
{
    robot_protocol_msgs::Edge e;

    e.sequenceId = seq;
    e.edgeId = "edge_" + std::to_string(seq);

    if (add_action)
        e.actions.push_back(
            MakeAction("edge_action_" + std::to_string(seq)));

    return e;
}

robot_protocol_msgs::Order MakeOrder(
    int node_count)
{
    robot_protocol_msgs::Order order;

    for (int i = 0; i < node_count; ++i)
        order.nodes.push_back(
            MakeNode(i));

    for (int i = 0; i < node_count - 1; ++i)
        order.edges.push_back(
            MakeEdge(i));

    return order;
}

} // namespace

//--------------------------------------------------------------
// EventBus
//--------------------------------------------------------------

TEST(EventBus, PriorityOrdering)
{
    EventBus bus;

    bus.push({EventType::PAUSE,
              PRIORITY_PAUSE,
              {}});

    bus.push({EventType::CANCEL,
              PRIORITY_CANCEL,
              {}});

    bus.push({EventType::EMERGENCY,
              PRIORITY_EMERGENCY,
              {}});

    Event e;

    ASSERT_TRUE(bus.pop(e));
    EXPECT_EQ(e.priority,
              PRIORITY_EMERGENCY);

    ASSERT_TRUE(bus.pop(e));
    EXPECT_EQ(e.priority,
              PRIORITY_CANCEL);

    ASSERT_TRUE(bus.pop(e));
    EXPECT_EQ(e.priority,
              PRIORITY_PAUSE);
}

TEST(EventBus, StopReturnsFalse)
{
    EventBus bus;

    std::thread t([&]()
                  { bus.stop(); });

    Event e;

    EXPECT_FALSE(bus.pop(e));

    t.join();
}

TEST(EventBus, ConcurrentPush)
{
    EventBus bus;

    constexpr int N = 100;

    std::thread producer([&]()
                         {
                             for (int i = 0; i < N; i++)
                             {
                                 bus.push(
                                     {EventType::NAV_DONE,
                                      PRIORITY_NAV_DONE,
                                      {}});
                             }
                         });

    int count = 0;

    while (count < N)
    {
        Event e;

        if (bus.pop(e))
            ++count;
    }

    producer.join();

    EXPECT_EQ(count, N);
}

//--------------------------------------------------------------
// GoalAdapter
//--------------------------------------------------------------

TEST(GoalAdapter, ConvertSingleGoal)
{
    GoalAdapter adapter;

    auto missions =
        adapter.convert(
            MakeGoal(5.5, 9.1));

    ASSERT_EQ(missions.size(), 1u);

    EXPECT_EQ(
        missions[0]->type,
        MissionType::SIMPLE_GOAL);

    EXPECT_DOUBLE_EQ(
        missions[0]->goal.pose.position.x,
        5.5);

    EXPECT_DOUBLE_EQ(
        missions[0]->goal.pose.position.y,
        9.1);
}

//--------------------------------------------------------------
// VDA5050Adapter
//--------------------------------------------------------------

TEST(VDA5050Adapter, EmptyOrder)
{
    VDA5050Adapter adapter;

    robot_protocol_msgs::Order order;

    auto missions =
        adapter.convert(order);

    EXPECT_TRUE(
        missions.empty());
}

TEST(VDA5050Adapter, OrderWithoutActionsCreatesTailMission)
{
    VDA5050Adapter adapter;

    auto order =
        MakeOrder(4);

    auto missions =
        adapter.convert(order);

    ASSERT_EQ(
        missions.size(),
        1u);

    EXPECT_EQ(
        missions[0]->nodes.size(),
        4u);

    EXPECT_EQ(
        missions[0]->edges.size(),
        3u);
}

TEST(VDA5050Adapter, SplitAtNodeAction)
{
    VDA5050Adapter adapter;

    auto order =
        MakeOrder(5);

    order.nodes[2].actions.push_back(
        MakeAction("dock"));

    auto missions =
        adapter.convert(order);

    ASSERT_EQ(
        missions.size(),
        2u);

    EXPECT_EQ(
        missions[0]->nodes.size(),
        3u);

    EXPECT_EQ(
        missions[1]->nodes.size(),
        3u);
}

TEST(VDA5050Adapter, CollectEdgeAction)
{
    VDA5050Adapter adapter;

    auto order =
        MakeOrder(2);

    order.nodes[1].actions.push_back(
        MakeAction("node"));

    order.edges[0].actions.push_back(
        MakeAction("edge"));

    auto missions =
        adapter.convert(order);

    ASSERT_EQ(
        missions.size(),
        1u);

    ASSERT_EQ(
        missions[0]->actions.size(),
        2u);

    EXPECT_LT(
        missions[0]->actions[0].sequenceId,
        missions[0]->actions[1].sequenceId);
}

//--------------------------------------------------------------
// MissionManager
//--------------------------------------------------------------

TEST(MissionManager, SubmitChangesState)
{
    MissionManager mgr;

    GoalAdapter adapter;

    mgr.submit(
        adapter.convert(
            MakeGoal(1, 2)));

    EXPECT_EQ(
        mgr.state(),
        MissionState::QUEUED);

    EXPECT_TRUE(
        mgr.hasMission());
}

TEST(MissionManager, NextMission)
{
    MissionManager mgr;

    GoalAdapter adapter;

    mgr.submit(
        adapter.convert(
            MakeGoal(1, 2)));

    auto m =
        mgr.nextMission();

    ASSERT_NE(
        m,
        nullptr);

    EXPECT_EQ(
        mgr.state(),
        MissionState::RUNNING);
}

TEST(MissionManager, NavigationDone)
{
    MissionManager mgr;

    GoalAdapter adapter;

    mgr.submit(
        adapter.convert(
            MakeGoal(0, 0)));

    mgr.nextMission();

    mgr.onNavigationDone();

    EXPECT_EQ(
        mgr.state(),
        MissionState::IDLE);

    EXPECT_FALSE(
        mgr.hasMission());
}

TEST(MissionManager, NavigationFailed)
{
    MissionManager mgr;

    GoalAdapter adapter;

    mgr.submit(
        adapter.convert(
            MakeGoal(0, 0)));

    mgr.nextMission();

    mgr.onNavigationFailed();

    EXPECT_EQ(
        mgr.state(),
        MissionState::FAILED);
}

TEST(MissionManager, PauseResume)
{
    MissionManager mgr;

    GoalAdapter adapter;

    mgr.submit(
        adapter.convert(
            MakeGoal(1, 1)));

    mgr.pause();

    EXPECT_EQ(
        mgr.state(),
        MissionState::PAUSED);

    mgr.resume();

    EXPECT_EQ(
        mgr.state(),
        MissionState::QUEUED);
}

TEST(MissionManager, Cancel)
{
    MissionManager mgr;

    GoalAdapter adapter;

    mgr.submit(
        adapter.convert(
            MakeGoal(1, 1)));

    mgr.cancel();

    EXPECT_EQ(
        mgr.state(),
        MissionState::CANCELLED);

    EXPECT_FALSE(
        mgr.hasMission());
}

TEST(MissionManager, Emergency)
{
    MissionManager mgr;

    GoalAdapter adapter;

    mgr.submit(
        adapter.convert(
            MakeGoal(2, 2)));

    mgr.emergency();

    EXPECT_EQ(
        mgr.state(),
        MissionState::EMERGENCY);
}

TEST(MissionManager, ClearEmergency)
{
    MissionManager mgr;

    mgr.emergency();

    mgr.clearEmergency();

    EXPECT_EQ(
        mgr.state(),
        MissionState::CLEAR_EMERGENCY);
}

//--------------------------------------------------------------
// EventProcessor
//--------------------------------------------------------------

TEST(EventProcessor, GoalEvent)
{
    MissionManager mgr;

    EventProcessor proc(mgr);

    proc.start();

    proc.goalEvent(
        MakeGoal(5, 6));

    std::this_thread::sleep_for(
        std::chrono::milliseconds(100));

    EXPECT_EQ(
        mgr.state(),
        MissionState::QUEUED);

    proc.stop();
}

TEST(EventProcessor, EmergencyHasHighestPriority)
{
    MissionManager mgr;

    EventProcessor proc(mgr);

    proc.start();

    proc.goalEvent(
        MakeGoal(1, 1));

    proc.emergencyEvent();

    std::this_thread::sleep_for(
        std::chrono::milliseconds(100));

    EXPECT_EQ(
        mgr.state(),
        MissionState::EMERGENCY);

    proc.stop();
}

//--------------------------------------------------------------
// MissionExecutor
//--------------------------------------------------------------

TEST(MissionExecutor, CallbackInvokedOnce)
{
    MissionManager mgr;

    GoalAdapter adapter;

    mgr.submit(
        adapter.convert(
            MakeGoal(10, 10)));

    MissionExecutor exec(mgr);

    std::atomic<int> count{0};

    exec.setMissionCallback(
        [&](const std::shared_ptr<Mission> &)
        {
            count++;
        });

    exec.start();

    std::this_thread::sleep_for(
        std::chrono::milliseconds(200));

    exec.stop();

    EXPECT_EQ(
        count.load(),
        1);
}

TEST(MissionExecutor, NewMissionAfterDone)
{
    MissionManager mgr;

    GoalAdapter adapter;

    mgr.submit(
        adapter.convert(
            MakeGoal(1, 1)));

    MissionExecutor exec(mgr);

    std::atomic<int> count{0};

    exec.setMissionCallback(
        [&](const std::shared_ptr<Mission> &)
        {
            count++;
        });

    exec.start();

    std::this_thread::sleep_for(
        std::chrono::milliseconds(100));

    mgr.onNavigationDone();

    mgr.submit(
        adapter.convert(
            MakeGoal(2, 2)));

    std::this_thread::sleep_for(
        std::chrono::milliseconds(150));

    exec.stop();

    EXPECT_EQ(
        count.load(),
        2);
}

//--------------------------------------------------------------

int main(int argc, char **argv)
{
    ::testing::InitGoogleTest(
        &argc,
        argv);

    return RUN_ALL_TESTS();
}