#include "traffic_light.hpp"
#include "modbus/crc16-modbus.hpp"

#include "3th-part/json.hpp"
#include <sstream>
#include <chrono>
#include <cstdint>

// const std::vector<const uint8_t> all_on {0x01, 0x05, 0x00, 0x00, 0xFF, 0x00, 0x8C, 0x3A};
// const std::vector<const uint8_t> all_off {};
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& id, const std::string& uart)
  : self_id(id)
  , uart_name_(uart)
  , enable_sim_(false)
{
    init();
}

TrafficLightControl::TrafficLightControl(nlohmann::json& j)
  : self_id(j["self_id"])
  , uart_name_(j["uart"])
  , enable_sim_(j["enable_sim"])
{
    init();
}


void TrafficLightControl::exec()
{
    __m_running = true;
    uint8_t lamp_code = 0;
    std::string traffic_light_status = cvt_light_status(LightState::ALL_OFF);
    auto start = std::chrono::steady_clock::now();
    while (__m_running) {
        
        auto now = std::chrono::steady_clock::now();
        
        // publish traffic light states once per second
        if(std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 1000)
        {
            std::cout << "--------------------------------------" << std::endl;
            start = now;
            
            // send read input status request to relay
            {
                std::cout << "read relay input status..." << std::endl;
                std::vector<uint8_t> request_cmd = {0x01, 0x02, 0x00, 0x00, 0x00, 0x04};
                auto crc = tools::crc16_modbus::crc16_code(request_cmd);
                request_cmd.insert(request_cmd.end(), crc.begin(), crc.end());
                uart_->write(request_cmd.data(), request_cmd.size());
            }

            // publish traffic light states
            {
                nlohmann::json j = nlohmann::json::object();
                j.emplace("self_id", self_id);
                j.emplace("relay_code", lamp_code);
                if(enable_sim_)
                {
                    j.emplace("status", cvt_light_status(currnt_lamp_));
                }
                else
                {
                    j.emplace("status", traffic_light_status);
                }

                auto containts = j.dump();
                mqtt_dataio::subscriber_basic::DataBuffer buffer;
                buffer.resize(containts.size());
                std::copy(containts.begin(), containts.end(), buffer.begin());
                std::cout << "publish traffic light states: " << containts << std::endl;
                __m_status_puber->publish(buffer);
            }
        }
        // read data from uart port
        uint8_t buffer[1024];
        int len = uart_->read(buffer, sizeof(buffer));
        if(len > 0)
        {
            for(int i = 0; i < len; i++)
            {
                std::cout << std::hex << (uint32_t)buffer[i] << " ";
            }
            std::cout << std::endl;

            // check crc code
            auto crc = tools::crc16_modbus::crc16_code(std::vector<uint8_t>(buffer, buffer + len - 2));
            if(crc[0] == buffer[len - 2] && crc[1] == buffer[len - 1])
            {
                std::cout << "crc check ok!" << std::endl;

                uint8_t func_code = buffer[1];
                if(func_code == 0x02)
                {
                    lamp_code = buffer[len - 3];
                    traffic_light_status = cvt_light_status((LightState)lamp_code);
                    std::cout << "relay code: " << (int)lamp_code << ", traffic light status: " << traffic_light_status << std::endl;
                }
                else if(func_code == 0x0f)
                {
                    // TODO: this is relay control command ack package.
                }
                else
                {
                    std::cout << "unknown func code: " << (int)func_code << std::endl;
                }
            }
            else
            {
                std::cerr << "check relay status failed, crc error: " << buffer[len - 2] << ", " << buffer[len - 1] << ", should be: " << crc[0] << ", " << crc[1] << std::endl;
            }
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

TrafficLightControl::~TrafficLightControl()
{
    __m_running = false;
}

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()
{
    uart_ = std::make_shared<awe::Uart>(uart_name_.c_str());
    uart_->setOpt(115200, 8, 'N', 1);
    __m_light_register_puber = std::make_shared<mqtt_dataio::publisher<mqtt_dataio::subscriber_basic::DataBuffer>>("/traffic_light_controller/register");

    // regist self to traffic light controller
    nlohmann::json j = nlohmann::json::object();
    j.emplace("self_id", self_id);
    j.emplace("type", "traffic_light");

    mqtt_dataio::subscriber_basic::DataBuffer buffer;
    auto containts = j.dump();
    buffer.resize(containts.size());
    std::copy(containts.begin(), containts.end(), buffer.begin());
    std::cout << "publish traffic light register: " << containts << ", " << buffer.size() << std::endl;
    __m_light_register_puber->publish(buffer);

    // publish traffic light status
    __m_status_puber = std::make_shared<mqtt_dataio::publisher<mqtt_dataio::subscriber_basic::DataBuffer>>("/" + self_id + "/self_status/report");
    // subscribe control request
    __m_control_suber = std::make_shared<mqtt_dataio::subscriber<mqtt_dataio::subscriber_basic::DataBuffer>>("/" + self_id + "/self_control/request", 
        std::bind(&TrafficLightControl::on_control_request, this, std::placeholders::_1));

    // on start up, turn off all lights
    currnt_lamp_ = LightState::ALL_OFF;
    relay_control(0, currnt_lamp_);
}

void TrafficLightControl::on_control_request(const mqtt_dataio::subscriber_basic::DataBuffer& msg)
{
    std::string str;
    str.resize(msg.size());
    memcpy((void*)str.data(), msg.data(), msg.size());

    nlohmann::json j = nlohmann::json::parse(str);
    std::cout << "recv control request: " << j.dump() << std::endl;

    if(j["id"] != self_id)
    {
        std::cerr << "invalid id: " << j["id"] << ", self id: " << self_id << ", ignore" << std::endl;
        return;
    }

    std::string status = j["status"];
    LightState lamp = cvt_light_status(status);
    relay_control(0, lamp);

    std::cout << "change traffic light status from: " << cvt_light_status(currnt_lamp_) << " to " << cvt_light_status(lamp) << std::endl;
    currnt_lamp_ = lamp;
}

void TrafficLightControl::relay_control(const int channel, const LightState lamp)
{
    std::vector<uint8_t> relay_cmd = {0x01, 0x0F, 0x00, 0x00, 0x00, 0x04, 0x01, (uint8_t)lamp};
    auto pkg = tools::crc16_modbus::crc16_code(relay_cmd);
    relay_cmd.insert(relay_cmd.end(), pkg.begin(), pkg.end());
    std::cout << "send relay control cmd: ";
    for(auto& v : relay_cmd)
    {
        std::cout << std::hex << (uint32_t)v << " ";
    }
    std::cout << std::endl;
    uart_->write(relay_cmd.data(), relay_cmd.size());
}

const TrafficLightControl::LightState TrafficLightControl::cvt_light_status(const std::string& status)
{
    // lower status string
    std::string s = status;
    std::transform(s.begin(), s.end(), s.begin(), ::tolower);
    if(s == "all_off")
    {
        return LightState::ALL_OFF;
    }
    else if(s == "red")
    {
        return LightState::RED;
    }
    else if(s == "green")
    {
        return LightState::GREEN;
    }
    else if(s == "yellow")
    {
        return LightState::YELLOW;
    }
    else if(s == "all_on")
    {
        return LightState::ALL_ON;
    }
    return LightState::ALL_OFF;
}
const std::string TrafficLightControl::cvt_light_status(const LightState& status)
{
    switch(status)
    {
        case LightState::ALL_OFF:
            return "all_off";
        case LightState::RED:
            return "red";
        case LightState::GREEN:
            return "green";
        case LightState::YELLOW:
            return "yellow";
        case LightState::ALL_ON:
            return "all_on";
        default:
            return "all_off";
    }
    return "all_off";
}
