candid_server 0.3.1

use std::io::{BufReader, Read, Write};
use std::net::TcpStream;
use std::sync::mpsc;
use std::thread;

use byteorder::{NetworkEndian, ReadBytesExt, WriteBytesExt};
use socketcan::{CanFrame, CanSocket, EmbeddedFrame, ExtendedId, Socket};

/// Spawns two threads: one to read incoming frames from the client (and write them to the
/// CanSocket), and one to read incoming frames from the `CANReader` (and write them to the client)
pub fn new_client_handler(
    mut connection: TcpStream,
    can_interface: String,
    can_reader: mpsc::Receiver<CanFrame>,
) -> (thread::JoinHandle<()>, thread::JoinHandle<()>) {
    let connection_clone = connection.try_clone().unwrap();
    // Thread to pass frames from the server to the client
    let tx_handle = thread::spawn(move || {
        let ip = connection.peer_addr().unwrap();

        loop {
            // Get an incoming frame from the reader
            let frame = can_reader.recv().unwrap();

            // Relay to the client
            if send_frame(&mut connection, frame).is_err() {
                println!("Connection to client {:?} dropped.", ip);
                break;
            }
        }
    });

    // Thread to pass frames from the client to the CAN socket
    let rx_handle = thread::spawn(move || {
        let ip = connection_clone.peer_addr().unwrap();
        let mut connection = BufReader::new(connection_clone);

        // Possible issue: There is a new socket for each thread
        let can_socket = CanSocket::open(&can_interface).unwrap();

        loop {
            // Get an incoming frame from the client
            let Some(id) = connection
                .read_u32::<NetworkEndian>()
                .ok()
                .and_then(ExtendedId::new)
            else {
                println!("Connection to client {:?} dropped.", ip);
                break;
            };

            let mut data = [0 as u8; 8];
            if connection.read_exact(&mut data).is_err() {
                println!("Connection to client {:?} dropped.", ip);
                break;
            }

            // Write the frame to the CAN Socket
            let frame = CanFrame::new(id, &data).unwrap();
            can_socket.write_frame_insist(&frame).unwrap();
        }
    });

    (tx_handle, rx_handle)
}

fn send_frame(connection: &mut TcpStream, frame: CanFrame) -> Result<(), std::io::Error> {
    let id = match frame.id() {
        socketcan::Id::Standard(id) => id.as_raw() as u32,
        socketcan::Id::Extended(id) => id.as_raw(),
    };
    connection.write_u32::<NetworkEndian>(id)?;
    connection.write_all(frame.data())?;
    connection.flush().unwrap();
    Ok(())
}