// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// 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.

/*!
 * @file main.cxx
 * This file acts as a main entry point to the application.
 *
 * This file was generated by the tool fastddsgen.
 */

#include <csignal>
#include <cstring>
#include <functional>
#include <iostream>
#include <stdexcept>
#include <thread>

#include <fastdds/dds/log/Log.hpp>

#include "Subscriber.hpp"
#include "Publisher.hpp"
#include "CanMsgHandler.hpp"
#include "msg.hpp"
#include "DEBUG.hpp"


// ── 拦车器测试函数 ──
void bar_test(std::shared_ptr<MsgHandler> handler, const std::string& bar_name)
{
    DEBUG(bar_name + " test started...");
    
    std::this_thread::sleep_for(std::chrono::seconds(3));
    DEBUG("3 seconds elapsed, raising " + bar_name + "...");
    
    std::vector<std::string> ports;
    ports.push_back(bar_name + "BarUp");
    handler->CtrlBar(ports);
    
    // 等待 CtrlBar 内部所有消息发送完成
    std::this_thread::sleep_for(std::chrono::seconds(5));
    
    DEBUG("5 seconds elapsed, lowering " + bar_name + "...");
    
    ports.clear();
    ports.push_back(bar_name + "BarDown");
    handler->CtrlBar(ports);
    
    DEBUG(bar_name + " test completed.");
}

void front_bar_test(std::shared_ptr<MsgHandler> handler)
{
    bar_test(handler, "Front");
}

void back_bar_test(std::shared_ptr<MsgHandler> handler)
{
    bar_test(handler, "Back");
}



using eprosima::fastdds::dds::Log;

std::function<void(int)> stop_handler;
void signal_handler(
        int signum)
{
    stop_handler(signum);
}

std::string parse_signal(
        const int& signum)
{
    switch (signum)
    {
        case SIGINT:
            return "SIGINT";
        case SIGTERM:
            return "SIGTERM";
#ifndef _WIN32
        case SIGQUIT:
            return "SIGQUIT";
        case SIGHUP:
            return "SIGHUP";
#endif // _WIN32
        default:
            return "UNKNOWN SIGNAL";
    }
}

std::queue<WeighingSystem::BarCommandUpdate> MsgData::BarCommandUpdate_queue_;
std::queue<WeighingSystem::LightsCommandUpdate> MsgData::LightsCommandUpdate_queue_;
std::mutex MsgData::queue_cv_mtx_;

int main(int argc, char** argv)
{
    auto ret = EXIT_SUCCESS;
    std::shared_ptr<SubscriberApp> sub;
    std::shared_ptr<PublisherApp> pub;
    std::shared_ptr<MsgHandler> dev;

    int domain_id = 0;
    const char* interface = "can";
    const char* port = "can0";
    const char* baudrate = "500000";
    const char* device = "5serial";
    int device_id = 1;

    for (int i = 1; i < argc; i++)
    {
        if (strcmp(argv[i], "--domain") == 0 && i + 1 < argc)
        {
            domain_id = atoi(argv[++i]);
        }
        else if (strcmp(argv[i], "--test_front_bar") == 0)
        {
            // 启用前拦车器测试功能
        }
        else if (strcmp(argv[i], "--test_back_bar") == 0)
        {
            // 启用后拦车器测试功能
        }
        else if (strcmp(argv[i], "--interface") == 0 && i + 1 < argc)
        {
            interface = argv[++i];
        }
        else if (strcmp(argv[i], "--port") == 0 && i + 1 < argc)
        {
            port = argv[++i];
        }
        else if (strcmp(argv[i], "--baudrate") == 0 && i + 1 < argc)
        {
            baudrate = argv[++i];
        }
        else if (strcmp(argv[i], "--device") == 0 && i + 1 < argc)
        {
            device = argv[++i];
        }
        else if (strcmp(argv[i], "--device_id") == 0 && i + 1 < argc)
        {
            device_id = atoi(argv[++i]);
        }
        else if (strcmp(argv[i], "--version") == 0)
        {
            std::cout << "Vesrion: " << PROGRAM_VERSION << "\n";
            return EXIT_SUCCESS;
        }
        else if (strcmp(argv[i], "--help") == 0)
        {
            std::cout << "Usage: [options]\n"
                      << "Options:\n"
                      << "  --domain        Set domain ID\n"
                      << "  --interface     Set interface (e.g., can)\n"
                      << "  --baudrate      Set baudrate (e.g., 9600)\n"
                      << "  --port          Set port name (e.g., can0)\n"
                      << "  --device        Set device name (e.g., 5serial)\n"
                      << "  --version       Show software Version\n"
                      << "  --help          Show this help message\n"
                      << "  --test_front_bar Test front bar (raise after 3s, lower after 3s)\n";
            return EXIT_SUCCESS;
        }
        else
        {
            std::cerr << "Unknown option: " << argv[i] << "\n";
            std::cout << "Usage: [options]\n"
                      << "Options:\n"
                      << "  --domain        Set domain ID\n"
                      << "  --interface     Set interface (e.g., can)\n"
                      << "  --baudrate      Set baudrate (e.g., 9600)\n"
                      << "  --port          Set port name (e.g., can0)\n"
                      << "  --device        Set device name (e.g., 5serial)\n"
                      << "  --version       Show software Version\n"
                      << "  --help          Show this help message\n"
                      << "  --test_front_bar Test front bar (raise after 3s, lower after 3s)\n";
            return EXIT_FAILURE;
        }
    }

    sub = std::make_shared<SubscriberApp>(domain_id);
    pub = std::make_shared<PublisherApp>(domain_id);
    dev = std::make_shared<CanMsgHandler>();

    dev->OpenPort(port, baudrate);
    dev->device = device;
    dev->device_id = device_id;


    // ── 拦车器测试模式 ──
    bool test_front_bar = false;
    bool test_back_bar = false;
    for (int i = 1; i < argc; i++)
    {
        if (strcmp(argv[i], "--test_front_bar") == 0)
        {
            test_front_bar = true;
        }
        else if (strcmp(argv[i], "--test_back_bar") == 0)
        {
            test_back_bar = true;
        }
    }

    std::thread test_thread;
    if (test_front_bar)
    {
        test_thread = std::thread(front_bar_test, dev);
    }
    else if (test_back_bar)
    {
        test_thread = std::thread(back_bar_test, dev);
    }


    std::thread sub_thread(&SubscriberApp::run, sub, dev);
    std::thread pub_thread(&PublisherApp::run, pub, dev);

    DEBUG("Program is running. Please press Ctrl+C to stop at any time.");

    stop_handler = [&](int signum)
            {
                std::cout << "\n" << parse_signal(signum) << " received, stopping " << argv[1]
                          << " execution." << std::endl;
                sub->stop();
                pub->stop();
            };

    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);
#ifndef _WIN32
    signal(SIGQUIT, signal_handler);
    signal(SIGHUP, signal_handler);
#endif // _WIN32

    if (test_front_bar || test_back_bar)
    {
        test_thread.join();
    }

    sub_thread.join();
    pub_thread.join();

    Log::Reset();
    return ret;
}