initial commit

src/server/backend.rs

@@ -0,0 +1,18 @@
+use serde::de::DeserializeOwned;
+use std::collections::HashMap;
+
+static BACKEND_URL: &'static str = "backend.beammp.com";
+static AUTH_URL: &'static str = "auth.beammp.com";
+
+pub async fn authentication_request<R: DeserializeOwned>(
+    target: &str,
+    map: HashMap<String, String>,
+) -> anyhow::Result<R> {
+    let client = reqwest::Client::new();
+    let resp = client
+        .post(format!("https://{}/{}", AUTH_URL, target))
+        .json(&map)
+        .send()
+        .await?;
+    Ok(resp.json().await?)
+}

src/server/car.rs

@@ -0,0 +1,35 @@
+use nalgebra::*;
+
+use std::time::{Instant, Duration};
+
+#[derive(Default, Clone, Debug)]
+pub struct Car {
+    pub car_json: String,
+
+    pub pos: Vector3<f64>,
+    pub rot: Quaternion<f64>,
+    pub vel: Vector3<f64>,
+    pub rvel: Vector3<f64>,
+    pub tim: f64,
+    pub ping: f64,
+    pub last_pos_update: Option<Instant>,
+}
+
+impl Car {
+    pub fn new(car_json: String) -> Self {
+        Self {
+            car_json: car_json,
+
+            ..Default::default()
+        }
+    }
+
+    pub fn pos(&self) -> Vector3<f64> {
+        self.pos + self.vel * self.last_pos_update.map(|t| t.elapsed().as_secs_f64()).unwrap_or(0.0)
+    }
+
+    pub fn rotation(&self) -> Quaternion<f64> {
+        let t = self.last_pos_update.map(|t| t.elapsed().as_secs_f64()).unwrap_or(0.0);
+        self.rot + UnitQuaternion::from_euler_angles(self.rvel.x * t, self.rvel.y * t, self.rvel.z * t).quaternion()
+    }
+}

src/server/client.rs

@@ -0,0 +1,488 @@
+use std::collections::HashMap;
+use std::net::SocketAddr;
+use std::ops::DerefMut;
+use std::sync::atomic::{AtomicU8, Ordering};
+use std::sync::Arc;
+use std::time::Instant;
+
+use tokio::io::{AsyncReadExt, AsyncWriteExt};
+use tokio::net::{
+    tcp::{OwnedReadHalf, OwnedWriteHalf},
+    TcpStream,
+};
+use tokio::sync::mpsc::{Receiver, Sender};
+use tokio::sync::Mutex;
+use tokio::task::JoinHandle;
+
+use nalgebra::*;
+
+use serde::Deserialize;
+
+use super::backend::*;
+use super::car::*;
+use super::packet::*;
+
+static ATOMIC_ID_COUNTER: AtomicU8 = AtomicU8::new(0);
+
+#[derive(PartialEq)]
+pub enum ClientState {
+    None,
+    Connecting,
+    SyncingResources,
+    Syncing,
+    Disconnect,
+}
+
+#[derive(Deserialize, Debug, PartialEq, Clone)]
+pub struct UserData {
+    pub createdAt: String,
+    pub guest: bool,
+    pub roles: String,
+    pub username: String,
+}
+
+pub struct Client {
+    pub id: u8,
+    pub udp_addr: Option<SocketAddr>,
+
+    socket: OwnedReadHalf,
+    write_half: Arc<Mutex<OwnedWriteHalf>>,
+    write_runtime: JoinHandle<()>,
+    write_runtime_sender: Sender<Packet>,
+
+    pub state: ClientState,
+    pub info: Option<UserData>,
+    pub cars: Vec<(u8, Car)>,
+
+    pub ready: bool,
+    pub grid_spot: usize,
+    pub finished: bool,
+
+    pub incidents: usize,
+    pub last_physics_update: Instant,
+}
+
+impl Drop for Client {
+    fn drop(&mut self) {
+        self.write_runtime.abort();
+    }
+}
+
+impl Client {
+    pub fn new(socket: TcpStream) -> Self {
+        let id = ATOMIC_ID_COUNTER.fetch_add(1, Ordering::SeqCst);
+        trace!("Client with ID #{} created!", id);
+
+        let (read_half, write_half) = socket.into_split();
+        let (tx, mut rx): (Sender<Packet>, Receiver<Packet>) = tokio::sync::mpsc::channel(128);
+        let write_half = Arc::new(Mutex::new(write_half));
+        let write_half_ref = Arc::clone(&write_half);
+        let handle: JoinHandle<()> = tokio::spawn(async move {
+            loop {
+                if let Some(packet) = rx.recv().await {
+                    // trace!("Runtime received packet...");
+                    let mut lock = write_half_ref.lock().await;
+                    // trace!("Runtime sending packet!");
+                    if let Err(e) = tcp_write(lock.deref_mut(), packet).await {
+                        error!("{:?}", e);
+                    };
+                    // trace!("Runtime sent packet!");
+                    drop(lock);
+                }
+            }
+        });
+
+        Self {
+            id: id,
+            udp_addr: None,
+
+            socket: read_half,
+            write_half: write_half,
+            write_runtime: handle,
+            write_runtime_sender: tx,
+
+            state: ClientState::Connecting,
+            info: None,
+            cars: Vec::new(),
+
+            ready: false,
+            grid_spot: 0,
+            finished: false,
+
+            incidents: 0,
+            last_physics_update: Instant::now(),
+        }
+    }
+
+    pub async fn authenticate(&mut self, config: &super::Config) -> anyhow::Result<()> {
+        debug!("Authenticating client {}...", self.id);
+
+        'waiting_for_c: loop {
+            self.socket.readable().await?;
+            let mut tmp = vec![0u8; 1];
+            while self.socket.peek(&mut tmp).await? == 0 {}
+            // Authentication works a little differently than normal
+            // Not sure why, but the BeamMP source code shows they
+            // also only read a single byte during authentication
+            let code = self.read_raw(1).await?[0];
+            debug!("code: '{}' / {}", code as char, code);
+            match code as char {
+                'C' => {
+                    // TODO: Check client version
+                    trace!("Client version packet");
+                    self.socket.readable().await?;
+                    let packet = self.read_packet_waiting().await?;
+                    debug!("{:?}", packet);
+                    break 'waiting_for_c;
+                }
+                'D' => {
+                    let id = self.read_raw(1).await?[0];
+                    debug!("HandleDownload connection for client id: {}", id);
+                    debug!("Not handling this for now!");
+                    return Err(ClientError::IsDownloader.into());
+                }
+                _ => {
+                    return Err(ClientError::AuthenticateError.into());
+                }
+            }
+        }
+
+        self.write_packet(Packet::Raw(RawPacket::from_code('S')))
+            .await?;
+
+        self.socket.readable().await?;
+        if let Some(packet) = self.read_packet_waiting().await? {
+            debug!("packet: {:?}", packet);
+            if packet.data.len() > 50 {
+                self.kick("Player key too big!").await;
+                return Err(ClientError::AuthenticateError.into());
+            }
+            let mut json = HashMap::new();
+            json.insert("key".to_string(), packet.data_as_string());
+            let user_data: UserData =
+                authentication_request("pkToUser", json)
+                    .await
+                    .map_err(|e| {
+                        error!("{:?}", e);
+                        e
+                    })?;
+            debug!("user_data: {:?}", user_data);
+            self.info = Some(user_data);
+        } else {
+            self.kick(
+                "Client never sent public key! If this error persists, try restarting your game.",
+            )
+            .await;
+        }
+
+        self.write_packet(Packet::Raw(RawPacket::from_str(&format!("P{}", self.id))))
+            .await?;
+
+        self.state = ClientState::SyncingResources;
+
+        debug!(
+            "Authentication of client {} succesfully completed! Syncing now...",
+            self.id
+        );
+        self.sync(config).await?;
+
+        Ok(())
+    }
+
+    // TODO: https://github.com/BeamMP/BeamMP-Server/blob/master/src/TNetwork.cpp#L619
+    pub async fn sync(&mut self, config: &super::Config) -> anyhow::Result<()> {
+        'syncing: while self.state == ClientState::SyncingResources {
+            self.socket.readable().await?;
+            if let Some(packet) = self.read_packet().await? {
+                if packet.data.len() == 0 {
+                    continue;
+                }
+                if packet.data.len() == 4 {
+                    if packet.data == [68, 111, 110, 101] {
+                        break 'syncing;
+                    }
+                }
+                match packet.data[0] as char {
+                    'S' if packet.data.len() > 1 => match packet.data[1] as char {
+                        'R' => {
+                            let file_packet = RawPacket::from_code('-');
+                            // let file_data = "/bepis_dysoon_uu201_v3.zip;/bepis_laudi_v8_revolution.zip;/simraceclubclient.zip;48353220;50283849;1937;";
+                            // let file_packet = RawPacket::from_str(file_data);
+                            self.write_packet(Packet::Raw(file_packet))
+                                .await?
+                        }
+                        _ => error!("Unknown packet! {:?}", packet),
+                    }
+                    'f' => {
+                        // Handle file download
+                        let mut file_name = packet.data_as_string().clone();
+                        file_name.remove(0); // Remove f
+                        debug!("Client requested file {}", file_name);
+                        self.kick(&format!("You have not yet downloaded {}!", file_name)).await;
+                        // let mut lock = self.write_half.lock().await;
+                        // lock.writable().await?;
+                        // trace!("Sending packet!");
+                        // if let Err(e) = tcp_send_file(lock.deref_mut(), file_name).await {
+                        //     error!("{:?}", e);
+                        // }
+                    }
+                    _ => error!("Unknown packet! {:?}", packet),
+                }
+            }
+        }
+        self.state = ClientState::None;
+        trace!("Done syncing! Sending map name...");
+        self.write_packet(Packet::Raw(RawPacket::from_str(&format!(
+            "M{}",
+            config.general.map
+        ))))
+        .await?;
+        trace!("Map name sent!");
+        Ok(())
+    }
+
+    /// This function should never block. It should simply check if there's a
+    /// packet, and then and only then should it read it. If this were to block, the server
+    /// would come to a halt until this function unblocks.
+    pub async fn process(&mut self) -> anyhow::Result<Option<RawPacket>> {
+        if let Some(packet) = self.read_packet().await? {
+            return Ok(Some(packet));
+        }
+        Ok(None)
+    }
+
+    pub fn disconnect(&mut self) {
+        self.state = ClientState::Disconnect;
+    }
+
+    pub async fn kick(&mut self, msg: &str) {
+        // let _ = self.socket.writable().await;
+        // let _ = self.write_packet(Packet::Raw(RawPacket::from_str(&format!("K{}", msg)))).await;
+        // self.disconnect();
+        let _ = self
+            .write_packet(Packet::Raw(RawPacket::from_str(&format!("K{}", msg))))
+            .await;
+        self.disconnect();
+    }
+
+    pub fn get_name(&self) -> &str {
+        &self.info.as_ref().unwrap().username
+    }
+
+    pub fn get_id(&self) -> u8 {
+        self.id
+    }
+
+    pub fn get_roles(&self) -> &str {
+        &self.info.as_ref().unwrap().roles
+    }
+
+    pub fn register_car(&mut self, car: Car) -> u8 {
+        let mut free_num = 0;
+        for (num, _) in &self.cars {
+            if num == &free_num {
+                free_num += 1;
+            } else {
+                break;
+            }
+        }
+        self.cars.push((free_num, car));
+        free_num
+    }
+
+    pub fn unregister_car(&mut self, car_id: u8) {
+        let prev_len = self.cars.len();
+        self.cars.retain(|(id, _)| id != &car_id);
+        if prev_len == self.cars.len() {
+            error!(
+                "Failed to unregister car #{} for client #{}! Ignoring for now...",
+                car_id, self.id
+            );
+        }
+    }
+
+    pub fn get_car_mut(&mut self, mut car_id: u8) -> Option<&mut Car> {
+        for (num, car) in &mut self.cars {
+            if num == &mut car_id {
+                return Some(car);
+            }
+        }
+        None
+    }
+
+    async fn read_raw(&mut self, count: usize) -> anyhow::Result<Vec<u8>> {
+        let mut b = vec![0u8; count];
+        self.socket.read_exact(&mut b).await?;
+        Ok(b)
+    }
+
+    async fn read_packet_waiting(&mut self) -> anyhow::Result<Option<RawPacket>> {
+        let start = std::time::Instant::now();
+        'wait: loop {
+            if let Some(packet) = self.read_packet().await? {
+                return Ok(Some(packet));
+            }
+            if start.elapsed().as_secs() >= 5 {
+                break 'wait;
+            }
+            tokio::time::sleep(std::time::Duration::from_millis(500)).await;
+        }
+        Err(ClientError::ConnectionTimeout.into())
+    }
+
+    /// Must be non-blocking
+    async fn read_packet(&mut self) -> anyhow::Result<Option<RawPacket>> {
+        let mut header = [0u8; 4];
+        match self.socket.try_read(&mut header) {
+            Ok(0) => {
+                error!("Socket is readable, yet has 0 bytes to read! Disconnecting client...");
+                self.disconnect();
+                return Ok(None);
+            }
+            Ok(_n) => {}
+            Err(ref e) if e.kind() == std::io::ErrorKind::WouldBlock => {
+                return Ok(None);
+            }
+            Err(e) => return Err(e.into()),
+        }
+
+        let expected_size = u32::from_le_bytes(header) as usize;
+        let mut data = Vec::new();
+        let mut tmp_data = vec![0u8; expected_size];
+        let mut data_size = 0;
+        while data_size < expected_size {
+            match self.socket.try_read(&mut tmp_data) {
+                Ok(0) => {
+                    error!("Socket is readable, yet has 0 bytes to read! Disconnecting client...");
+                    self.disconnect();
+                    return Ok(None);
+                }
+                Ok(n) => {
+                    data_size += n;
+                    data.extend_from_slice(&tmp_data[..n]);
+                },
+                Err(ref e) if e.kind() == std::io::ErrorKind::WouldBlock => {
+                    // debug!("Packet appears to be ready, yet can't be read yet!");
+                    // self.socket.read(&mut data).await?;
+                    return Ok(None);
+                }
+                Err(e) => return Err(e.into()),
+            }
+        }
+
+        Ok(Some(RawPacket {
+            header: data_size as u32,
+            data: data[..data_size].to_vec(),
+        }))
+    }
+
+    /// Blocking write
+    pub async fn write_packet(&mut self, packet: Packet) -> anyhow::Result<()> {
+        let mut lock = self.write_half.lock().await;
+        lock.writable().await?;
+        trace!("Sending packet!");
+        if let Err(e) = tcp_write(lock.deref_mut(), packet).await {
+            error!("{:?}", e);
+        }
+        trace!("Packet sent!");
+        drop(lock);
+        Ok(())
+    }
+
+    pub async fn queue_packet(&self, packet: Packet) {
+        // TODO: If packet gets lost, put it back at the front of the queue?
+        let _ = self.write_runtime_sender.send(packet).await;
+    }
+
+    pub async fn trigger_client_event<S: Into<String>, D: Into<String>>(&self, event_name: S, data: D) {
+        let event_name = event_name.into();
+        let data = data.into();
+        debug!("Calling client event '{}' with data '{}'", event_name, data);
+        let packet_data = format!("E:{}:{}", event_name, data);
+        self.queue_packet(Packet::Raw(RawPacket::from_str(&packet_data))).await;
+    }
+}
+
+#[derive(Debug)]
+pub enum ClientError {
+    AuthenticateError,
+    ConnectionTimeout,
+    IsDownloader,
+}
+
+impl std::fmt::Display for ClientError {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> Result<(), std::fmt::Error> {
+        write!(f, "{:?}", self)?;
+        Ok(())
+    }
+}
+
+impl std::error::Error for ClientError {}
+
+#[async_trait]
+trait Writable {
+    async fn writable(&self) -> std::io::Result<()>;
+}
+
+#[async_trait]
+impl Writable for TcpStream {
+    async fn writable(&self) -> std::io::Result<()> {
+        self.writable().await
+    }
+}
+
+#[async_trait]
+impl Writable for OwnedWriteHalf {
+    async fn writable(&self) -> std::io::Result<()> {
+        self.writable().await
+    }
+}
+
+async fn tcp_write<W: AsyncWriteExt + Writable + std::marker::Unpin>(
+    w: &mut W,
+    mut packet: Packet,
+) -> anyhow::Result<()> {
+    let compressed = match packet.get_code() {
+        Some('O') => true,
+        Some('T') => true,
+        _ => packet.get_data().len() > 400,
+    };
+
+    if compressed {
+        let mut compressed: Vec<u8> = Vec::with_capacity(100_000);
+        let mut compressor = flate2::Compress::new(flate2::Compression::best(), true);
+        compressor.compress_vec(
+            packet.get_data(),
+            &mut compressed,
+            flate2::FlushCompress::Sync,
+        )?;
+        let mut new_data = "ABG:".as_bytes()[..4].to_vec();
+        new_data.append(&mut compressed);
+        packet.set_header(new_data.len() as u32);
+        packet.set_data(new_data);
+    }
+
+    tcp_write_raw(w, packet).await?;
+
+    Ok(())
+}
+
+async fn tcp_write_raw<W: AsyncWriteExt + Writable + std::marker::Unpin>(
+    w: &mut W,
+    mut packet: Packet,
+) -> anyhow::Result<()> {
+    let mut raw_data: Vec<u8> = packet.get_header().to_le_bytes().to_vec();
+    raw_data.extend_from_slice(packet.get_data());
+    w.writable().await?;
+    w.write(&raw_data).await?;
+    Ok(())
+}
+
+async fn tcp_send_file<W: AsyncWriteExt + Writable + std::marker::Unpin>(
+    w: &mut W,
+    file_name: String,
+) -> anyhow::Result<()> {
+    debug!("Sending file '{}'", file_name);
+    tcp_write(w, Packet::Raw(RawPacket::from_str("KYou have not downloaded the mod manually!"))).await?;
+    Ok(())
+}

src/server/mod.rs

@@ -0,0 +1,631 @@
+use std::net::SocketAddr;
+use std::sync::{Arc, Mutex, atomic::{AtomicBool, Ordering}};
+use std::time::Instant;
+
+use tokio::net::{TcpListener, UdpSocket};
+use tokio::task::JoinHandle;
+
+use num_enum::IntoPrimitive;
+
+use nalgebra::*;
+
+mod backend;
+mod car;
+mod client;
+mod packet;
+
+pub use backend::*;
+pub use car::*;
+pub use client::*;
+pub use packet::*;
+
+pub use crate::config::Config;
+
+#[derive(PartialEq, IntoPrimitive, Copy, Clone, Debug)]
+#[repr(u8)]
+enum ServerState {
+    Unknown = 0,
+
+    WaitingForClients,
+    WaitingForReady,
+    WaitingForSpawns,
+    Qualifying,
+    LiningUp,
+    Countdown,
+    Race,
+    Finish,
+}
+
+pub struct Server {
+    tcp_listener: Arc<TcpListener>,
+    udp_socket: Arc<UdpSocket>,
+
+    clients_incoming: Arc<Mutex<Vec<Client>>>,
+
+    pub clients: Vec<Client>,
+
+    connect_runtime_handle: JoinHandle<()>,
+
+    config: Arc<Config>,
+}
+
+impl Server {
+    pub async fn new(config: Arc<Config>) -> anyhow::Result<Self> {
+        let config_ref = Arc::clone(&config);
+
+        let port = config.general.port.unwrap_or(48900);
+
+        let tcp_listener = {
+            let bind_addr = &format!("0.0.0.0:{}", port);
+            Arc::new(TcpListener::bind(bind_addr).await?)
+        };
+        let tcp_listener_ref = Arc::clone(&tcp_listener);
+
+        let udp_socket = {
+            let bind_addr = &format!("0.0.0.0:{}", port);
+            Arc::new(UdpSocket::bind(bind_addr).await?)
+        };
+
+        let clients_incoming = Arc::new(Mutex::new(Vec::new()));
+        let clients_incoming_ref = Arc::clone(&clients_incoming);
+        debug!("Client acception runtime starting...");
+        let connect_runtime_handle = tokio::spawn(async move {
+            loop {
+                match tcp_listener_ref.accept().await {
+                    Ok((socket, addr)) => {
+                        info!("New client connected: {:?}", addr);
+
+                        let mut client = Client::new(socket);
+                        match client.authenticate(&config_ref).await {
+                            Ok(_) => {
+                                let mut lock = clients_incoming_ref
+                                    .lock()
+                                    .map_err(|e| error!("{:?}", e))
+                                    .expect("Failed to acquire lock on mutex!");
+                                lock.push(client);
+                                drop(lock);
+                            },
+                            Err(e) => {
+                                error!("Authentication error occured, kicking player...");
+                                error!("{:?}", e);
+                                client.kick("Failed to authenticate player!").await;
+                            }
+                        }
+                    }
+                    Err(e) => error!("Failed to accept incoming connection: {:?}", e),
+                }
+            }
+        });
+        debug!("Client acception runtime started!");
+
+        Ok(Self {
+            tcp_listener: tcp_listener,
+            udp_socket: udp_socket,
+
+            clients_incoming: clients_incoming,
+
+            clients: Vec::new(),
+
+            connect_runtime_handle: connect_runtime_handle,
+
+            config: config,
+        })
+    }
+
+    pub async fn process(&mut self) -> anyhow::Result<()> {
+        // Bit weird, but this is all to avoid deadlocking the server if anything goes wrong
+        // with the client acception runtime. If that one locks, the server won't accept
+        // more clients, but it will at least still process all other clients
+        let mut joined_names = Vec::new();
+        if let Ok(mut clients_incoming_lock) = self.clients_incoming.try_lock() {
+            if clients_incoming_lock.len() > 0 {
+                trace!(
+                    "Accepting {} incoming clients...",
+                    clients_incoming_lock.len()
+                );
+                for i in 0..clients_incoming_lock.len() {
+                    joined_names.push(
+                        clients_incoming_lock[i]
+                            .info
+                            .as_ref()
+                            .unwrap()
+                            .username
+                            .clone(),
+                    );
+                    self.clients.push(clients_incoming_lock.swap_remove(i));
+                }
+                trace!("Accepted incoming clients!");
+            }
+        }
+
+        // Process UDP packets
+        // TODO: Use a UDP addr -> client ID look up table
+        for (addr, packet) in self.read_udp_packets().await {
+            if packet.data.len() == 0 {
+                continue;
+            }
+            let id = packet.data[0] - 1; // Offset by 1
+            let data = packet.data[2..].to_vec();
+            let packet_processed = RawPacket {
+                header: data.len() as u32,
+                data,
+            };
+            'search: for i in 0..self.clients.len() {
+                if self.clients[i].id == id {
+                    self.parse_packet_udp(i, addr, packet_processed).await?;
+                    break 'search;
+                }
+            }
+        }
+
+        // Process all the clients (TCP)
+        let mut packets: Vec<(usize, RawPacket)> = Vec::new();
+        for i in 0..self.clients.len() {
+            if let Some(client) = self.clients.get_mut(i) {
+                match client.process().await {
+                    Ok(packet_opt) => {
+                        if let Some(raw_packet) = packet_opt {
+                            packets.push((i, raw_packet.clone()));
+                        }
+                    }
+                    Err(e) => client.kick(&format!("Kicked: {:?}", e)).await,
+                }
+
+                // More efficient than broadcasting as we are already looping
+                for name in joined_names.iter() {
+                    self.clients[i]
+                        .queue_packet(Packet::Notification(NotificationPacket::new(format!(
+                            "Welcome {}!",
+                            name.to_string()
+                        ))))
+                        .await;
+                }
+            }
+        }
+        for (i, packet) in packets {
+            self.parse_packet(i, packet).await?
+        }
+
+        // I'm sorry for this code :(
+        for i in 0..self.clients.len() {
+            if self.clients.get(i).ok_or(ServerError::ClientDoesntExist)?.state == ClientState::Disconnect {
+                let id = self.clients.get(i).ok_or(ServerError::ClientDoesntExist)?.id;
+                for j in 0..self.clients.get(i).ok_or(ServerError::ClientDoesntExist)?.cars.len() {
+                    let car_id = self.clients.get(i).ok_or(ServerError::ClientDoesntExist)?.cars[j].0;
+                    let delete_packet = format!("Od:{}-{}", id, car_id);
+                    self.broadcast(Packet::Raw(RawPacket::from_str(&delete_packet)), None)
+                        .await;
+                }
+                info!("Disconnecting client {}...", id);
+                self.clients.remove(i);
+                info!("Client {} disconnected!", id);
+            }
+        }
+
+        Ok(())
+    }
+
+    async fn broadcast(&self, packet: Packet, owner: Option<u8>) {
+        for client in &self.clients {
+            if let Some(id) = owner {
+                if id == client.id {
+                    continue;
+                }
+            }
+            client.queue_packet(packet.clone()).await;
+        }
+    }
+
+    async fn broadcast_udp(&self, packet: Packet, owner: Option<u8>) {
+        for client in &self.clients {
+            if let Some(id) = owner {
+                if id == client.id {
+                    continue;
+                }
+            }
+            // client.queue_packet(packet.clone()).await;
+            if let Some(udp_addr) = client.udp_addr {
+                self.send_udp(udp_addr, &packet).await;
+            }
+        }
+    }
+
+    async fn send_udp(&self, udp_addr: SocketAddr, packet: &Packet) {
+        let data = packet.get_data();
+        if data.len() > 400 {
+            trace!("Compressing...");
+            let mut compressed: Vec<u8> = Vec::with_capacity(100_000);
+            let mut compressor = flate2::Compress::new(flate2::Compression::best(), true);
+            if let Err(e) = compressor.compress_vec(
+                data,
+                &mut compressed,
+                flate2::FlushCompress::Sync,
+            ) {
+                error!("Compression failed!");
+                return;
+            }
+            let mut new_data = "ABG:".as_bytes()[..4].to_vec();
+            new_data.append(&mut compressed);
+            if let Err(e) = self.udp_socket.try_send_to(&new_data, udp_addr) {
+                error!("UDP Packet send error: {:?}", e);
+            }
+        } else {
+            if let Err(e) = self.udp_socket.try_send_to(&data, udp_addr) {
+                error!("UDP Packet send error: {:?}", e);
+            }
+        }
+    }
+
+    async fn read_udp_packets(&self) -> Vec<(SocketAddr, RawPacket)> {
+        let mut packets = Vec::new();
+        'read: loop {
+            let mut data = vec![0u8; 4096];
+            let data_size;
+            let data_addr;
+
+            match self.udp_socket.try_recv_from(&mut data) {
+                Ok((0, _)) => {
+                    error!("UDP socket is readable, yet has 0 bytes to read!");
+                    break 'read;
+                }
+                Ok((n, addr)) => (data_size, data_addr) = (n, addr),
+                Err(_) => break 'read,
+            }
+
+            let packet = RawPacket {
+                header: data_size as u32,
+                data: data[..data_size].to_vec(),
+            };
+            packets.push((data_addr, packet));
+        }
+        packets
+    }
+
+    async fn parse_packet_udp(
+        &mut self,
+        client_idx: usize,
+        udp_addr: SocketAddr,
+        mut packet: RawPacket,
+    ) -> anyhow::Result<()> {
+        if packet.data.len() > 0 {
+            let client = &mut self.clients[client_idx];
+            let client_id = client.get_id();
+
+            client.udp_addr = Some(udp_addr);
+
+            // Check if compressed
+            let mut is_compressed = false;
+            if packet.data.len() > 3 {
+                let string_data = String::from_utf8_lossy(&packet.data[..4]);
+                if string_data.starts_with("ABG:") {
+                    is_compressed = true;
+                    trace!("Packet is compressed!");
+                }
+            }
+
+            if is_compressed {
+                let compressed = &packet.data[4..];
+                let mut decompressed: Vec<u8> = Vec::with_capacity(100_000);
+                let mut decompressor = flate2::Decompress::new(true);
+                decompressor.decompress_vec(
+                    compressed,
+                    &mut decompressed,
+                    flate2::FlushDecompress::Finish,
+                )?;
+                packet.header = decompressed.len() as u32;
+                packet.data = decompressed;
+                // let string_data = String::from_utf8_lossy(&packet.data[..]);
+                // debug!("Unknown packet - String data: `{}`; Array: `{:?}`; Header: `{:?}`", string_data, packet.data, packet.header);
+            }
+
+            // Check packet identifier
+            let packet_identifier = packet.data[0] as char;
+            if packet.data[0] >= 86 && packet.data[0] <= 89 {
+                self.broadcast_udp(Packet::Raw(packet), Some(client_id))
+                    .await;
+            } else {
+                match packet_identifier {
+                    'p' => {
+                        self.send_udp(udp_addr, &Packet::Raw(RawPacket::from_code('p')))
+                            .await;
+                    }
+                    'Z' => {
+                        if packet.data.len() < 7 {
+                            error!("Position packet too small!");
+                            return Err(ServerError::BrokenPacket.into());
+                        } else {
+                            // Sent as text so removing 48 brings it from [48-57] to [0-9]
+                            let client_id = packet.data[3] - 48;
+                            let car_id = packet.data[5] - 48;
+
+                            let pos_json = &packet.data[7..];
+                            let pos_data: TransformPacket =
+                                serde_json::from_str(&String::from_utf8_lossy(pos_json))?;
+
+                            let p = Packet::Raw(packet);
+
+                            for i in 0..self.clients.len() {
+                                if self.clients[i].id == client_id {
+                                    let client = &mut self.clients[i];
+                                    let car = client
+                                        .get_car_mut(car_id)
+                                        .ok_or(ServerError::CarDoesntExist)?;
+                                    car.pos = pos_data.pos.into();
+                                    car.rot = Quaternion::new(
+                                        pos_data.rot[3],
+                                        pos_data.rot[0],
+                                        pos_data.rot[1],
+                                        pos_data.rot[2],
+                                    );
+                                    car.vel = pos_data.vel.into();
+                                    car.rvel = pos_data.rvel.into();
+                                    car.tim = pos_data.tim;
+                                    car.ping = pos_data.ping;
+                                    car.last_pos_update = Some(Instant::now());
+                                } else {
+                                    if let Some(udp_addr) = self.clients[i].udp_addr {
+                                        self.send_udp(udp_addr, &p).await;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                    _ => {
+                        let string_data = String::from_utf8_lossy(&packet.data[..]);
+                        debug!(
+                            "Unknown packet UDP - String data: `{}`; Array: `{:?}`; Header: `{:?}`",
+                            string_data, packet.data, packet.header
+                        );
+                    }
+                }
+            }
+        }
+        Ok(())
+    }
+
+    async fn parse_packet(
+        &mut self,
+        client_idx: usize,
+        mut packet: RawPacket,
+    ) -> anyhow::Result<()> {
+        if packet.data.len() > 0 {
+            let client_id = {
+                let client = &mut self.clients[client_idx];
+                client.get_id()
+            };
+
+            // Check if compressed
+            let mut is_compressed = false;
+            if packet.data.len() > 3 {
+                let string_data = String::from_utf8_lossy(&packet.data[..4]);
+                if string_data.starts_with("ABG:") {
+                    is_compressed = true;
+                    // trace!("Packet is compressed!");
+                }
+            }
+
+            if is_compressed {
+                let compressed = &packet.data[4..];
+                let mut decompressed: Vec<u8> = Vec::with_capacity(100_000);
+                let mut decompressor = flate2::Decompress::new(true);
+                decompressor.decompress_vec(
+                    compressed,
+                    &mut decompressed,
+                    flate2::FlushDecompress::Finish,
+                )?;
+                packet.header = decompressed.len() as u32;
+                packet.data = decompressed;
+                // let string_data = String::from_utf8_lossy(&packet.data[..]);
+                // debug!("Unknown packet - String data: `{}`; Array: `{:?}`; Header: `{:?}`", string_data, packet.data, packet.header);
+            }
+
+            // Check packet identifier
+            if packet.data[0] >= 86 && packet.data[0] <= 89 {
+                self.broadcast(Packet::Raw(packet), Some(client_id)).await;
+            } else {
+                let packet_identifier = packet.data[0] as char;
+                match packet_identifier {
+                    'H' => {
+                        // Full sync with server
+                        self.clients[client_idx]
+                            .queue_packet(Packet::Raw(RawPacket::from_str(&format!(
+                                "Sn{}",
+                                self.clients[client_idx]
+                                    .info
+                                    .as_ref()
+                                    .unwrap()
+                                    .username
+                                    .clone()
+                            ))))
+                            .await;
+
+                        // TODO: Sync all existing cars on server (this code is broken)
+                        for client in &self.clients {
+                            let pid = client.id as usize;
+                            if pid != client_idx {
+                                for (vid, car) in &client.cars {
+                                    self.clients[client_idx]
+                                        .queue_packet(Packet::Raw(RawPacket::from_str(&format!(
+                                            "Os:{vid}:{}",
+                                            car.car_json,
+                                        ))))
+                                        .await;
+                                }
+                            }
+                        }
+                    }
+                    'O' => self.parse_vehicle_packet(client_idx, packet).await?,
+                    'C' => {
+                        // TODO: Chat filtering?
+                        let packet_data = packet.data_as_string();
+                        let message = packet_data.split(":").collect::<Vec<&str>>().get(2).map(|s| s.to_string()).unwrap_or(String::new());
+                        let message = message.trim();
+                        if message.starts_with("!") {
+                            if message == "!ready" {
+                                self.clients[client_idx].ready = true;
+                                self.clients[client_idx].queue_packet(Packet::Raw(RawPacket::from_str("C:Server:You are now ready!"))).await;
+                            } else if message == "!pos" {
+                                let car = &self.clients[client_idx].cars.get(0).ok_or(ServerError::CarDoesntExist)?.1;
+                                trace!("car transform (pos/rot/vel/rvel): {:?}", (car.pos, car.rot, car.vel, car.rvel));
+                            } else {
+                                self.clients[client_idx].queue_packet(Packet::Raw(RawPacket::from_str("C:Server:Unknown command!"))).await;
+                            }
+                        } else {
+                            self.broadcast(Packet::Raw(packet), None).await;
+                        }
+                    }
+                    _ => {
+                        let string_data = String::from_utf8_lossy(&packet.data[..]);
+                        debug!(
+                            "Unknown packet - String data: `{}`; Array: `{:?}`; Header: `{:?}`",
+                            string_data, packet.data, packet.header
+                        );
+                    }
+                }
+            }
+        }
+        Ok(())
+    }
+
+    async fn parse_vehicle_packet(
+        &mut self,
+        client_idx: usize,
+        packet: RawPacket,
+    ) -> anyhow::Result<()> {
+        if packet.data.len() < 6 {
+            error!("Vehicle packet too small!");
+            return Ok(()); // TODO: Return error here
+        }
+        let code = packet.data[1] as char;
+        match code {
+            's' => {
+                let client = &mut self.clients[client_idx];
+                let mut allowed = true;
+                if let Some(max_cars) = self.config.general.max_cars {
+                    if client.cars.len() >= max_cars as usize { allowed = false; }
+                }
+                // trace!("Packet string: `{}`", packet.data_as_string());
+                let split_data = packet
+                    .data_as_string()
+                    .splitn(3, ':')
+                    .map(|s| s.to_string())
+                    .collect::<Vec<String>>();
+                let car_json_str = &split_data.get(2).ok_or(std::fmt::Error)?;
+                // let car_json: serde_json::Value = serde_json::from_str(&car_json_str)?;
+                let car_id = client.register_car(Car::new(car_json_str.to_string()));
+                let client_id = client.get_id();
+                if allowed {
+                    client.trigger_client_event("GetSize", client_id.to_string()).await;
+                    let packet_data = format!(
+                        "Os:{}:{}:{}-{}:{}",
+                        client.get_roles(),
+                        client.get_name(),
+                        client_id,
+                        car_id,
+                        car_json_str
+                    );
+                    let response = RawPacket::from_str(&packet_data);
+                    self.broadcast(Packet::Raw(response), None).await;
+                    info!("Spawned car for client #{}!", client_id);
+                } else {
+                    let packet_data = format!(
+                        "Os:{}:{}:{}-{}:{}",
+                        client.get_roles(),
+                        client.get_name(),
+                        client_id,
+                        car_id,
+                        car_json_str
+                    );
+                    let response = RawPacket::from_str(&packet_data);
+                    client.write_packet(Packet::Raw(response)).await;
+                    let packet_data = format!(
+                        "Od:{}-{}",
+                        client_id,
+                        car_id,
+                    );
+                    let response = RawPacket::from_str(&packet_data);
+                    client.write_packet(Packet::Raw(response)).await;
+                    client.unregister_car(car_id);
+                    info!("Blocked spawn for client #{}!", client_id);
+                }
+            }
+            'c' => {
+                // let split_data = packet.data_as_string().splitn(3, ':').map(|s| s.to_string()).collect::<Vec<String>>();
+                // let car_json_str = &split_data.get(2).ok_or(std::fmt::Error)?;
+                let client_id = packet.data[3] - 48;
+                let car_id = packet.data[5] - 48;
+                let car_json = String::from_utf8_lossy(&packet.data[7..]).to_string();
+                let response = Packet::Raw(packet.clone());
+                for i in 0..self.clients.len() {
+                    if self.clients[i].id == client_id {
+                        if let Some(car) = self.clients[i].get_car_mut(car_id) {
+                            car.car_json = car_json.clone();
+                        }
+                    } else {
+                        // Already looping so more efficient to send here
+                        // if let Some(udp_addr) = self.clients[i].udp_addr {
+                        //     self.write_udp(udp_addr, &response).await;
+                        // }
+                        self.clients[i].write_packet(response.clone()).await;
+                    }
+                }
+            }
+            'd' => {
+                debug!("packet: {:?}", packet);
+                let split_data = packet
+                    .data_as_string()
+                    .splitn(3, [':', '-'])
+                    .map(|s| s.to_string())
+                    .collect::<Vec<String>>();
+                let client_id = split_data[1].parse::<u8>()?;
+                let car_id = split_data[2].parse::<u8>()?;
+                for i in 0..self.clients.len() {
+                    if self.clients[i].id == client_id {
+                        self.clients[i].unregister_car(car_id);
+                    }
+                    // Don't broadcast, we are already looping anyway
+                    // if let Some(udp_addr) = self.clients[i].udp_addr {
+                    //     self.send_udp(udp_addr, &Packet::Raw(packet.clone())).await;
+                    // }
+                    self.clients[i].write_packet(Packet::Raw(packet.clone())).await;
+                }
+                info!("Deleted car for client #{}!", client_id);
+            }
+            'r' => {
+                self.broadcast(Packet::Raw(packet), Some(self.clients[client_idx].id)).await;
+            }
+            't' => {
+                self.broadcast(Packet::Raw(packet), Some(self.clients[client_idx].id))
+                    .await;
+            }
+            'm' => {
+                self.broadcast(Packet::Raw(packet), None).await;
+            }
+            _ => error!("Unknown vehicle related packet!\n{:?}", packet), // TODO: Return error here
+        }
+        Ok(())
+    }
+}
+
+impl Drop for Server {
+    fn drop(&mut self) {
+        // Not sure how needed this is but it seems right?
+        self.connect_runtime_handle.abort();
+    }
+}
+
+#[derive(Debug)]
+pub enum ServerError {
+    BrokenPacket,
+    CarDoesntExist,
+    ClientDoesntExist,
+}
+
+impl std::fmt::Display for ServerError {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> Result<(), std::fmt::Error> {
+        write!(f, "{:?}", self)?;
+        Ok(())
+    }
+}
+
+impl std::error::Error for ServerError {}

src/server/packet.rs

@@ -0,0 +1,131 @@
+use serde::{Deserialize, Serialize};
+
+#[derive(Debug, Clone)]
+pub enum Packet {
+    Raw(RawPacket),
+    Notification(NotificationPacket),
+}
+
+impl Packet {
+    pub fn get_header(&self) -> u32 {
+        match self {
+            Self::Raw(raw) => raw.header,
+            Self::Notification(msg) => self.get_data().len() as u32,
+        }
+    }
+
+    pub fn get_data(&self) -> &[u8] {
+        match self {
+            Self::Raw(raw) => &raw.data,
+            Self::Notification(p) => p.0.as_bytes(),
+        }
+    }
+
+    pub fn get_code(&self) -> Option<char> {
+        match self {
+            Self::Raw(raw) => raw.data.get(0).map(|c| *c as char),
+            Self::Notification(_) => Some('J'),
+        }
+    }
+
+    pub fn set_data(&mut self, data: Vec<u8>) {
+        match self {
+            Self::Raw(raw) => raw.data = data,
+            Self::Notification(_) => todo!(),
+        }
+    }
+
+    pub fn set_header(&mut self, header: u32) {
+        match self {
+            Self::Raw(raw) => raw.header = header,
+            Self::Notification(_) => todo!(),
+        }
+    }
+
+    pub fn data_as_string(&self) -> String {
+        String::from_utf8_lossy(&self.get_data()).to_string()
+    }
+}
+
+#[derive(Debug, Clone)]
+pub struct NotificationPacket(String);
+
+impl NotificationPacket {
+    pub fn new<S: Into<String>>(msg: S) -> Self {
+        Self(format!("J{}", msg.into()))
+    }
+}
+
+/// Protocol:
+/// Header: 4 bytes, contains data size
+/// Data: Contains packet data
+#[derive(Clone)]
+pub struct RawPacket {
+    pub header: u32,
+    pub data: Vec<u8>,
+}
+
+impl RawPacket {
+    pub fn from_code(code: char) -> Self {
+        Self {
+            header: 1,
+            data: vec![code as u8],
+        }
+    }
+
+    pub fn from_str(str_data: &str) -> Self {
+        let data = str_data.as_bytes().to_vec();
+        Self {
+            header: data.len() as u32,
+            data: data,
+        }
+    }
+
+    pub fn data_as_string(&self) -> String {
+        String::from_utf8_lossy(&self.data).to_string()
+    }
+}
+
+impl std::fmt::Debug for RawPacket {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> Result<(), std::fmt::Error> {
+        write!(
+            f,
+            "Header: `{:?}` - Bytes: `{:?}` - String: `{}`",
+            self.header,
+            self.data,
+            self.data_as_string()
+        )?;
+        Ok(())
+    }
+}
+
+#[derive(Default, Serialize, Deserialize)]
+pub struct RespawnPacketData {
+    pub pos: RespawnPacketDataPos,
+    pub rot: RespawnPacketDataRot,
+}
+
+#[derive(Default, Serialize, Deserialize)]
+pub struct RespawnPacketDataPos {
+    pub x: f64,
+    pub y: f64,
+    pub z: f64,
+}
+
+#[derive(Default, Serialize, Deserialize)]
+pub struct RespawnPacketDataRot {
+    pub x: f64,
+    pub y: f64,
+    pub z: f64,
+    pub w: f64,
+}
+
+#[derive(Default, Serialize, Deserialize)]
+pub struct TransformPacket {
+    pub rvel: [f64; 3],
+    pub tim: f64,
+    pub pos: [f64; 3],
+    pub ping: f64,
+    pub rot: [f64; 4],
+    pub vel: [f64; 3],
+}