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
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123

pub mod tags;

extern crate std;

use self::tags::Tag;

#[derive(Debug)]
pub struct UdpOutPacket {
    pub sequence: u16,
    pub comm_version: u8,
    pub control: Control,
    pub request: Request,
    pub alliance: Alliance,
    pub tags: Vec<Box<Tag>>,
}

#[derive(Debug, Copy, Clone)]
pub enum Alliance {
    Red(u8),
    Blue(u8),
}

bitflags! {
    pub struct Control : u8 {
        const E_STOP = 0b1000_0000;
        const FMS_CONNECTED = 0b0000_1000;
        const ENABLED = 0b0000_0100;

        const AUTONOMOUS = 0b0010;
        const TEST = 0b0001;
        const TELEOP = 0b0000;
    }
}

bitflags! {
    pub struct Request : u8 {
        const REBOOT_RIO = 0b1000;
        const RESTART_CODE = 0b0100;
    }
}

impl UdpOutPacket {
    pub fn new(
        control: Control,
        request: Request,
        alliance: Alliance,
        tags: Vec<Box<Tag>>,
    ) -> UdpOutPacket {
        UdpOutPacket {
            sequence: 0,
            comm_version: 0x01,
            control,
            request,
            alliance,
            tags,
        }
    }

    pub fn next(&mut self) -> Vec<u8> {
        self.sequence += 1;
        self.as_u8_vec()
    }

    pub fn as_u8_vec(&self) -> Vec<u8> {
        let mut vec: Vec<u8> = Vec::new();
        vec.push((self.sequence >> 8) as u8);
        vec.push((self.sequence & 0xff) as u8);
        vec.push(self.comm_version);
        vec.push(self.control.bits);
        vec.push(self.request.bits);
        vec.push(self.alliance.to_u8());
        for item in &self.tags {
            vec.extend_from_slice((*item).construct().as_slice())
        }

        vec
    }
}

impl Alliance {
    pub fn new(is_red: bool, position: u8) -> Alliance {
        if is_red {
            Alliance::Red(position)
        } else {
            Alliance::Blue(position)
        }
    }

    pub fn to_u8(&self) -> u8 {
        match *self {
            //TODO this is wrong I think
            Alliance::Red(a) => a - 1,
            Alliance::Blue(a) => a + 2,
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn red_alliance() {
        assert_eq!(Alliance::Red(1).to_u8(), 0);
        assert_eq!(Alliance::Red(3).to_u8(), 2);
    }

    #[test]
    fn blue_alliance() {
        assert_eq!(Alliance::Blue(1).to_u8(), 3);
        assert_eq!(Alliance::Blue(3).to_u8(), 5);
    }

    #[test]
    fn control() {
        assert_eq!(Control::all(), Control::from_bits_truncate(0b1000_1111));
    }

    #[test]
    fn request() {
        assert_eq!(Request::all(), Request::from_bits_truncate(0b1100));
    }
}