#include "traffic_light_control.hpp"
#include "modbus/crc16-modbus.hpp"
#include "3th-part/json.hpp"
#include <sstream>

#include <chrono>
#include <cstdint>

uint64_t get_current_timestamp()
{
    auto now = std::chrono::system_clock::now();
    auto duration_since_epoch = now.time_since_epoch();
    auto millis_since_epoch = std::chrono::duration_cast<std::chrono::milliseconds>(duration_since_epoch).count();
    return static_cast<uint64_t>(millis_since_epoch);
}

TrafficLightControl::TrafficLightControl(
    const std::string& traffic_states_topic,
    const std::map<std::string, roadside_info_config>& roadside_info_config_map,
    const std::string& light_control_topic
    )
    : __m_running(true),
      __m_traffic_states_topic(traffic_states_topic), 
      __m_roadside_info_map(roadside_info_config_map),
      __m_light_control_topic(light_control_topic)
{
    init();
}

void TrafficLightControl::exec()
{
    __m_running = true;
    while (__m_running) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

TrafficLightControl::~TrafficLightControl()
{
    __m_running = false;
    if (__m_resp_thread) {
        __m_resp_thread->join();
        __m_resp_thread.reset();
    }
}

void printf_hex(const std::vector<uint8_t>& data)
{
    std::ostringstream ss;
    for (auto& v : data) {
        ss << "0x" << std::hex << (uint32_t)v << " ";
    }
    std::cout << ss.str() << std::endl;
}

void TrafficLightControl::init()
{

    for (auto it : __m_roadside_info_map) {
        __m_traffic_control_pubers.insert(
            { it.first, std::make_shared<mqtt_dataio::publisher<std::vector<uint8_t>>>("/" + it.first + "/self_control/request") });


        auto traffic_status_sub_topic = "/" + it.first + "/self_status/report";
        __m_traffic_control_subers.insert(
            { it.first, std::make_shared<mqtt_dataio::subscriber<std::vector<uint8_t>>>(traffic_status_sub_topic,
            std::bind(&TrafficLightControl::on_traffic_light_feedback, this, std::placeholders::_1)) });

        std::cout << "traffic id: " << it.first << ", status topic: " << traffic_status_sub_topic << std::endl;

        fleet::RoadsideInfo item;
        item.set_id(it.second.id);
        item.set_altitude(it.second.altitude);
        item.set_longitude(it.second.longitude);
        item.set_latitude(it.second.latitude);
        item.set_islight(true);
        item.set_lightstatus(fleet::LightStatus::UNKNOWN_STATUS_1);

        __m_roadsize_info.insert({ it.first, item });
    }

    __m_light_control_suber = std::make_shared<mqtt_dataio::subscriber<fleet::LightControl>>(
        __m_light_control_topic, [=](const fleet::LightControl& data) {
            std::string road_id = data.id();
            auto roadside_info = __m_roadside_info_map.find(road_id);
            if (roadside_info == __m_roadside_info_map.end()) {
                std::cout << "no match road id " << road_id << " do nothing" << std::endl;
                return;
            }

            auto light_status = data.lightstatus();
            uint64_t timestamp = data.timestamp();
            std::cout << "---------------start--------------" << std::endl;
            std::cout << "road_id: " << road_id << " light_status: " << (int)light_status
                        << " update_time: " << timestamp << std::endl;
            auto it = __m_traffic_control_pubers.find(road_id);
            if (it != __m_traffic_control_pubers.end()) {

                nlohmann::json j = nlohmann::json::object();
                j.emplace("id", it->first);
                j.emplace("status", cvt_light_status(light_status));
                auto str = j.dump();
                std::vector<uint8_t> control_data;
                control_data.insert(control_data.end(), str.begin(), str.end());
                it->second->publish(control_data);
                printf_hex(control_data);
            } else {
                std::cout << "not find match puber: " << it->first << std::endl;
            }

            std::cout << "---------------end--------------" << std::endl;
        });


    // initilize traffic light states service
    __m_roadside_info_puber = std::make_shared<mqtt_dataio::publisher<fleet::RoadsideInfoList>>(
        __m_traffic_states_topic
    );
    __m_resp_thread = std::make_shared<std::thread>([=]() {
        std::cout << "traffic light states feedback service start" << std::endl;
        while(__m_running)
        {
            // execute once per second
            auto start = std::chrono::steady_clock::now();
            {
                std::lock_guard<std::mutex> lock(__m_roadside_info_mtx);
                fleet::RoadsideInfoList status_list;
                for (auto it : __m_roadsize_info) {
                    auto item = status_list.add_roadsidelist();
                    item->CopyFrom(it.second);
                }
                __m_roadside_info_puber->publish(status_list);
            }
            auto end = std::chrono::steady_clock::now();
            std::chrono::duration<double, std::milli> diff = end - start;
            if (diff.count() < 1000) {
                std::this_thread::sleep_for(std::chrono::milliseconds(1000 - (int)diff.count()));
            }
        }
        std::cout << "traffic light states feedback service stop" << std::endl;
    });
}

void TrafficLightControl::on_traffic_light_feedback(const std::vector<uint8_t>& msg)
{
    std::string str = std::string(msg.begin(), msg.end());
    std::cout << "recv traffic light feedback: " << str << std::endl;
    nlohmann::json j = nlohmann::json::parse(str);

    std::cout << j.dump() << std::endl;

    std::string road_id = j["self_id"];
    auto it = __m_roadsize_info.find(road_id);
    if (it == __m_roadsize_info.end()) {
        std::cout << "no match road id " << road_id << " do nothing" << std::endl;
        return;
    }
    std::string st_str = j["status"];
    auto s = cvt_light_status(st_str);
    it->second.set_lightstatus(s);
}

std::string TrafficLightControl::cvt_light_status(const fleet::LightStatus &status)
{
    if(status == fleet::LightStatus::LIGHT_RED)
    {
        return "red";
    }
    else if(status == fleet::LightStatus::LIGHT_GREEN)
    {
        return "green";
    }
    else if(status == fleet::LightStatus::LIGHT_YELLOW)
    {
        return "yellow";
    }
    return "all_off";
}
fleet::LightStatus TrafficLightControl::cvt_light_status(const std::string &status)
{
    std::string s = status;
    std::transform(s.begin(), s.end(), s.begin(), ::tolower);
    if(s == "all_off")
    {
        return fleet::LightStatus::UNKNOWN_STATUS_1;
    }
    else if(s == "red")
    {
        return fleet::LightStatus::LIGHT_RED;
    }
    else if(s == "green")
    {
        return fleet::LightStatus::LIGHT_GREEN;
    }
    else if(s == "yellow")
    {
        return fleet::LightStatus::LIGHT_YELLOW;
    }
    return fleet::LightStatus::UNKNOWN_STATUS_1;
}
