#include "acoustic_analyzer/ros/threat_publisher.h"
#include <acoustic_analyzer/AcousticThreat.h>

namespace acoustic {

struct ThreatPublisher::Impl {
    ros::Publisher pub_;
    std::size_t num_published_ = 0;
};

ThreatPublisher::ThreatPublisher(ros::NodeHandle& nh, const std::string& topic)
    : impl_(std::make_unique<Impl>()) {
    impl_->pub_ = nh.advertise<acoustic_analyzer::AcousticThreatArray>(topic, 10);
}

ThreatPublisher::~ThreatPublisher() = default;
ThreatPublisher::ThreatPublisher(ThreatPublisher&&) noexcept = default;
ThreatPublisher& ThreatPublisher::operator=(ThreatPublisher&&) noexcept = default;

void ThreatPublisher::Publish(const AcousticFrame& frame) {
    acoustic_analyzer::AcousticThreatArray msg;
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = "acoustic_array";

    for (const auto& t : frame.threats) {
        acoustic_analyzer::AcousticThreat threat_msg;
        threat_msg.threat_id = t.threat_id;
        threat_msg.sound_type = t.sound_type;
        threat_msg.confidence = t.confidence;
        threat_msg.azimuth = t.azimuth;
        threat_msg.elevation = t.elevation;
        threat_msg.distance = t.distance;
        threat_msg.distance_confidence = t.distance_confidence;
        msg.threats.push_back(threat_msg);
    }

    impl_->pub_.publish(msg);
    ++impl_->num_published_;
}

std::size_t ThreatPublisher::NumPublished() const noexcept {
    return impl_->num_published_;
}

} // namespace acoustic
