1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
use tracing::{debug, info};

use std::sync::{Arc, RwLock};

use http_body::Frame;
use serde::{Deserialize, Serialize};

use event_stream_types::{AcceptsEventStream, EventBroadcaster};

use crate::{Result, TimeDataPassthrough};

use flydra_types::{FlydraFloatTimestampLocal, SyncFno, Triggerbox};

const EVENTS_PATH: &str = "/events";

#[cfg(feature = "bundle_files")]
static ASSETS_DIR: include_dir::Dir<'static> =
    include_dir::include_dir!("$CARGO_MANIFEST_DIR/static");

async fn events_handler(
    axum::extract::State(app_state): axum::extract::State<ModelServerAppState>,
    _: AcceptsEventStream,
) -> impl axum::response::IntoResponse {
    let key = {
        let mut next_connection_id = app_state.next_connection_id.write().unwrap();
        let key = *next_connection_id;
        *next_connection_id += 1;
        key
    };
    let (tx, body) = app_state.event_broadcaster.new_connection(key);

    // If we have a calibration, extract it.
    let cal_data = {
        // scope for read lock on app_state.current_calibration
        let current_calibration = app_state.current_calibration.read().unwrap();
        if let Some((cal_data, tdpt)) = &*current_calibration {
            let data = (
                SendType::CalibrationFlydraXml(cal_data.clone()),
                tdpt.clone(),
            );
            Some(data)
        } else {
            None
        }
    };

    // If we extracted a calibration above, send it already now.
    if let Some(cal_data) = cal_data {
        let cal_body = get_body(&cal_data);
        tx.send(Ok(Frame::data(cal_body.into()))).await.unwrap();
    }

    body
}

#[derive(Clone)]
struct ModelServerAppState {
    current_calibration: Arc<RwLock<Option<(String, TimeDataPassthrough)>>>,
    event_broadcaster: EventBroadcaster<usize>,
    next_connection_id: Arc<RwLock<usize>>,
}

impl Default for ModelServerAppState {
    fn default() -> Self {
        Self {
            current_calibration: Arc::new(RwLock::new(None)),
            event_broadcaster: Default::default(),
            next_connection_id: Arc::new(RwLock::new(0)),
        }
    }
}

#[allow(non_snake_case)]
#[derive(Debug, Serialize, Deserialize, Clone)]
pub struct SendKalmanEstimatesRow {
    pub obj_id: u32,
    pub frame: SyncFno,
    pub x: f64,
    pub y: f64,
    pub z: f64,
    pub xvel: f64,
    pub yvel: f64,
    pub zvel: f64,
    pub P00: f64,
    pub P01: f64,
    pub P02: f64,
    pub P11: f64,
    pub P12: f64,
    pub P22: f64,
    pub P33: f64,
    pub P44: f64,
    pub P55: f64,
}

impl From<flydra_types::KalmanEstimatesRow> for SendKalmanEstimatesRow {
    fn from(orig: flydra_types::KalmanEstimatesRow) -> SendKalmanEstimatesRow {
        SendKalmanEstimatesRow {
            obj_id: orig.obj_id,
            frame: orig.frame,
            x: orig.x,
            y: orig.y,
            z: orig.z,
            xvel: orig.xvel,
            yvel: orig.yvel,
            zvel: orig.zvel,
            P00: orig.P00,
            P01: orig.P01,
            P02: orig.P02,
            P11: orig.P11,
            P12: orig.P12,
            P22: orig.P22,
            P33: orig.P33,
            P44: orig.P44,
            P55: orig.P55,
        }
    }
}

#[derive(Serialize, Deserialize, Debug, Clone)]
pub enum SendType {
    // IMPORTANT NOTE: if you change this type, be sure to change the version
    // value `v`. Search for the string ZP4q and `Braid pose API`.
    Birth(SendKalmanEstimatesRow),
    Update(SendKalmanEstimatesRow),
    Death(u32), // obj_id

    EndOfFrame(SyncFno),
    /// the multicamera calibration serialized into a flydra xml file
    CalibrationFlydraXml(String),
}

#[derive(Serialize, Deserialize, Debug)]
pub struct ToListener {
    // IMPORTANT NOTE: if you change this type, be sure to change the version
    // value `v`. Search for the string ZP4q and `Braid pose API`.
    /// version
    v: u16,
    msg: SendType,
    latency: f64,
    synced_frame: SyncFno,
    #[serde(with = "flydra_types::timestamp_opt_f64")]
    trigger_timestamp: Option<FlydraFloatTimestampLocal<Triggerbox>>,
}

pub async fn new_model_server(
    mut data_rx: tokio::sync::mpsc::Receiver<(SendType, TimeDataPassthrough)>,
    addr: std::net::SocketAddr,
) -> Result<()> {
    let app_state = ModelServerAppState::default();

    let listener = tokio::net::TcpListener::bind(addr).await?;

    #[cfg(feature = "bundle_files")]
    let serve_dir = tower_serve_static::ServeDir::new(&ASSETS_DIR);

    #[cfg(feature = "serve_files")]
    let serve_dir = tower_http::services::fs::ServeDir::new(
        std::path::PathBuf::from(env!("CARGO_MANIFEST_DIR")).join("static"),
    );

    // Create axum router.
    let router = axum::Router::new()
        .route(EVENTS_PATH, axum::routing::get(events_handler))
        .nest_service("/", serve_dir)
        .with_state(app_state.clone());

    // create future for our app
    let http_serve_future = {
        use std::future::IntoFuture;
        axum::serve(listener, router).into_future()
    };

    info!("ModelServer at http://{}:{}/", addr.ip(), addr.port());

    debug!(
        "ModelServer events at http://{}:{}{}",
        addr.ip(),
        addr.port(),
        EVENTS_PATH,
    );

    // Infinite loop to process and forward data.
    let app_state2 = app_state.clone();
    let new_data_processor_future = async move {
        let app_state = app_state2;

        const ENV_KEY: &str = "RERUN_VIEWER_ADDR";
        let rec = std::env::var_os(ENV_KEY).map(|addr_str| {
            let socket_addr = std::net::ToSocketAddrs::to_socket_addrs(addr_str.to_str().unwrap())
                .unwrap()
                .next()
                .unwrap();
            tracing::info!("Streaming data to rerun at {socket_addr}");
            let rec = rerun::RecordingStreamBuilder::new("braid")
                .connect_opts(socket_addr, None)
                .unwrap();
            rec
        });

        if rec.is_none() {
            tracing::info!(
                "No Rerun viewer address specified with environment variable \
            \"{ENV_KEY}\", not logging data to Rerun. (Hint: the Rerun Viewer \
                listens by default on port 9876.)"
            );
        }

        // Wait for the next update time to arrive ...
        loop {
            let opt_new_data = data_rx.recv().await;
            match &opt_new_data {
                Some(data) => {
                    if let (SendType::CalibrationFlydraXml(calib), tdpt) = &data {
                        let mut current_calibration =
                            app_state.current_calibration.write().unwrap();
                        *current_calibration = Some((calib.clone(), tdpt.clone()));
                    }
                    send_msg(data, &app_state).await?;

                    if let Some(rec) = &rec {
                        match data {
                            (SendType::CalibrationFlydraXml(_calib), _tdpt) => {}
                            (SendType::Birth(row), _tdpt) | (SendType::Update(row), _tdpt) => {
                                let obj_id = format!("{}", row.obj_id);
                                let position =
                                    rerun::Vec3D::new(row.x as f32, row.y as f32, row.z as f32);
                                rec.log(obj_id, &rerun::Points3D::new([position])).unwrap();
                            }
                            (SendType::Death(_x), _tdpt) => {}
                            (SendType::EndOfFrame(_x), _tdpt) => {}
                        }
                    }
                }
                None => {
                    // All senders done. No new data will be coming, so quit.
                    break;
                }
            }
        }
        Ok::<_, crate::Error>(())
    };

    // Wait for one of our futures to finish...
    tokio::select! {
        result = new_data_processor_future => {result?}
        _ = http_serve_future => {}
    }
    // ...then exit.

    Ok(())
}

fn get_body(data: &(SendType, TimeDataPassthrough)) -> String {
    let (msg, tdpt) = data;
    let latency: f64 = if let Some(ref tt) = tdpt.trigger_timestamp() {
        let now_f64 = datetime_conversion::datetime_to_f64(&chrono::Local::now());
        now_f64 - tt.as_f64()
    } else {
        std::f64::NAN
    };

    // Send updates after each observation for lowest-possible latency.
    let data = ToListener {
        // Braid pose API
        v: 3, // <- Bump when ToListener or SendType definition changes ZP4q
        msg: msg.clone(),
        latency,
        synced_frame: tdpt.synced_frame(),
        trigger_timestamp: tdpt.trigger_timestamp(),
    };

    // Serialize to JSON.
    let buf = serde_json::to_string(&data).unwrap();
    // Encode as event source.
    let buf = format!("event: braid\ndata: {}\n\n", buf);
    buf
}

async fn send_msg(
    data: &(SendType, TimeDataPassthrough),
    app_state: &ModelServerAppState,
) -> Result<()> {
    let buf = get_body(data);
    app_state.event_broadcaster.broadcast_frame(buf).await;
    Ok(())
}