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

#![feature(nll)]

pub mod ds_to_rio;
pub mod rio_to_ds;

#[macro_use]
extern crate bitflags;
extern crate byteorder;
extern crate failure;

use std::net::{UdpSocket, SocketAddr, IpAddr};

pub type Result<T> = std::result::Result<T, failure::Error>;

pub struct RoboRio {
    pub target_ip: IpAddr,
    udp_out: UdpSocket,
    udp_in: UdpSocket,
    //tcp_stream: TcpStream,
}

impl RoboRio {
    pub fn new(team_number: u32) -> std::io::Result<RoboRio> {
        Ok(RoboRio {
            target_ip: ip_from_team_number(team_number).parse().unwrap(),
            udp_out: UdpSocket::bind("0.0.0.0:0")?,
            udp_in: UdpSocket::bind("0.0.0.0:1150")?,
            //tcp_stream: TcpStream::connect(SocketAddr::from((ip, 1740)))?,
        })
    }

    pub fn new_from_ip(rio_ip: [u8; 4]) -> std::io::Result<RoboRio> {
        Ok(RoboRio {
            target_ip: IpAddr::from(rio_ip),
            udp_out: UdpSocket::bind("0.0.0.0:0")?,
            udp_in: UdpSocket::bind("0.0.0.0:1150")?,
        })
    }

    pub fn send_udp(&self, packet: Vec<u8>) -> std::io::Result<()> {
        self.udp_out.send_to(packet.as_slice(), SocketAddr::new(self.target_ip, 1110))?; // TODO add sending to 1115 if fms is connected
        Ok(())
    }

    pub fn recv_udp(&self, buf: &mut [u8]) -> std::io::Result<()> {
        self.udp_in.recv_from(buf)?;
        Ok(())
    }
}

// unabashedly stolen directly from Rhys Kenwell
pub fn ip_from_team_number(team: u32) -> String {
    let s = team.to_string();

    match s.len() {
        1 | 2 => format!("10.0.{}.2", team),
        3 => format!("10.{}.{}.2", &s[0..1], &s[1..3]),
        4 => format!("10.{}.{}.2", &s[0..2], &s[2..4]),
        _ => unreachable!()
    }
}