Weight/readcard/Publisher.cxx

#include "Publisher.hpp"

#include <condition_variable>
#include <csignal>
#include <stdexcept>
#include <thread>

#include <sstream>
#include <iomanip>

#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
#include <fastdds/dds/log/Log.hpp>
#include <fastdds/dds/publisher/DataWriter.hpp>
#include <fastdds/dds/publisher/Publisher.hpp>
#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
#include <fastdds/dds/publisher/qos/PublisherQos.hpp>


#include <fastdds/dds/subscriber/DataReaderListener.hpp>
#include <fastdds/dds/subscriber/Subscriber.hpp>
#include <fastdds/dds/subscriber/qos/DataReaderQos.hpp>
#include <fastdds/dds/subscriber/qos/SubscriberQos.hpp>
//#include <fastrtps/types/TypesBase.h>
#include <fastdds/rtps/common/Types.hpp>
#include <fastdds/dds/core/Types.hpp>
#include <fastdds/dds/subscriber/SampleInfo.hpp>
#include <fastdds/dds/subscriber/DataReader.hpp>

#include "WeighingDDSTypePubSubTypes.hpp"

#include "msg.hpp"
#include "MsgHandler.hpp"
#include "DEBUG.hpp"

using namespace eprosima::fastdds::dds;

PublisherApp::PublisherApp(
        const int& domain_id)
    : factory_(nullptr)
    , participant_(nullptr)
    , publisher_(nullptr)
    , topic_(nullptr)
    , writer_(nullptr)
    , type_(new WeighingSystem::ReadCardRspPubSubType())
    , matched_(0)
    , samples_sent_(0)
    , stop_(false)
    , scale_type_(new WeighingSystem::ScaleInfoPubSubType())
    , summary_type_(new WeighingSystem::SummaryUpdatePubSubType())
    , infrared_subscriber_(nullptr)
    , infrared_topic_(nullptr)
    , infrared_reader_(nullptr)
    , infrared_type_(new WeighingSystem::InfraredUpdatePubSubType())
    , summary_pending_(false)
{
    // Create the participant
    DomainParticipantQos pqos = PARTICIPANT_QOS_DEFAULT;
    pqos.name("ReadCard_pub_participant");
    pqos.wire_protocol().builtin.discovery_config.leaseDuration = Duration_t(60, 0);
    pqos.wire_protocol().builtin.discovery_config.leaseDuration_announcementperiod = Duration_t(30, 0);
    factory_ = DomainParticipantFactory::get_shared_instance();
    participant_ = factory_->create_participant(domain_id, pqos, nullptr, StatusMask::none());
    if (participant_ == nullptr)
    {
        throw std::runtime_error("InternalSystem::ReadCardRsp Participant initialization failed");
    }

    // Register the type
    type_.register_type(participant_);

    // Create the publisher
    PublisherQos pub_qos = PUBLISHER_QOS_DEFAULT;
    participant_->get_default_publisher_qos(pub_qos);
    publisher_ = participant_->create_publisher(pub_qos, nullptr, StatusMask::none());
    if (publisher_ == nullptr)
    {
        throw std::runtime_error("InternalSystem::ReadCardRsp Publisher initialization failed");
    }

    // Create the topic
    TopicQos topic_qos = TOPIC_QOS_DEFAULT;
    participant_->get_default_topic_qos(topic_qos);
    topic_ = participant_->create_topic("ReadCardRsp", type_.get_type_name(), topic_qos);
    if (topic_ == nullptr)
    {
        throw std::runtime_error("InternalSystem::ReadCardRsp Topic initialization failed");
    }

    // Create the data writer
    DataWriterQos writer_qos = DATAWRITER_QOS_DEFAULT;
    publisher_->get_default_datawriter_qos(writer_qos);
    writer_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
    writer_qos.durability().kind = DurabilityQosPolicyKind::VOLATILE_DURABILITY_QOS;
    writer_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
    writer_ = publisher_->create_datawriter(topic_, writer_qos, this, StatusMask::all());
    if (writer_ == nullptr)
    {
        throw std::runtime_error("InternalSystem::ReadCardRsp DataWriter initialization failed");
    }

    // ── 创建 ScaleInfo 订阅者 ──
    scale_type_ = TypeSupport(new WeighingSystem::ScaleInfoPubSubType());
    scale_type_.register_type(participant_);

    SubscriberQos scale_sub_qos = SUBSCRIBER_QOS_DEFAULT;
    participant_->get_default_subscriber_qos(scale_sub_qos);
    scale_subscriber_ = participant_->create_subscriber(scale_sub_qos, nullptr, StatusMask::none());

    TopicQos scale_topic_qos = TOPIC_QOS_DEFAULT;
    participant_->get_default_topic_qos(scale_topic_qos);
    scale_topic_ = participant_->create_topic("ScaleInfo", scale_type_.get_type_name(), scale_topic_qos);

    DataReaderQos scale_reader_qos = DATAREADER_QOS_DEFAULT;
    scale_subscriber_->get_default_datareader_qos(scale_reader_qos);
    scale_reader_qos.reliability().kind = RELIABLE_RELIABILITY_QOS;
    scale_reader_qos.durability().kind = VOLATILE_DURABILITY_QOS;
    scale_reader_ = scale_subscriber_->create_datareader(
        scale_topic_, scale_reader_qos, this, StatusMask::all());

    // ── 创建 SummaryUpdate 发布者 ──
    summary_type_ = TypeSupport(new WeighingSystem::SummaryUpdatePubSubType());
    summary_type_.register_type(participant_);

    PublisherQos summary_pub_qos = PUBLISHER_QOS_DEFAULT;
    participant_->get_default_publisher_qos(summary_pub_qos);
    summary_publisher_ = participant_->create_publisher(summary_pub_qos, nullptr, StatusMask::none());

    TopicQos summary_topic_qos = TOPIC_QOS_DEFAULT;
    participant_->get_default_topic_qos(summary_topic_qos);
    summary_topic_ = participant_->create_topic("SummaryUpdate", summary_type_.get_type_name(), summary_topic_qos);

    DataWriterQos summary_writer_qos = DATAWRITER_QOS_DEFAULT;
    summary_publisher_->get_default_datawriter_qos(summary_writer_qos);
    summary_writer_qos.reliability().kind = RELIABLE_RELIABILITY_QOS;
    summary_writer_qos.durability().kind = VOLATILE_DURABILITY_QOS;
    summary_writer_ = summary_publisher_->create_datawriter(
        summary_topic_, summary_writer_qos, this, StatusMask::all());



    // ── 创建 InfraredUpdate 订阅者 ──
    infrared_type_ = TypeSupport(new WeighingSystem::InfraredUpdatePubSubType());
    infrared_type_.register_type(participant_);

    SubscriberQos ir_sub_qos = SUBSCRIBER_QOS_DEFAULT;
    participant_->get_default_subscriber_qos(ir_sub_qos);
    infrared_subscriber_ = participant_->create_subscriber(ir_sub_qos, nullptr, StatusMask::none());

    TopicQos ir_topic_qos = TOPIC_QOS_DEFAULT;
    participant_->get_default_topic_qos(ir_topic_qos);
    infrared_topic_ = participant_->create_topic("InfraredUpdate", infrared_type_.get_type_name(), ir_topic_qos);

    DataReaderQos ir_reader_qos = DATAREADER_QOS_DEFAULT;
    infrared_subscriber_->get_default_datareader_qos(ir_reader_qos);
    ir_reader_qos.reliability().kind = RELIABLE_RELIABILITY_QOS;
    ir_reader_qos.durability().kind   = VOLATILE_DURABILITY_QOS;
    infrared_reader_ = infrared_subscriber_->create_datareader(
        infrared_topic_, ir_reader_qos, this, StatusMask::all());

    // 缓存 topic 名（fastdds 的 DataReader 没有 get_topic）   ← 新增
    {
        std::lock_guard<std::mutex> lk(reader_topic_mtx_);
        scale_topic_name_    = scale_topic_->get_name();
        infrared_topic_name_ = infrared_topic_->get_name();
    }

}

PublisherApp::~PublisherApp()
{
    if (nullptr != participant_)
    {
        // Delete DDS entities contained within the DomainParticipant
        participant_->delete_contained_entities();

        // Delete DomainParticipant
        factory_->delete_participant(participant_);
    }
}

void PublisherApp::on_publication_matched(
        DataWriter* writer,
        const PublicationMatchedStatus& info)
{
    if (info.current_count_change == 1)
    {
        {
            std::lock_guard<std::mutex> lock(mutex_);
            matched_ = info.current_count;
        }
        DEBUG(writer->get_topic()->get_name() << " Publisher matched.");
        cv_.notify_one();
    }
    else if (info.current_count_change == -1)
    {
        {
            std::lock_guard<std::mutex> lock(mutex_);
            matched_ = info.current_count;
        }
        DEBUG(writer->get_topic()->get_name() << " Publisher unmatched.");
    }
    else
    {
        DEBUG(info.current_count_change << " is not a valid value for PublicationMatchedStatus current count change");
    }
}

// void PublisherApp::run(std::shared_ptr<MsgHandler> handler)
// {
//     InternalSystem::ReadCardRsp rsp;

//     while (!is_stopped())
//     {
//         if (handler->isOpen() == true)
//         {
//             if (handler->HandleDeviceMsg(rsp))
//             {
//                 writer_->write(&rsp);
//                 rsp.msg().clear();
//             }
//             handler->AutoFindTick();
//         }

//         // Wait for period or stop event
//         std::unique_lock<std::mutex> period_lock(mutex_);
//         cv_.wait_for(period_lock, std::chrono::milliseconds(period_ms_), [this]()
//                 {
//                     return is_stopped();
//                 });
//     }
// }

void PublisherApp::run(std::shared_ptr<MsgHandler> handler)
{
    WeighingSystem::ReadCardRsp rsp;

    while (!is_stopped())
    {
        if (handler->isOpen() == true)
        {
            // ========== 新增：检查单块操作超时 ==========
            // 修复：必须在 WAIT_DDS_READ 状态 且真的在读块过程中才允许超时跳过
            //原因：weight 判断阶段（m_CurrentReadIndex=0, m_RequestedBlocks 空）时不该触发 SkipCurrentBlock
            if (handler->GetCardState() == CardState::WAIT_DDS_READ
                && handler->IsBlockOperationPending()
                && handler->GetBlockOpElapsed() >= MsgHandler::BLOCK_OPERATION_TIMEOUT_MS
                && !handler->m_IsInTimeoutRecovery)   // 【新增】不在恢复中才检查
            {
                DEBUG("Block operation timeout elapsed_ms=" << handler->GetBlockOpElapsed()
                      << " limit_ms=" << MsgHandler::BLOCK_OPERATION_TIMEOUT_MS
                      << " block=" << static_cast<int>(handler->GetCurrentOperatingBlock()));
                handler->SkipCurrentBlock();
            }

            // 处理设备消息
            if (handler->HandleDeviceMsg(rsp))
            {
                rsp.index(handler->GetCurrentIndex());
                // ★ 用 count() 判断，不走 operator[]，不碰脏 rsp.msg()
                if (rsp.msg().count("read") > 0)
                {
                    DEBUG("Publishing DDS response: read success, index=" << rsp.index()
                                                                          << ", msg keys=" << rsp.msg().size()
                                                                          << ", read data size=" << rsp.msg()["read"].size());
                }
                else if (rsp.msg().count("write") > 0)
                {
                    DEBUG("Publishing DDS response: write success, index=" << rsp.index()
                                                                           << ", msg keys=" << rsp.msg().size()
                                                                           << ", write data size=" << rsp.msg()["write"].size());
                }
                writer_->write(&rsp);
                rsp.msg().clear();



            }

            // ========== 新增：如果有待发送的读卡响应，立即发布 ==========
            if (handler->HasPendingReadResponse())
            {
                auto pendingRsp = handler->GetPendingReadResponse();
                if (!pendingRsp.empty())
                {
                    rsp.msg()["read"] = pendingRsp;
                    rsp.index(handler->GetCurrentIndex());
                    DEBUG("Publishing pending read response (from timeout skip)");
                    writer_->write(&rsp);
                    rsp.msg().clear();
                }
            }

            // ========== 检查 DDS 超时 ==========
            if (handler->CheckDdsTimeout())
            {
                // 超时触发了状态切换，重新启动寻卡
                DEBUG("DDS timeout, auto-find restored");
            }

            // ========== 检查是否需要发布 SummaryUpdate ==========
            CheckAndPublishSummary(handler);


            // ========== 【新增】AUTO_FIND 异常重置 ==========
            // 检测 TryExtractDeviceFrame / RecvMsg 置位的 m_AutoFindNeedsReset。
            // 和你 WAIT_DDS_WRITE 超时的恢复走同一个套路：清串口 + 重置 RC522 + 恢复 AUTO_FIND。
            if (handler->ConsumeAutoFindNeedsReset())
            {
                DEBUG("AUTO_FIND recovery: invoking ResetSerialAndReinit due to CRC/timeout failure");
                handler->SetAutoFind(false);
                handler->ClearPendingCommand();
                // 清串口残留帧（避免下个循环再拿到 5 字节尾巴）
                // 注意：m_ReceiveBuffer 是 private，需要走 ClearReadBuffer()
                handler->ClearReadBuffer();
                // tcflush 走 ResetSerialAndReinit 内部已经做了，这里不必再调
                if (handler->ResetSerialAndReinit())
                {
                    DEBUG("AUTO_FIND recovery: succeeded, resuming");
                    	
                    handler->SetAutoFind(true);
                    handler->SetFindIntervalMs(5000);
                    handler->SetLastFindTimeToNow();
                    handler->SetCardState(CardState::AUTO_FIND);
                }
                else
                {
                    DEBUG("AUTO_FIND recovery: failed, will retry on next loop");
                    	
                    handler->SetAutoFind(true);
                    handler->SetFindIntervalMs(5000);
                    handler->SetLastFindTimeToNow();
                    handler->SetCardState(CardState::AUTO_FIND);
                }
            }


            // 自动寻卡
            handler->AutoFindTick();
        }

        std::unique_lock<std::mutex> period_lock(mutex_);
        cv_.wait_for(period_lock, std::chrono::milliseconds(10), [this]()
                     {
                         return is_stopped();
                     });
    }
}

bool PublisherApp::is_stopped()
{
    return stop_.load();
}


// void PublisherApp::on_data_available(
//     DataReader* reader)
// {
//     SampleInfo info;
//     WeighingSystem::ScaleInfo sample;

//     while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample, &info)))
//     {
//         if (info.instance_state == ALIVE_INSTANCE_STATE && info.valid_data)
//         {
//             std::unique_lock<std::mutex> lock(MsgData::queue_cv_mtx_);
//             MsgData::ScaleInfo_queue_.push(std::move(sample));
//             lock.unlock();
//         }
//     }
// }
void PublisherApp::on_data_available(DataReader* reader)
{
    SampleInfo info;

    std::string topic_name;
    {
        std::lock_guard<std::mutex> lk(reader_topic_mtx_);
        if (reader == scale_reader_)         topic_name = scale_topic_name_;
        else if (reader == infrared_reader_) topic_name = infrared_topic_name_;
    }

    if (topic_name == "ScaleInfo")
    {
        WeighingSystem::ScaleInfo sample;
        while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample, &info)))
        {
            if (info.instance_state == ALIVE_INSTANCE_STATE && info.valid_data)
            {
                std::unique_lock<std::mutex> lock(MsgData::queue_cv_mtx_);
                MsgData::ScaleInfo_queue_.push(std::move(sample));
                lock.unlock();
            }
        }
    }
    else if (topic_name == "InfraredUpdate")
    {
        WeighingSystem::InfraredUpdate sample;
        while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample, &info)))
        {
            if (info.instance_state == ALIVE_INSTANCE_STATE && info.valid_data)
            {
                std::lock_guard<std::mutex> lock(MsgData::infrared_mtx_);
                MsgData::front_blocked_    = sample.FrontResistanceSignal();
                MsgData::back_blocked_     = sample.BackResistanceSignal();
                MsgData::infrared_ready_   = true;
            }
        }
    }
}




// void PublisherApp::on_subscription_matched(
//     DataReader* reader,
//     const SubscriptionMatchedStatus& info)
// {
//     if (info.current_count_change == 1)
//     {
//         DEBUG(reader->get_topicdescription()->get_name() << " Subscriber matched.");
//     }
//     else if (info.current_count_change == -1)
//     {
//         DEBUG(reader->get_topicdescription()->get_name() << " Subscriber unmatched.");
//     }
// }
void PublisherApp::on_subscription_matched(DataReader* reader, const SubscriptionMatchedStatus& info)
{
    std::string topic_name;
    {
        std::lock_guard<std::mutex> lk(reader_topic_mtx_);
        if (reader == scale_reader_)         topic_name = scale_topic_name_;
        else if (reader == infrared_reader_) topic_name = infrared_topic_name_;
    }

    if (info.current_count_change == 1)
    {
        DEBUG(topic_name << " Subscriber matched.");
    }
    else if (info.current_count_change == -1)
    {
        DEBUG(topic_name << " Subscriber unmatched.");
    }
}





void PublisherApp::stop()
{
    stop_.store(true);
    cv_.notify_one();
}


bool PublisherApp::is_infrared_blocked()
{
    std::lock_guard<std::mutex> lock(MsgData::infrared_mtx_);

    // 还没收到过红外消息 → 当作未遮挡（不阻塞发卡）
    if (!MsgData::infrared_ready_)
    {
        return false;
    }

    // 只检查前红外（0=无遮挡，1=有遮挡）
    return MsgData::front_blocked_;
}


void PublisherApp::PublishSummaryUpdate(const std::string& cardId, float weightTon)
{
    std::lock_guard<std::mutex> lock(summary_mtx_);
    pending_card_id_ = cardId;
    summary_pending_ = true;

    // 注意：实际发布需要在 run 循环中执行，以避免线程问题
    // 这里只设置标志
}

void PublisherApp::CheckAndPublishSummary(std::shared_ptr<MsgHandler> handler)
{
    std::lock_guard<std::mutex> lock(summary_mtx_);
    if (summary_pending_)
    {
        // 从 ScaleInfo 队列获取最后一帧的实时重量
        float weightTon = default_weight_;
        float currentValue = default_weight_;
        bool hasNewFrame = false;

        // 检查 ScaleInfo 队列是否有新数据
        {
            std::lock_guard<std::mutex> scale_lock(MsgData::queue_cv_mtx_);

            // 只取队列最后一帧（最新的一帧）
            while (!MsgData::ScaleInfo_queue_.empty())
            {
                WeighingSystem::ScaleInfo scaleInfo = std::move(MsgData::ScaleInfo_queue_.front());
                MsgData::ScaleInfo_queue_.pop();

                currentValue = scaleInfo.Value();
                hasNewFrame = true;

                DEBUG("Queue check: Value=" << currentValue
                                            << ", StableValue=" << scaleInfo.StableValue());
            }

            // 如果有新帧，添加到历史记录
            if (hasNewFrame)
            {
                recentValues_.push_back(currentValue);

                // 限制最多保留 MAX_RECENT_FRAMES_ 帧
                if (recentValues_.size() > MAX_RECENT_FRAMES_)
                {
                    recentValues_.erase(
                        recentValues_.begin(),
                        recentValues_.end() - MAX_RECENT_FRAMES_
                        );
                }
                DEBUG("History updated: size=" << recentValues_.size()
                                               << ", last=" << recentValues_.back());
            }
        }

        // ===== 稳定判断：最近3帧都相同 =====
        has_weight_ = false;
        if (recentValues_.size() >= 3)
        {
            // 取最后3帧
            float v0 = recentValues_[recentValues_.size() - 3];
            float v1 = recentValues_[recentValues_.size() - 2];
            float v2 = recentValues_[recentValues_.size() - 1];

            // 判断最后3帧是否都相同（保留2位小数比较）
            v0 = std::round(v0 * 100.0f) / 100.0f;
            v1 = std::round(v1 * 100.0f) / 100.0f;
            v2 = std::round(v2 * 100.0f) / 100.0f;

            if (v0 == v1 && v1 == v2)
            {
                has_weight_ = true;
                weightTon = currentValue;
                DEBUG("Weight stable: last 3 frames all same = " << weightTon
                                                                 << " (frames: " << v0 << ", " << v1 << ", " << v2 << ")");
            }
            else
            {
                DEBUG("Weight not stable yet: last 3 frames = "
                      << v0 << ", " << v1 << ", " << v2);
            }
        }
        else
        {
            DEBUG("Collecting weight data... (" << recentValues_.size() << "/3 frames)");
        }

        // ===== 新增：必须获取到有效稳定重量才发布 =====
        if (!has_weight_)
        {
            DEBUG("Waiting for stable weight...");
            return;  // 等待下次检查
        }

        // ===== 新增：红外对射必须无遮挡 =====
        if (is_infrared_blocked())
        {
            DEBUG("Waiting for infrared unblocked (front_blocked_="
                  << MsgData::front_blocked_ << ")");
            return;  // 等待下次检查
        }

        WeighingSystem::SummaryUpdate update;

        std::string cardIdDecimal = pending_card_id_;

        // 保留2位小数
        weightTon = std::round(weightTon * 100.0f) / 100.0f;

        update.License(cardIdDecimal);

        //update.License(pending_card_id_);
        update.StableValue(weightTon);

        // ===== 打印转换后的卡号用于调试 =====
        DEBUG("Card ID decimal: " << cardIdDecimal);

        summary_writer_->write(&update);
        DEBUG("Published SummaryUpdate: License=" << pending_card_id_
                                                  << ", StableValue=" << weightTon << "吨");

        // ★ 新增：SummaryUpdate 发完后立刻停 RF-feed，避免后续 DDS 读写时 RF 干扰
        handler->SetAutoFind(false);           // 关掉定时喂 RF
        //handler_->StopCard();                   // 主动给 RC522 发停卡命令（让 RC522 释放 Crypto1）

        summary_pending_ = false;
        recentValues_.clear();  // 新增：清空历史帧，避免下一辆车使用上一辆车的数据
    }
}

void PublisherApp::RestoreAutoFind()
{
    // 这个方法可以在写卡完成后调用，恢复自动寻卡
    DEBUG("Restoring auto-find mode");
}