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
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191

use std::{u8, i16};
use std::io::{Read, Seek, SeekFrom};
use byteorder::{ReadBytesExt, BigEndian, LittleEndian};

use {Result as Res, Error, Button};

/// The controller state.
#[derive(Clone, Copy, PartialEq, Debug)]
pub enum State {
	/// The controller is powering on or off.
	Power(bool),

	Idle {
		/// Sequence number for the state.
		sequence: u32,
	},

	Input {
		/// Sequence number for the state.
		sequence: u32,

		/// Button state of the controller.
		buttons: Button,

		/// Trigger state of the controller.
		trigger: Trigger,

		/// Pads state.
		pad: Pad,

		/// Orientation of the controller if sensors are enabled.
		orientation: Angles,

		/// Acceleration of the controller if sensors are enabled.
		acceleration: Angles,
	}
}

/// The pressure on the triggers of the controller.
#[derive(Clone, Copy, PartialEq, Default, Debug)]
pub struct Trigger {
	/// The left trigger.
	pub left: f32,

	/// The right trigger.
	pub right: f32,
}

/// The pads of the controller.
#[derive(Clone, Copy, PartialEq, Eq, Default, Debug)]
pub struct Pad {
	/// The left pad.
	pub left: Axis,

	/// The right pad.
	pub right: Axis,
}

/// Axis on the pad.
#[derive(Clone, Copy, PartialEq, Eq, Default, Debug)]
pub struct Axis {
	/// The X axis.
	pub x: i16,

	/// The Y axis.
	pub y: i16,
}

impl Axis {
	pub fn is_empty(&self) -> bool {
		self.x == 0 && self.y == 0
	}
}

/// 3D position of the controller.
#[derive(Clone, Copy, PartialEq, Eq, Default, Debug)]
pub struct Angles {
	/// The pitch.
	pub pitch: i16,

	/// The roll.
	pub roll: i16,

	/// The yaw.
	pub yaw: i16,
}

impl State {
	/// Parse the state from a given packet.
	pub fn parse<R: Read + Seek>(id: u8, mut buffer: R) -> Res<State> {
		match id {
			0x01 => {
				let sequence = try!(buffer.read_u32::<LittleEndian>());

				let buttons = try!(buffer.read_u32::<BigEndian>());
				let ltrig   = buttons & 0xff;
				let buttons = buttons >> 8;
				let rtrig   = try!(buffer.read_u8());

				try!(buffer.seek(SeekFrom::Current(3)));

				let lpad_x = try!(buffer.read_i16::<LittleEndian>());
				let lpad_y = try!(buffer.read_i16::<LittleEndian>());
				let rpad_x = try!(buffer.read_i16::<LittleEndian>());
				let rpad_y = try!(buffer.read_i16::<LittleEndian>());

				let ltrigp = try!(buffer.read_u16::<LittleEndian>());
				let rtrigp = try!(buffer.read_u16::<LittleEndian>());

				try!(buffer.seek(SeekFrom::Current(8)));

				let apitch = try!(buffer.read_i16::<LittleEndian>());
				let ayaw   = try!(buffer.read_i16::<LittleEndian>());
				let aroll  = try!(buffer.read_i16::<LittleEndian>());

				let opitch = try!(buffer.read_i16::<LittleEndian>());
				let oyaw   = try!(buffer.read_i16::<LittleEndian>());
				let oroll  = try!(buffer.read_i16::<LittleEndian>());

				Ok(State::Input {
					sequence: sequence,

					buttons: try!(Button::from_bits(buttons).ok_or(Error::InvalidParameter)),

					trigger: Trigger {
						left: if ltrigp != 0 {
							ltrigp as f32 / i16::max_value() as f32
						}
						else {
							ltrig as f32 / u8::max_value() as f32
						},

						right: if rtrigp != 0 {
							rtrigp as f32 / i16::max_value() as f32
						}
						else {
							rtrig as f32 / u8::max_value() as f32
						},
					},

					pad: Pad {
						left: Axis {
							x: lpad_x,
							y: lpad_y,
						},

						right: Axis {
							x: rpad_x,
							y: rpad_y,
						},
					},

					orientation: Angles {
						roll:  oroll,
						pitch: opitch,
						yaw:   oyaw,
					},

					acceleration: Angles {
						roll:  aroll,
						pitch: apitch,
						yaw:   ayaw,
					}
				})
			}

			0x03 => {
				let mode = try!(buffer.read_u8());

				Ok(State::Power(match mode {
					0x01 => false,
					0x02 => true,

					_ =>
						return Err(Error::InvalidParameter)
				}))
			}

			0x04 => {
				let sequence = try!(buffer.read_u32::<LittleEndian>());

				Ok(State::Idle {
					sequence: sequence,
				})
			}

			_ =>
				Err(Error::InvalidParameter)
		}
	}
}