laserspace/src/trap/tracks.rs

use bevy::prelude::*;
use serde::{Serialize,Deserialize};
use std::time::Instant;
use nannou_laser as laser;
use serde_repr::*;

use crate::trap::laser::LaserPoints;

#[derive(Serialize,Deserialize)]
pub struct Frame {
    pub tracks: std::collections::HashMap<String, Track>
}

#[derive(Serialize, Deserialize, Component, Debug)]
pub struct Detection{
    track_id: u64,
    l: f32,
    t: f32,
    w: f32,
    h: f32,
    conf: f32,
    state: u8,
    frame_nr: u64,
    det_class: u8,
}

impl Detection {
    fn to_point(&self) -> Vec2 {
        let x = self.l + self.w/2.;
        let y = self.t + self.h;
        Vec2::new(x, y)
    }
}

#[derive(Serialize, Deserialize, Component, Debug)]
pub struct Track {
    pub track_id: u64,
    // nr: u32,
    pub history: Vec<Vec2>, // projected foot coordintes //Vec<Detection>, // history
    pub predictor_history: Option<Vec<Vec2>>, // history
    pub predictions: Option<Vec<Vec<Vec2>>>,
}

// coordinates in world space. To be converted to laser::Point or a drawable point
// TODO migrate to euclid::Point2D<f32, WorldSpace>
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct RenderablePoint{
    pub position: Vec2,
    pub color: Srgba
}

#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct RenderableLine{
    pub points: Vec<RenderablePoint>
}

impl RenderableLine {
    pub fn with_alpha(&self, alpha: f32) -> Self {
        if alpha == 1. {
            self.clone();
        }

        Self { points: self.points.iter().map(|p| {
            RenderablePoint{
                position: p.position,
                color: p.color.with_alpha(alpha),
            }
        }).collect()}
    }
}

// see also trap/lines.py for matching values
#[derive(Clone, Debug, Serialize_repr, Deserialize_repr, Copy)]
#[repr(u8)]
pub enum CoordinateSpace {
    Image = 1,
    UndistortedImage = 2,
    World = 3,
    Laser = 4,
}

#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct RenderableLines{
    pub lines: Vec<RenderableLine>,
    pub space: CoordinateSpace, // enum in python
}

impl RenderableLines{
    pub fn new() -> Self{
        RenderableLines { lines: Vec::new(), space: CoordinateSpace::World }
    }

    // pub fn append(&mut self, &mut rl: RenderableLines){
    //     self.lines.append(rl.lines);
    // }

    pub fn point_count(&self) -> usize {
        let s = self.lines.iter().map(|x| x.points.len()).sum();
        s


    }

    pub fn with_alpha(&self, alpha: f32) -> Self {
        if alpha == 1. {
            return self.clone();
        }

        Self { lines: self.lines.iter().map(|l| { l.with_alpha(alpha)}).collect(), space: self.space.clone() }
    }

}

impl From<&RenderablePoint> for laser::Point {
    fn from(point: &RenderablePoint) -> Self{
        let rgba: Srgba = point.color.into();
        let color = [
            rgba.red * rgba.alpha,
            rgba.green * rgba.alpha,
            rgba.blue * rgba.alpha,
            ];
        Self::new(point.position.into(), color)
    }
}
impl From<&Track> for RenderableLines{
    fn from(track: &Track) -> Self{
        let mut lines: Vec<RenderableLine> = Vec::new();
        if track.history.len() < 2 {
            // no history
        } else {
            let mut points = Vec::new();
            for position in track.history.iter() {
                points.push(RenderablePoint{
                    position: position.clone(),
                    color: Srgba::new(1.0, 0., 0., 1.0)
                });
            }
            lines.push(RenderableLine{points})
        }
        RenderableLines { lines, space: CoordinateSpace::World }
    }
}

// TODO migrate to euclid::Point2D<f32, LaserSpace>

impl From<&RenderableLines> for LaserPoints {
    // much like nannou_laser::stream::frame::add_lines() 
    // turn the lines into a sequence of points with a blanked section inbetween
    // this list can then be past as a whole to add_lines()
    fn from(lineset: &RenderableLines) -> Self{
        let mut points: Vec<laser::Point> = Vec::new();
        for line in lineset.lines.iter() {
            if points.len() > 0 {
                let last = points.last().unwrap();
                points.push(last.clone().blanked());
                if let Some(first) = line.points.first() {
                    let laserpoint: nannou_laser::Point = first.into();
                    points.push(laserpoint.blanked());
                    points.push(laserpoint);
                }
            }
            points.extend(line.points.iter().map(|p| laser::Point::from(p)));
        }
        Self{
            points,
            space: CoordinateSpace::World
        }
    }
}


// check: https://www.reddit.com/r/bevy/comments/y1km8n/how_to_link_components_in_bevy_or_am_i_too_oop/
#[derive(Serialize, Deserialize, Component)]
pub struct PredictedTrajectory{
    person: Entity,
    points: Vec<Vec2>,
}

#[derive(Component)]
pub struct SpawnedTime{
    pub instant: Instant,
}

#[derive(Bundle)]
pub struct TrackBundle {
    track: Track,
    created_at: SpawnedTime,
}

impl From<Track> for TrackBundle{
    fn from(track: Track) -> Self {
        let name = track.track_id.to_string();
        TrackBundle{
            track: track,
            created_at: SpawnedTime{instant: Instant::now()}
            // label: TextBundle{
            //     text: Text::from_section(name, TextStyle::default()),
            //     transform: Transform::from_translation(250. * Vec3::Y),
            //     ..default()
            // }
        }
    }
}

// #[derive(Bundle)]
// pub struct TrackBundle {
//     pub history: TrajectoryBundle,
//     pub time: Time,

// }

// #[derive(Bundle)]
// pub struct TrajectoryBundle {
//     pub line: PolylineBundle
//     // pub trajectory: Trajectory,
// }

#[derive(Component)]
pub struct Trajectory {
    pub points: Vec<Vec3>,
}