super_cereal 0.1.0

use crate::message::{
    down_topic_name, parse_channel_from_topic, SerialChunk, DEFAULT_HISTORY_DEPTH, TYPE_NAME,
    up_topic_name,
};
use anyhow::{Context, Result};
use rustdds::no_key::DataReader;
use rustdds::{
    policy::{Durability, History, Reliability},
    CDRDeserializerAdapter, CDRSerializerAdapter, DomainParticipant, QosPolicyBuilder, TopicKind,
};
use std::collections::BTreeSet;
use std::time::Duration;
use tokio::time::sleep;
use tracing::info;

pub type ChunkWriter =
    rustdds::no_key::DataWriter<SerialChunk, CDRSerializerAdapter<SerialChunk>>;
pub type ChunkReader = DataReader<SerialChunk, CDRDeserializerAdapter<SerialChunk>>;

pub struct DdsParticipant {
    pub participant: DomainParticipant,
}

impl DdsParticipant {
    pub fn new(domain_id: u16) -> Result<Self> {
        let participant = DomainParticipant::new(domain_id)
            .map_err(|e| anyhow::anyhow!("failed to create DDS participant: {e:?}"))?;
        Ok(Self { participant })
    }
}

pub struct DdsChannel {
    pub up_writer: ChunkWriter,
    pub down_writer: ChunkWriter,
    pub up_reader: ChunkReader,
    pub down_reader: ChunkReader,
}

pub fn channel_qos() -> rustdds::QosPolicies {
    QosPolicyBuilder::new()
        .reliability(Reliability::Reliable {
            max_blocking_time: rustdds::Duration::ZERO,
        })
        .history(History::KeepLast {
            depth: DEFAULT_HISTORY_DEPTH,
        })
        .durability(Durability::Volatile)
        .build()
}

impl DdsChannel {
    pub fn open(participant: &DomainParticipant, channel: &str) -> Result<Self> {
        let qos = channel_qos();

        let publisher = participant
            .create_publisher(&qos)
            .map_err(|e| anyhow::anyhow!("failed to create publisher: {e:?}"))?;
        let subscriber = participant
            .create_subscriber(&qos)
            .map_err(|e| anyhow::anyhow!("failed to create subscriber: {e:?}"))?;

        let up_topic = participant
            .create_topic(
                up_topic_name(channel),
                TYPE_NAME.to_string(),
                &qos,
                TopicKind::NoKey,
            )
            .map_err(|e| anyhow::anyhow!("failed to create up topic: {e:?}"))?;
        let down_topic = participant
            .create_topic(
                down_topic_name(channel),
                TYPE_NAME.to_string(),
                &qos,
                TopicKind::NoKey,
            )
            .map_err(|e| anyhow::anyhow!("failed to create down topic: {e:?}"))?;

        let up_writer = publisher
            .create_datawriter_no_key::<SerialChunk, CDRSerializerAdapter<SerialChunk>>(
                &up_topic,
                None,
            )
            .map_err(|e| anyhow::anyhow!("failed to create up writer: {e:?}"))?;
        let down_writer = publisher
            .create_datawriter_no_key::<SerialChunk, CDRSerializerAdapter<SerialChunk>>(
                &down_topic,
                None,
            )
            .map_err(|e| anyhow::anyhow!("failed to create down writer: {e:?}"))?;

        let up_reader = subscriber
            .create_datareader_no_key::<SerialChunk, CDRDeserializerAdapter<SerialChunk>>(
                &up_topic,
                None,
            )
            .map_err(|e| anyhow::anyhow!("failed to create up reader: {e:?}"))?;
        let down_reader = subscriber
            .create_datareader_no_key::<SerialChunk, CDRDeserializerAdapter<SerialChunk>>(
                &down_topic,
                None,
            )
            .map_err(|e| anyhow::anyhow!("failed to create down reader: {e:?}"))?;

        info!(
            channel,
            up = %up_topic_name(channel),
            down = %down_topic_name(channel),
            "DDS channel opened"
        );

        Ok(Self {
            up_writer,
            down_writer,
            up_reader,
            down_reader,
        })
    }
}

pub async fn write_chunk_async(writer: &ChunkWriter, chunk: SerialChunk) -> Result<()> {
    writer
        .async_write(chunk, None)
        .await
        .map_err(|e| anyhow::anyhow!("DDS async write failed: {e:?}"))
}

pub async fn discover_channels(
    participant: &DomainParticipant,
    wait_secs: u64,
) -> Result<Vec<String>> {
    info!(wait_secs, "waiting for DDS topic discovery");
    sleep(Duration::from_secs(wait_secs)).await;

    let mut channels = BTreeSet::new();
    for topic in participant.discovered_topics() {
        if let Some((channel, _direction)) = parse_channel_from_topic(&topic.topic_name()) {
            channels.insert(channel);
        }
    }

    Ok(channels.into_iter().collect())
}

pub async fn wait_for_shutdown() -> Result<()> {
    tokio::signal::ctrl_c()
        .await
        .context("failed to listen for ctrl-c")?;
    info!("shutdown signal received");
    Ok(())
}