pub mod packet;
pub mod hdl64;
pub mod hdl32;
use std::{io, fmt};
use std::cmp::max;
use std::marker::PhantomData;
use std::net::SocketAddrV4;
use crate::packet::{PacketSource, RawPacket, StatusBytes, PacketMeta};
#[derive(Default, Copy, Clone, Debug)]
pub struct FullPoint {
pub xyz: [f32; 3],
pub laser_id: u8,
pub intensity: u8,
pub timestamp: u32,
}
impl From<FullPoint> for [f32; 3] {
fn from(p: FullPoint) -> Self { p.xyz }
}
#[derive(Copy, Clone, Debug)]
pub struct ConversionError;
impl fmt::Display for ConversionError {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
fmt::Debug::fmt(self, f)
}
}
impl std::error::Error for ConversionError {}
pub trait Convertor {
fn convert<F, P>(&self, raw_point: &RawPacket, f: F)
-> Result<PacketMeta, ConversionError>
where F: FnMut(P), P: From<FullPoint>;
}
pub trait StatusListener: Sized {
type Status: Clone;
fn init<T: PacketSource>(packet_source: &mut T) -> io::Result<Self>;
fn feed(&mut self, status: StatusBytes);
fn get_status(&self) -> &Self::Status;
}
#[derive(Copy, Clone, Debug, Default)]
pub struct DummyStatusListener;
impl StatusListener for DummyStatusListener {
type Status = ();
fn init<T: PacketSource>(_source: &mut T) -> io::Result<Self> {
Ok(DummyStatusListener)
}
fn feed(&mut self, _status: StatusBytes) { }
fn get_status(&self) -> &Self::Status { &() }
}
pub struct PointSource<T, C, S>
where T: PacketSource, C: Convertor, S: StatusListener
{
packet_source: T,
status_lst: S,
convertor: C,
}
impl<T, C, S> PointSource<T, C, S>
where T: PacketSource, C: Convertor, S: StatusListener
{
pub fn new(mut packet_source: T, convertor: C) -> io::Result<Self> {
let status_lst = S::init(&mut packet_source)?;
Ok(Self { packet_source, status_lst, convertor })
}
pub fn get_status(&self) -> &S::Status {
self.status_lst.get_status()
}
pub fn process_points<F, P>(&mut self, process_point: F)
-> io::Result<Option<(SocketAddrV4, PacketMeta)>>
where P: From<FullPoint>, F: FnMut(P)
{
let packets = &mut self.packet_source;
let convertor = &self.convertor;
let (addr, packet) = match packets.next_packet()? {
Some(val) => val,
None => return Ok(None),
};
let meta = convertor.convert(packet, process_point)
.map_err(|_| io::Error::new(io::ErrorKind::InvalidData,
"invalid block header"))?;
self.status_lst.feed(meta.status);
Ok(Some((addr, meta)))
}
}
impl<T: PacketSource> PointSource<T, hdl64::Hdl64Convertor, hdl64::StatusListener> {
pub fn hdl64_init(mut packet_source: T) -> io::Result<Self> {
let status_lst = hdl64::StatusListener::init(&mut packet_source)?;
let db = status_lst.get_calib_db(0.2);
let convertor = hdl64::Hdl64Convertor::new(db);
Ok(Self { packet_source, status_lst, convertor })
}
pub fn hdl64_set_calib_db(&mut self, calib_db: hdl64::CalibDb) {
self.convertor = hdl64::Hdl64Convertor::new(calib_db);
}
pub fn get_calib_db(&self) -> hdl64::CalibDb {
self.convertor.db.clone()
}
}
impl<T: PacketSource> PointSource<T, hdl32::Hdl32Convertor, DummyStatusListener> {
pub fn hdl32_init(packet_source: T) -> Self {
Self {
packet_source,
status_lst: Default::default(),
convertor: Default::default(),
}
}
}
pub struct TurnIterator<T, C, S, P>
where T: PacketSource, C: Convertor, S: StatusListener, P: From<FullPoint>
{
point_source: PointSource<T, C, S>,
cap: usize,
prev_azimuth: u16,
split_azimuth: u16,
_p: PhantomData<P>,
}
impl<T, C, S, P> TurnIterator<T, C, S, P>
where T: PacketSource, C: Convertor, S: StatusListener, P: From<FullPoint>
{
pub fn new(packet_source: T, convertor: C) -> io::Result<Self> {
let point_source = PointSource::new(packet_source, convertor)?;
Ok(Self {
point_source, cap: 0, prev_azimuth: 0, split_azimuth: 0,
_p: Default::default(),
})
}
pub fn set_split_azimuth(&mut self, val: u16) {
self.split_azimuth = val % 36000;
}
}
impl<T, P> TurnIterator<T, hdl64::Hdl64Convertor, hdl64::StatusListener, P>
where T: PacketSource, P: From<FullPoint>
{
pub fn hdl64_init(packet_source: T) -> io::Result<Self> {
let point_source = PointSource::hdl64_init(packet_source)?;
Ok(Self {
point_source, cap: 0, prev_azimuth: 0, split_azimuth: 0,
_p: Default::default(),
})
}
pub fn hdl64_set_calib_db(&mut self, calib_db: hdl64::CalibDb) {
self.point_source.hdl64_set_calib_db(calib_db);
}
}
impl<T, P> TurnIterator<T, hdl32::Hdl32Convertor, DummyStatusListener, P>
where T: PacketSource, P: From<FullPoint>
{
pub fn hdl32_init(packet_source: T) -> Self {
let point_source = PointSource::hdl32_init(packet_source);
Self {
point_source, cap: 0, prev_azimuth: 0, split_azimuth: 0,
_p: Default::default(),
}
}
}
impl<T, C, S, P> Iterator for TurnIterator<T, C, S, P>
where T: PacketSource, C: Convertor, S: StatusListener, P: From<FullPoint>
{
type Item = io::Result<(S::Status, Vec<P>)>;
fn next(&mut self) -> Option<Self::Item> {
let mut buf = Vec::with_capacity(self.cap);
loop {
let res = self.point_source.process_points(|point| buf.push(point));
let azimuth = match res {
Ok(Some((_, meta))) => meta.azimuth,
Ok(None) => return None,
Err(err) => return Some(Err(err)),
};
let sa = self.split_azimuth;
let flag = if self.prev_azimuth > azimuth {
!(self.prev_azimuth >= sa && sa > azimuth)
} else {
azimuth >= sa && sa > self.prev_azimuth
};
self.prev_azimuth = azimuth;
if flag { break; }
}
self.cap = max(self.cap, (11*buf.len())/10);
let status = self.point_source.get_status().clone();
Some(Ok((status, buf)))
}
}