Queue/weigh/WeightStabilityDetector.cxx

#include "WeightStabilityDetector.hpp"

WeightStabilityDetector::WeightStabilityDetector(const Config& config) :
    config_(config),
    current_state_(VehicleState::NO_VEHICLE),
    current_weight_(0.0f),
    previous_weight_(0.0f),
    stable_weight_(0.0f),
    max_weight_(0.0f),
    current_window_size_(static_cast<size_t>(config.window_size)),
    vehicle_window_size_(static_cast<size_t>(config.window_size)),
    stable_count_(0),
    continuous_increase_count_(0),
    total_samples_(0),
    state_callback_(nullptr),
    weight_callback_(nullptr),
    state_callback_data_(nullptr),
    weight_callback_data_(nullptr),
    debug_output_(nullptr),
    is_initialized_(false),
    has_vehicle_on_(false),
    is_leaving_state_(false) {
    
    reset(false); // 初始化时不通知回调
    
    // 默认调试输出到标准输出
    debug_output_ = [](const std::string& msg) {
        std::cout << "[WeightDetector] " << msg << std::endl;
    };
}

WeightStabilityDetector::~WeightStabilityDetector() {
    // 清理资源
}

void WeightStabilityDetector::reset(bool notify_callback) {
    VehicleState old_state = current_state_;
    current_state_ = VehicleState::NO_VEHICLE;
    
    weight_window_.clear();
    vehicle_weight_window_.clear();
    weight_history_.clear();
    
    current_weight_ = 0.0f;
    previous_weight_ = 0.0f;
    stable_weight_ = 0.0f;
    max_weight_ = 0.0f;
    
    stable_count_ = 0;
    continuous_increase_count_ = 0;
    total_samples_ = 0;
    
    stats_ = Statistics{};
    has_vehicle_on_ = false;
    is_leaving_state_ = false;
    
    last_state_change_time_ = std::chrono::steady_clock::now();
    last_weight_time_ = std::chrono::steady_clock::now();
    
    is_initialized_ = true;
    
    // 通知状态变化
    if (notify_callback && state_callback_ && old_state != current_state_) {
        state_callback_(current_state_, 0.0f, state_callback_data_);
    }
    
    debugLog("Detector reset");
}

void WeightStabilityDetector::setConfig(const Config& config) {
    config_ = config;
    current_window_size_ = static_cast<size_t>(config.window_size);
    vehicle_window_size_ = static_cast<size_t>(config.window_size);
    
    // 如果窗口大小改变，需要调整窗口
    if (weight_window_.size() > current_window_size_) {
        while (weight_window_.size() > current_window_size_) {
            weight_window_.pop_front();
        }
    }
    
    if (vehicle_weight_window_.size() > vehicle_window_size_) {
        while (vehicle_weight_window_.size() > vehicle_window_size_) {
            vehicle_weight_window_.pop_front();
        }
    }
    
    // 重新计算统计信息
    calculateVehicleStatistics();
}

bool WeightStabilityDetector::processWeight(float weight_kg) {
    return processWeightData(WeightData(weight_kg));
}

bool WeightStabilityDetector::processWeightData(const WeightData& data) {
    if (!data.is_valid) {
        debugLog("Invalid weight data received");
        return false;
    }
    
    // 保存旧状态用于比较
    VehicleState old_state = current_state_;
    
    // 更新时间戳
    last_weight_time_ = data.timestamp;
    
    // 更新重量值
    previous_weight_ = current_weight_;
    current_weight_ = data.value_kg;
    
    // 记录最大重量（非离开状态时）
    if (!is_leaving_state_ && current_weight_ > max_weight_) {
        max_weight_ = current_weight_;
    }
    
    // 添加到历史记录
    addWeightToHistory(current_weight_);
    
    // 更新全局滑动窗口
    updateWindow(current_weight_);
    
    // 如果不是离开状态，检查车辆状态
    if (!is_leaving_state_) {
        if (isVehicleOn(current_weight_) && !has_vehicle_on_) {
            has_vehicle_on_ = true;
            if (config_.reset_window_on_vehicle_on) {
                vehicle_weight_window_.clear();  // 车辆上磅时清空车辆重量窗口
                max_weight_ = current_weight_;   // 重置最大重量
            }
            debugLog("Vehicle on scale detected, clearing vehicle weight window");
        }
        
        // 如果有车在磅上，更新车辆重量窗口
        if (has_vehicle_on_ && current_weight_ > config_.empty_car_threshold) {
            updateVehicleWindow(current_weight_);
        } else if (current_weight_ < config_.empty_car_threshold) {
            // 如果重量低于空车阈值，重置车辆状态
            has_vehicle_on_ = false;
            vehicle_weight_window_.clear();
        }
    }
    
    // 计算统计信息（基于车辆重量窗口）
    // if (!vehicle_weight_window_.empty()) {
        calculateVehicleStatistics();
    // }
    
    // 检查状态转换
    checkStateTransition(current_weight_);
    
    // 触发回调
    bool state_changed = (old_state != current_state_);
    
    if (state_callback_ && state_changed) {
        state_callback_(current_state_, current_weight_, state_callback_data_);
    }
    
    if (weight_callback_) {
        weight_callback_(current_weight_, isStable(), weight_callback_data_);
    }
    
    // 调试输出
    if (config_.enable_debug_log) {
        std::ostringstream oss;
        oss << std::fixed << std::setprecision(2)
            << "Weight: " << current_weight_ << "kg, "
            << "State: " << static_cast<int>(current_state_) << ", "
            << "StableCount: " << stable_count_ << ", "
            << "VehicleWindow: " << vehicle_weight_window_.size() << ", "
            << "StdDev: " << stats_.std_dev << "kg, "
            << "MaxWeight: " << max_weight_ << "kg";
        debugLog(oss.str());
    }
    
    return true;
}

void WeightStabilityDetector::updateWindow(float weight_kg) {
    weight_window_.push_back(weight_kg);
    
    // 保持窗口大小
    if (weight_window_.size() > current_window_size_) {
        weight_window_.pop_front();
    }
}

void WeightStabilityDetector::updateVehicleWindow(float weight_kg) {
    vehicle_weight_window_.push_back(weight_kg);
    
    // 保持窗口大小
    if (vehicle_weight_window_.size() > vehicle_window_size_) {
        vehicle_weight_window_.pop_front();
    }
}

void WeightStabilityDetector::calculateVehicleStatistics() {
    // 使用车辆重量窗口计算统计信息
    if (vehicle_weight_window_.empty()) {
        stats_ = Statistics{};
        return;
    }
    
    // 计算基本统计量
    size_t n = vehicle_weight_window_.size();
    
    // 计算最小值和最大值
    stats_.min = *std::min_element(vehicle_weight_window_.begin(), vehicle_weight_window_.end());
    stats_.max = *std::max_element(vehicle_weight_window_.begin(), vehicle_weight_window_.end());
    
    // 计算平均值
    float sum = std::accumulate(vehicle_weight_window_.begin(), vehicle_weight_window_.end(), 0.0f);
    stats_.mean = sum / static_cast<float>(n);
    
    // 计算方差和标准差
    float variance_sum = 0.0f;
    for (float w : vehicle_weight_window_) {
        float diff = w - stats_.mean;
        variance_sum += diff * diff;
    }
    stats_.variance = variance_sum / static_cast<float>(n);
    stats_.std_dev = std::sqrt(stats_.variance);
    
    // 更新计数器
    stats_.sample_count = static_cast<int>(n);
    stats_.stable_count = stable_count_;
    stats_.increase_count = continuous_increase_count_;
}

void WeightStabilityDetector::checkStateTransition(float weight_kg) {
    switch (current_state_) {
        case VehicleState::NO_VEHICLE:
            if (isVehicleOn(weight_kg)) {
                transitionToState(VehicleState::VEHICLE_ON);
            }
            break;
            
        case VehicleState::VEHICLE_ON: {
            // 检查重量抖动
            float weight_diff = std::abs(weight_kg - previous_weight_);
            
            if (isJitterExceeded(weight_diff)) {
                // 抖动过大，重置计数
                stable_count_ = 0;
                continuous_increase_count_ = 0;
                
                debugLog("Jitter exceeded, resetting stability counters");
            } else if (weight_kg > config_.min_weight_threshold) {
                // 增加稳定计数
                stable_count_++;
                
                // 检查是否连续增长
                if (weight_kg > previous_weight_) {
                    continuous_increase_count_++;
                } else {
                    continuous_increase_count_ = 0;
                }
                
                // 检查是否达到稳定条件
                if (checkStableCondition()) {
                    transitionToState(VehicleState::VEHICLE_STABLE);
                }
            }
            
            // 检查是否离开（包括快速下降）
            if (isVehicleLeaving(weight_kg) || checkFastDrop(weight_kg)) {
                transitionToState(VehicleState::VEHICLE_LEAVING);
            }
            break;
        }
            
        case VehicleState::VEHICLE_STABLE: {
            // 稳定状态下继续监测是否离开
            if (isVehicleLeaving(weight_kg) || checkFastDrop(weight_kg)) {
                transitionToState(VehicleState::VEHICLE_LEAVING);
            }
            else if (isVehicleShaking(weight_kg)) {
                transitionToState(VehicleState::VEHICLE_ON);
            }
            break;
        }
            
        case VehicleState::VEHICLE_LEAVING: {
            // 离开状态持续一段时间后自动回到无车状态
            auto now = std::chrono::steady_clock::now();
            auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
                now - last_state_change_time_).count();
            
            if (elapsed > config_.leave_reset_delay_ms) {
                // 重置但不通知回调，由processWeight统一处理
                reset(false);
                // 状态已改变，需要在processWeight中触发回调
                // 这里不需要额外处理，因为reset(false)不会触发回调
                // 状态变化会在processWeight返回前被检测到并触发回调
            }
            break;
        }
            
        default:
            // 处理未知状态
            break;
    }
}

void WeightStabilityDetector::transitionToState(VehicleState new_state) {
    current_state_ = new_state;
    last_state_change_time_ = std::chrono::steady_clock::now();
    
    // 状态特定处理
    switch (new_state) {
        case VehicleState::VEHICLE_STABLE:
            stable_weight_ = stats_.mean; // 使用均值作为稳定重量
            debugLog("Vehicle stable, weight: " + std::to_string(stable_weight_) + "kg");
            break;
            
        case VehicleState::VEHICLE_LEAVING:
            vehicle_weight_window_.clear(); // 清空车辆重量窗口
            is_leaving_state_ = true;      // 标记为离开状态
            debugLog("Vehicle leaving detected");
            break;
            
        case VehicleState::NO_VEHICLE:
            is_leaving_state_ = false;     // 清除离开状态标记
            debugLog("No vehicle on scale");
            break;
            
        case VehicleState::VEHICLE_ON:
            is_leaving_state_ = false;     // 清除离开状态标记
            debugLog("Vehicle detected on scale");
            break;
            
        default:
            break;
    }
}

bool WeightStabilityDetector::isJitterExceeded(float weight_diff) const {
    return weight_diff > config_.jitter_threshold;
}

bool WeightStabilityDetector::isVehicleOn(float weight_kg) const {
    return weight_kg > config_.min_weight_threshold;
}

bool WeightStabilityDetector::isVehicleLeaving(float weight_kg) const {
    return weight_kg < config_.empty_car_threshold;
}

bool WeightStabilityDetector::isVehicleShaking(float weight_kg) {
    if (std::abs(weight_kg - previous_weight_) > config_.jitter_threshold)
    {
        is_leaving_state_ = false;      // 清除离开状态标记
        stable_count_ = 0;              // 重置稳定计数
        continuous_increase_count_ = 0; // 重置连续增长计数
        vehicle_weight_window_.clear(); // 重置重量窗口
        return true;
    }
    return false;
}

bool WeightStabilityDetector::checkFastDrop(float weight_kg) const {
    // 检查快速下降：当前重量远低于历史最高重量
    if (max_weight_ > config_.min_weight_threshold) {
        float drop_amount = max_weight_ - weight_kg;
        if (drop_amount > config_.fast_drop_threshold) {
            if (config_.enable_debug_log && debug_output_) {
                std::ostringstream oss;
                oss << "Fast drop detected: max=" << max_weight_ 
                    << "kg, current=" << weight_kg << "kg, drop=" << drop_amount << "kg";
                debug_output_(oss.str());
            }
            return true;
        }
    }
    return false;
}

bool WeightStabilityDetector::checkStableCondition() const {
    // 条件1：稳定次数达到要求
    if (stable_count_ < config_.required_stable_count) {
        return false;
    }
    
    // 条件2：车辆重量窗口至少有3个样本
    if (vehicle_weight_window_.size() < 3) {
        return false;
    }
    
    // 条件3：基于车辆重量的稳定性判断
    // 计算极差（最大值-最小值）
    float weight_range = stats_.max - stats_.min;
    
    // 条件3.1：标准差小于阈值
    bool std_dev_stable = (stats_.std_dev < config_.max_std_dev);
    
    // 条件3.2：极差小于2倍抖动阈值
    bool range_stable = (weight_range < (2.0f * config_.jitter_threshold));
    
    // 条件4：抖动次数要求
    int non_increase_count = stable_count_ - continuous_increase_count_;
    bool jitter_met = (non_increase_count >= config_.min_jitter_count);
    
    // 条件5：超时稳定
    bool timeout_stable = (stable_count_ >= (config_.required_stable_count + config_.timeout_extra));
    
    // 综合判断：统计稳定且（满足抖动要求或超时）
    bool is_stable = (std_dev_stable || range_stable) && (jitter_met || timeout_stable);
    
    if (config_.enable_debug_log && debug_output_) {
        std::ostringstream oss;
        oss << "Stable Check: std_dev=" << stats_.std_dev 
            << ", range=" << weight_range
            << ", non_increase=" << non_increase_count
            << ", stable_count=" << stable_count_
            << ", vehicle_window=" << vehicle_weight_window_.size()
            << ", result=" << (is_stable ? "STABLE" : "NOT_STABLE");
        debug_output_(oss.str());
    }
    
    return is_stable;
}

void WeightStabilityDetector::addWeightToHistory(float weight_kg) {
    weight_history_.push_back(weight_kg);
    total_samples_++;
    
    // 限制历史记录大小
    if (weight_history_.size() > MAX_HISTORY_SIZE) {
        weight_history_.erase(weight_history_.begin());
    }
}

void WeightStabilityDetector::debugLog(const std::string& message) const {
    if (config_.enable_debug_log && debug_output_) {
        debug_output_(message);
    }
}