<commit_before>//
// FlightVars
// Copyright (c) 2015, 2016 Alvaro Polo
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

use std::fmt;
use std::io;
use std::str::FromStr;

use byteorder::{BigEndian, ReadBytesExt};
use hex::FromHex;

#[derive(Debug, PartialEq)]
pub struct OffsetAddr(u16);

impl FromHex for OffsetAddr {
    type Error = io::Error;
    fn from_hex(s: &str) -> Result<Self, Self::Error> {
        let error = || io::Error::new(
            io::ErrorKind::InvalidInput,
            format!("invalid FSUIPC offset address in '{}'", s));
        let buf: Vec<u8> = try!(FromHex::from_hex(s).map_err(|_| error()));
        if buf.len() > 2 { Err(error()) }
        else {
            (&buf[..]).read_u16::<BigEndian>()
                .map(|v| OffsetAddr(v))
                .map_err(|_| error())
        }
    }
}

impl fmt::Display for OffsetAddr {
    fn fmt(&self, f: &mut fmt::Formatter) -> Result<(), fmt::Error> {
        write!(f, "{:x}", self.0)
    }
}

#[derive(Debug, PartialEq)]
pub enum OffsetLen { Ub, Sb, Uw, Sw, Ud, Sd }

impl fmt::Display for OffsetLen {
    fn fmt(&self, f: &mut fmt::Formatter) -> Result<(), fmt::Error> {
        match *self {
            OffsetLen::Ub => write!(f, "UB"),
            OffsetLen::Sb => write!(f, "SB"),
            OffsetLen::Uw => write!(f, "UW"),
            OffsetLen::Sw => write!(f, "SW"),
            OffsetLen::Ud => write!(f, "UD"),
            OffsetLen::Sd => write!(f, "SD"),
        }
    }
}

impl FromStr for OffsetLen {
    type Err = io::Error;
    fn from_str(s: &str) -> Result<OffsetLen, io::Error> {
        match s {
            "UB" => Ok(OffsetLen::Ub),
            "SB" => Ok(OffsetLen::Sb),
            "UW" => Ok(OffsetLen::Uw),
            "SW" => Ok(OffsetLen::Sw),
            "UD" => Ok(OffsetLen::Ud),
            "SD" => Ok(OffsetLen::Sd),
            _ => Err(io::Error::new(
                io::ErrorKind::InvalidInput,
                format!("invalid FSUIPC offset length in '{}'", s))),
        }
    }
}

#[derive(Debug, PartialEq)]
pub struct Offset(OffsetAddr, OffsetLen);

impl fmt::Display for Offset {
    fn fmt(&self, f: &mut fmt::Formatter) -> Result<(), fmt::Error> {
        write!(f, "{}:{}", self.0, self.1)
    }
}

impl FromStr for Offset {
    type Err = io::Error;
    fn from_str(s: &str) -> Result<Offset, io::Error> {
        let pair: Vec<&str> = s.split(":").collect();
        if pair.len() == 2 {
            let addr = try!(OffsetAddr::from_hex(pair[0]));
            let len = try!(OffsetLen::from_str(pair[1]));
            Ok(Offset(addr, len))
        } else {
            Err(io::Error::new(
                io::ErrorKind::InvalidInput,
                format!("invalid FSUIPC offset in '{}'", s)))
        }
    }
}

#[derive(Debug, PartialEq)]
pub enum Message {
    Begin { version: u16, client_id: String },
    WriteLvar { lvar: String, value: i64 },
    WriteOffset { offset: Offset, value: i64 },
    ObserveLvar { lvar: String },
    ObserveOffset { offset: Offset },
    EventLvar { lvar: String, value: i64 },
    EventOffset { offset: OffsetAddr, value: i64 }
}

impl Message {

    pub fn begin(version: u16, client_id: &str) -> Message {
        Message::Begin { version: version, client_id: client_id.to_string() }
    }

    pub fn write_lvar(lvar: &str, value: i64) -> Message {
        Message::WriteLvar { lvar: lvar.to_string(), value: value }
    }

    pub fn write_offset(offset: Offset, value: i64) -> Message {
        Message::WriteOffset { offset: offset, value: value }
    }

    pub fn obs_lvar(lvar: &str) -> Message {
        Message::ObserveLvar { lvar: lvar.to_string() }
    }

    pub fn obs_offset(offset: Offset) -> Message {
        Message::ObserveOffset { offset: offset }
    }

    pub fn event_lvar(lvar: &str, value: i64) -> Message {
        Message::EventLvar { lvar: lvar.to_string(), value: value }
    }

    pub fn event_offset(offset: OffsetAddr, value: i64) -> Message {
        Message::EventOffset { offset: offset, value: value }
    }
}

impl fmt::Display for Message {
    fn fmt(&self, f: &mut fmt::Formatter) -> Result<(), fmt::Error> {
        match self {
            &Message::Begin { version, ref client_id } =>
                write!(f, "BEGIN {} {}", version, client_id),
            &Message::WriteLvar { ref lvar, value} =>
                write!(f, "WRITE_LVAR {} {}", lvar, value),
            &Message::WriteOffset { ref offset, value } =>
                write!(f, "WRITE_OFFSET {} {}", offset, value),
            &Message::ObserveLvar { ref lvar } =>
                write!(f, "OBS_LVAR {}", lvar),
            &Message::ObserveOffset { ref offset } =>
                write!(f, "OBS_OFFSET {}", offset),
            &Message::EventLvar { ref lvar, value } =>
                write!(f, "EVENT_LVAR {} {}", lvar, value),
            &Message::EventOffset { ref offset, value } =>
                write!(f, "EVENT_OFFSET {} {}", offset, value),
        }
    }
}

impl FromStr for Message {
    type Err = io::Error;
    fn from_str(s: &str) -> Result<Message, io::Error> {
        let deco = MessageParser::new(s);
        deco.parse()
    }
}

pub struct MessageParser<'a> {
    input: &'a str
}

impl<'a> MessageParser<'a> {
    pub fn new(input: &'a str) -> MessageParser {
        MessageParser { input: input }
    }

    pub fn parse(self) -> io::Result<Message> {
        let args: Vec<&str> = self.input.split_whitespace().collect();
        let cmd = args[0];
        let args = &args[1..];
        match &cmd.to_uppercase()[..] {
            "BEGIN" => self.parse_begin(&args),
            "WRITE_LVAR" => self.parse_write_lvar(&args),
            "WRITE_OFFSET" => self.parse_write_offset(&args),
            "OBS_LVAR" => self.parse_obs_lvar(&args),
            "OBS_OFFSET" => self.parse_obs_offset(&args),
            "EVENT_LVAR" => self.parse_event_lvar(&args),
            "EVENT_OFFSET" => self.parse_event_offset(&args),
            _ => Err(self.input_error()),
        }
    }

    fn parse_begin(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let ver = try!(args[0].parse().map_err(|_| self.input_error()));
        Ok(Message::begin(ver, args[1]))
    }

    fn parse_write_lvar(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let lvar = args[0];
        let value = try!(args[1].parse().map_err(|_| self.input_error()));
        Ok(Message::write_lvar(&lvar, value))
    }

    fn parse_write_offset(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let offset = try!(args[0].parse());
        let value = try!(args[1].parse().map_err(|_| self.input_error()));
        Ok(Message::write_offset(offset, value))
    }

    fn parse_obs_lvar(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 1));
        Ok(Message::obs_lvar(args[0]))
    }

    fn parse_obs_offset(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 1));
        Ok(Message::obs_offset(try!(args[0].parse())))
    }

    fn parse_event_lvar(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let lvar = args[0];
        let value = try!(args[1].parse().map_err(|_| self.input_error()));
        Ok(Message::event_lvar(lvar, value))
    }

    fn parse_event_offset(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let offset = try!(OffsetAddr::from_hex(args[0]).map_err(|_| self.input_error()));
        let value = try!(args[1].parse().map_err(|_| self.input_error()));
        Ok(Message::event_offset(offset, value))
    }

    fn require_argc(&self, args: &[&str], expected: usize) -> io::Result<()> {
        if args.len() == expected { Ok(()) }
        else { Err(self.input_error()) }
    }

    fn input_error(&self) -> io::Error {
        io::Error::new(
            io::ErrorKind::InvalidInput,
            format!("invalid oacsp message in '{}'", self.input))
    }
}

#[cfg(test)]
mod tests {
    use std::str::FromStr;

    use super::*;

    #[test]
    fn should_display_begin_msg() {
        let msg = Message::begin(1, "arduino");
        let buf = format!("{}", msg);
        assert_eq!(buf, "BEGIN 1 arduino")
    }

    #[test]
    fn should_parse_begin_msg() {
        let buf = "BEGIN 1 arduino";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::begin(1, "arduino"));
    }

    #[test]
    fn should_display_write_lvar_msg() {
        let msg = Message::write_lvar("foobar", 42);
        let buf = format!("{}", msg);
        assert_eq!(buf, "WRITE_LVAR foobar 42")
    }

    #[test]
    fn should_parse_write_lvar_msg() {
        let buf = "WRITE_LVAR foobar 42";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::write_lvar("foobar", 42));
    }

    #[test]
    fn should_display_write_offset_msg() {
        let msg = Message::write_offset(Offset(OffsetAddr(0x1234), OffsetLen::Uw), 42);
        let buf = format!("{}", msg);
        assert_eq!(buf, "WRITE_OFFSET 1234:UW 42")
    }

    #[test]
    fn should_parse_write_offset_msg() {
        let buf = "WRITE_OFFSET 1234:UW 42";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::write_offset(Offset(OffsetAddr(0x1234), OffsetLen::Uw), 42));
    }

    #[test]
    fn should_display_obs_lvar_msg() {
        let msg = Message::obs_lvar("foobar");
        let buf = format!("{}", msg);
        assert_eq!(buf, "OBS_LVAR foobar")
    }

    #[test]
    fn should_parse_obs_lvar_msg() {
        let buf = "OBS_LVAR foobar";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::obs_lvar("foobar"));
    }

    #[test]
    fn should_display_obs_offset_msg() {
        let msg = Message::obs_offset(Offset(OffsetAddr(0x1234), OffsetLen::Uw));
        let buf = format!("{}", msg);
        assert_eq!(buf, "OBS_OFFSET 1234:UW")
    }

    #[test]
    fn should_parse_obs_offset_msg() {
        let buf = "OBS_OFFSET 1234:UW";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::obs_offset(Offset(OffsetAddr(0x1234), OffsetLen::Uw)));
    }

    #[test]
    fn should_display_event_lvar_msg() {
        let msg = Message::event_lvar("foobar", 42);
        let buf = format!("{}", msg);
        assert_eq!(buf, "EVENT_LVAR foobar 42")
    }

    #[test]
    fn should_parse_event_lvar_msg() {
        let buf = "EVENT_LVAR foobar 42";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::event_lvar("foobar", 42));
    }

    #[test]
    fn should_display_event_offset_msg() {
        let msg = Message::event_offset(OffsetAddr(0x1234), 42);
        let buf = format!("{}", msg);
        assert_eq!(buf, "EVENT_OFFSET 1234 42")
    }

    #[test]
    fn should_parse_event_offset_msg() {
        let buf = "EVENT_OFFSET 1234 42";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::event_offset(OffsetAddr(0x1234), 42));
    }
}
<commit_msg>Introduce `Value` type in `oacsp`<commit_after>//
// FlightVars
// Copyright (c) 2015, 2016 Alvaro Polo
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

use std::fmt;
use std::io;
use std::str::FromStr;

use byteorder::{BigEndian, ReadBytesExt};
use hex::FromHex;

/// The value of a data transferred using OACSP
pub type Value = i32;

#[derive(Debug, PartialEq)]
pub struct OffsetAddr(u16);

impl FromHex for OffsetAddr {
    type Error = io::Error;
    fn from_hex(s: &str) -> Result<Self, Self::Error> {
        let error = || io::Error::new(
            io::ErrorKind::InvalidInput,
            format!("invalid FSUIPC offset address in '{}'", s));
        let buf: Vec<u8> = try!(FromHex::from_hex(s).map_err(|_| error()));
        if buf.len() > 2 { Err(error()) }
        else {
            (&buf[..]).read_u16::<BigEndian>()
                .map(|v| OffsetAddr(v))
                .map_err(|_| error())
        }
    }
}

impl fmt::Display for OffsetAddr {
    fn fmt(&self, f: &mut fmt::Formatter) -> Result<(), fmt::Error> {
        write!(f, "{:x}", self.0)
    }
}

#[derive(Debug, PartialEq)]
pub enum OffsetLen { Ub, Sb, Uw, Sw, Ud, Sd }

impl fmt::Display for OffsetLen {
    fn fmt(&self, f: &mut fmt::Formatter) -> Result<(), fmt::Error> {
        match *self {
            OffsetLen::Ub => write!(f, "UB"),
            OffsetLen::Sb => write!(f, "SB"),
            OffsetLen::Uw => write!(f, "UW"),
            OffsetLen::Sw => write!(f, "SW"),
            OffsetLen::Ud => write!(f, "UD"),
            OffsetLen::Sd => write!(f, "SD"),
        }
    }
}

impl FromStr for OffsetLen {
    type Err = io::Error;
    fn from_str(s: &str) -> Result<OffsetLen, io::Error> {
        match s {
            "UB" => Ok(OffsetLen::Ub),
            "SB" => Ok(OffsetLen::Sb),
            "UW" => Ok(OffsetLen::Uw),
            "SW" => Ok(OffsetLen::Sw),
            "UD" => Ok(OffsetLen::Ud),
            "SD" => Ok(OffsetLen::Sd),
            _ => Err(io::Error::new(
                io::ErrorKind::InvalidInput,
                format!("invalid FSUIPC offset length in '{}'", s))),
        }
    }
}

#[derive(Debug, PartialEq)]
pub struct Offset(OffsetAddr, OffsetLen);

impl fmt::Display for Offset {
    fn fmt(&self, f: &mut fmt::Formatter) -> Result<(), fmt::Error> {
        write!(f, "{}:{}", self.0, self.1)
    }
}

impl FromStr for Offset {
    type Err = io::Error;
    fn from_str(s: &str) -> Result<Offset, io::Error> {
        let pair: Vec<&str> = s.split(":").collect();
        if pair.len() == 2 {
            let addr = try!(OffsetAddr::from_hex(pair[0]));
            let len = try!(OffsetLen::from_str(pair[1]));
            Ok(Offset(addr, len))
        } else {
            Err(io::Error::new(
                io::ErrorKind::InvalidInput,
                format!("invalid FSUIPC offset in '{}'", s)))
        }
    }
}

#[derive(Debug, PartialEq)]
pub enum Message {
    Begin { version: u16, client_id: String },
    WriteLvar { lvar: String, value: Value },
    WriteOffset { offset: Offset, value: Value },
    ObserveLvar { lvar: String },
    ObserveOffset { offset: Offset },
    EventLvar { lvar: String, value: Value },
    EventOffset { offset: OffsetAddr, value: Value }
}

impl Message {

    pub fn begin(version: u16, client_id: &str) -> Message {
        Message::Begin { version: version, client_id: client_id.to_string() }
    }

    pub fn write_lvar(lvar: &str, value: Value) -> Message {
        Message::WriteLvar { lvar: lvar.to_string(), value: value }
    }

    pub fn write_offset(offset: Offset, value: Value) -> Message {
        Message::WriteOffset { offset: offset, value: value }
    }

    pub fn obs_lvar(lvar: &str) -> Message {
        Message::ObserveLvar { lvar: lvar.to_string() }
    }

    pub fn obs_offset(offset: Offset) -> Message {
        Message::ObserveOffset { offset: offset }
    }

    pub fn event_lvar(lvar: &str, value: Value) -> Message {
        Message::EventLvar { lvar: lvar.to_string(), value: value }
    }

    pub fn event_offset(offset: OffsetAddr, value: Value) -> Message {
        Message::EventOffset { offset: offset, value: value }
    }
}

impl fmt::Display for Message {
    fn fmt(&self, f: &mut fmt::Formatter) -> Result<(), fmt::Error> {
        match self {
            &Message::Begin { version, ref client_id } =>
                write!(f, "BEGIN {} {}", version, client_id),
            &Message::WriteLvar { ref lvar, value} =>
                write!(f, "WRITE_LVAR {} {}", lvar, value),
            &Message::WriteOffset { ref offset, value } =>
                write!(f, "WRITE_OFFSET {} {}", offset, value),
            &Message::ObserveLvar { ref lvar } =>
                write!(f, "OBS_LVAR {}", lvar),
            &Message::ObserveOffset { ref offset } =>
                write!(f, "OBS_OFFSET {}", offset),
            &Message::EventLvar { ref lvar, value } =>
                write!(f, "EVENT_LVAR {} {}", lvar, value),
            &Message::EventOffset { ref offset, value } =>
                write!(f, "EVENT_OFFSET {} {}", offset, value),
        }
    }
}

impl FromStr for Message {
    type Err = io::Error;
    fn from_str(s: &str) -> Result<Message, io::Error> {
        let deco = MessageParser::new(s);
        deco.parse()
    }
}

pub struct MessageParser<'a> {
    input: &'a str
}

impl<'a> MessageParser<'a> {
    pub fn new(input: &'a str) -> MessageParser {
        MessageParser { input: input }
    }

    pub fn parse(self) -> io::Result<Message> {
        let args: Vec<&str> = self.input.split_whitespace().collect();
        let cmd = args[0];
        let args = &args[1..];
        match &cmd.to_uppercase()[..] {
            "BEGIN" => self.parse_begin(&args),
            "WRITE_LVAR" => self.parse_write_lvar(&args),
            "WRITE_OFFSET" => self.parse_write_offset(&args),
            "OBS_LVAR" => self.parse_obs_lvar(&args),
            "OBS_OFFSET" => self.parse_obs_offset(&args),
            "EVENT_LVAR" => self.parse_event_lvar(&args),
            "EVENT_OFFSET" => self.parse_event_offset(&args),
            _ => Err(self.input_error()),
        }
    }

    fn parse_begin(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let ver = try!(args[0].parse().map_err(|_| self.input_error()));
        Ok(Message::begin(ver, args[1]))
    }

    fn parse_write_lvar(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let lvar = args[0];
        let value = try!(args[1].parse().map_err(|_| self.input_error()));
        Ok(Message::write_lvar(&lvar, value))
    }

    fn parse_write_offset(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let offset = try!(args[0].parse());
        let value = try!(args[1].parse().map_err(|_| self.input_error()));
        Ok(Message::write_offset(offset, value))
    }

    fn parse_obs_lvar(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 1));
        Ok(Message::obs_lvar(args[0]))
    }

    fn parse_obs_offset(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 1));
        Ok(Message::obs_offset(try!(args[0].parse())))
    }

    fn parse_event_lvar(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let lvar = args[0];
        let value = try!(args[1].parse().map_err(|_| self.input_error()));
        Ok(Message::event_lvar(lvar, value))
    }

    fn parse_event_offset(self, args: &[&str]) -> io::Result<Message> {
        try!(self.require_argc(args, 2));
        let offset = try!(OffsetAddr::from_hex(args[0]).map_err(|_| self.input_error()));
        let value = try!(args[1].parse().map_err(|_| self.input_error()));
        Ok(Message::event_offset(offset, value))
    }

    fn require_argc(&self, args: &[&str], expected: usize) -> io::Result<()> {
        if args.len() == expected { Ok(()) }
        else { Err(self.input_error()) }
    }

    fn input_error(&self) -> io::Error {
        io::Error::new(
            io::ErrorKind::InvalidInput,
            format!("invalid oacsp message in '{}'", self.input))
    }
}

#[cfg(test)]
mod tests {
    use std::str::FromStr;

    use super::*;

    #[test]
    fn should_display_begin_msg() {
        let msg = Message::begin(1, "arduino");
        let buf = format!("{}", msg);
        assert_eq!(buf, "BEGIN 1 arduino")
    }

    #[test]
    fn should_parse_begin_msg() {
        let buf = "BEGIN 1 arduino";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::begin(1, "arduino"));
    }

    #[test]
    fn should_display_write_lvar_msg() {
        let msg = Message::write_lvar("foobar", 42);
        let buf = format!("{}", msg);
        assert_eq!(buf, "WRITE_LVAR foobar 42")
    }

    #[test]
    fn should_parse_write_lvar_msg() {
        let buf = "WRITE_LVAR foobar 42";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::write_lvar("foobar", 42));
    }

    #[test]
    fn should_display_write_offset_msg() {
        let msg = Message::write_offset(Offset(OffsetAddr(0x1234), OffsetLen::Uw), 42);
        let buf = format!("{}", msg);
        assert_eq!(buf, "WRITE_OFFSET 1234:UW 42")
    }

    #[test]
    fn should_parse_write_offset_msg() {
        let buf = "WRITE_OFFSET 1234:UW 42";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::write_offset(Offset(OffsetAddr(0x1234), OffsetLen::Uw), 42));
    }

    #[test]
    fn should_display_obs_lvar_msg() {
        let msg = Message::obs_lvar("foobar");
        let buf = format!("{}", msg);
        assert_eq!(buf, "OBS_LVAR foobar")
    }

    #[test]
    fn should_parse_obs_lvar_msg() {
        let buf = "OBS_LVAR foobar";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::obs_lvar("foobar"));
    }

    #[test]
    fn should_display_obs_offset_msg() {
        let msg = Message::obs_offset(Offset(OffsetAddr(0x1234), OffsetLen::Uw));
        let buf = format!("{}", msg);
        assert_eq!(buf, "OBS_OFFSET 1234:UW")
    }

    #[test]
    fn should_parse_obs_offset_msg() {
        let buf = "OBS_OFFSET 1234:UW";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::obs_offset(Offset(OffsetAddr(0x1234), OffsetLen::Uw)));
    }

    #[test]
    fn should_display_event_lvar_msg() {
        let msg = Message::event_lvar("foobar", 42);
        let buf = format!("{}", msg);
        assert_eq!(buf, "EVENT_LVAR foobar 42")
    }

    #[test]
    fn should_parse_event_lvar_msg() {
        let buf = "EVENT_LVAR foobar 42";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::event_lvar("foobar", 42));
    }

    #[test]
    fn should_display_event_offset_msg() {
        let msg = Message::event_offset(OffsetAddr(0x1234), 42);
        let buf = format!("{}", msg);
        assert_eq!(buf, "EVENT_OFFSET 1234 42")
    }

    #[test]
    fn should_parse_event_offset_msg() {
        let buf = "EVENT_OFFSET 1234 42";
        let msg = Message::from_str(&buf).unwrap();
        assert_eq!(msg, Message::event_offset(OffsetAddr(0x1234), 42));
    }
}
<|endoftext|>


<commit_before>// rouler - A container-based system for generating die rolls
// Copyright (C) 2016 by John Berry
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

use pest::{
    prec_climber::*,
    iterators::*,
};
use random::*;

lazy_static! {
    static ref PREC_CLIMBER: PrecClimber<Rule> = {
        use self::Assoc::*;
        use self::Rule::*;

        // Order of precedence: "+-" is less than "*/" is less than "dD"
        PrecClimber::new(vec![
            Operator::new(plus, Left) | Operator::new(minus, Left),
            Operator::new(times, Left) | Operator::new(slash, Left),
            Operator::new(roll, Right),
        ])
    };
}

#[derive(Parser)]
#[grammar = "rouler.pest"]
pub struct RollParser;

// Force recompile when parse changes
const _GRAMMAR : &'static str = include_str!("rouler.pest");

pub fn compute(expr: Pairs<Rule>) -> i64 {
    PREC_CLIMBER.climb(
        expr,
        |pair: Pair<Rule>| match pair.as_rule() {
            Rule::number => pair.as_str().parse::<i64>().unwrap().into(),
            Rule::expr => compute(pair.into_inner()),
            Rule::custom_dice => {
                let mut inner = pair.into_inner();
                // LHS
                let num = inner.next().unwrap();
                let lhs = num.as_str().parse::<i64>().expect("Did not find a number on LHS!");
                // Operator
                let d = inner.next().unwrap().as_str();
                assert!(d == "d" || d == "D");
                // RHS
                let sides : Vec<u64> = inner.next().unwrap().as_str().trim_matches(|c| c == '[' || c == ']')
                    .split(",").map(|s| {
                        s.trim().parse::<u64>().expect("Did not find a number on RHS!")
                    })
                    .collect();
                lhs.signum() * roll_custom_dice_raw(lhs.abs(), &sides)
            },
            _ => unreachable!(),
        },
        |lhs: i64, op: Pair<Rule>, rhs: i64| match op.as_rule() {
            Rule::roll => {
                if rhs < 1 {
                    panic!("Sides must be greater than zero")
                } else {
                    match lhs.signum() {
                        0 => panic!("Number of sides must not be zero"),
                        -1 => -roll_dice_raw(lhs.abs(), rhs as u64),
                        1 => roll_dice_raw(lhs.abs(), rhs as u64),
                        _ => unreachable!(),
                    }
                }
            },
            Rule::plus => lhs + rhs,
            Rule::minus => lhs - rhs,
            Rule::times => lhs * rhs,
            Rule::slash => lhs / rhs,
            _ => unreachable!(),
        }
    )
}
<commit_msg>Fix arbitrary sides parse grabbing first number<commit_after>// rouler - A container-based system for generating die rolls
// Copyright (C) 2016 by John Berry
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

use pest::{
    prec_climber::*,
    iterators::*,
};
use random::*;

lazy_static! {
    static ref PREC_CLIMBER: PrecClimber<Rule> = {
        use self::Assoc::*;
        use self::Rule::*;

        // Order of precedence: "+-" is less than "*/" is less than "dD"
        PrecClimber::new(vec![
            Operator::new(plus, Left) | Operator::new(minus, Left),
            Operator::new(times, Left) | Operator::new(slash, Left),
            Operator::new(roll, Right),
        ])
    };
}

#[derive(Parser)]
#[grammar = "rouler.pest"]
pub struct RollParser;

// Force recompile when parse changes
const _GRAMMAR : &'static str = include_str!("rouler.pest");

pub fn compute(expr: Pairs<Rule>) -> i64 {
    PREC_CLIMBER.climb(
        expr,
        |pair: Pair<Rule>| match pair.as_rule() {
            Rule::number => pair.as_str().parse::<i64>().unwrap().into(),
            Rule::expr => compute(pair.into_inner()),
            Rule::custom_dice => {
                let mut inner = pair.into_inner();
                // LHS
                let num = inner.next().unwrap();
                let lhs = num.as_str().parse::<i64>().expect("Did not find a number on LHS!");
                // Operator
                let d = inner.next().unwrap().as_str();
                assert!(d == "d" || d == "D");
                // RHS
                let mut sides = vec![];
                while let Some(s) = inner.next() {
                    // Collect numbers
                    if s.as_rule() == Rule::number {
                        sides.push(s.as_str().parse::<u64>().expect("Non-number found on RHS!"));
                    }
                }
                lhs.signum() * roll_custom_dice_raw(lhs.abs(), &sides)
            },
            _ => unreachable!(),
        },
        |lhs: i64, op: Pair<Rule>, rhs: i64| match op.as_rule() {
            Rule::roll => {
                if rhs < 1 {
                    panic!("Sides must be greater than zero")
                } else {
                    match lhs.signum() {
                        0 => panic!("Number of sides must not be zero"),
                        -1 => -roll_dice_raw(lhs.abs(), rhs as u64),
                        1 => roll_dice_raw(lhs.abs(), rhs as u64),
                        _ => unreachable!(),
                    }
                }
            },
            Rule::plus => lhs + rhs,
            Rule::minus => lhs - rhs,
            Rule::times => lhs * rhs,
            Rule::slash => lhs / rhs,
            _ => unreachable!(),
        }
    )
}
<|endoftext|>


<commit_before>use crate::route_guide::Feature;
use crate::route_guide::Point;
use crate::route_guide::Rectangle;
use crate::route_guide::RouteNote;
use crate::route_guide::RouteSummary;
use crate::route_guide_grpc::RouteGuide;

use futures::future;

use futures::stream;
use futures::TryFutureExt;
use futures::TryStreamExt;

use futures::stream::StreamExt;

use grpc::Metadata;
use grpc::ServerHandlerContext;
use grpc::ServerRequest;
use grpc::ServerRequestSingle;
use grpc::ServerResponseSink;
use grpc::ServerResponseUnarySink;
use json::JsonValue;
use std::collections::HashMap;
use std::f64;
use std::fs;
use std::io::Read;
use std::path::Path;
use std::sync::Arc;
use std::sync::Mutex;
use std::task::Poll;
use std::time::Instant;

// https://github.com/grpc/grpc-go/blob/master/examples/route_guide/server/server.go
#[derive(Default)]
pub struct RouteGuideImpl {
    saved_features: Arc<Vec<Feature>>,
    route_notes: Arc<Mutex<HashMap<String, Vec<RouteNote>>>>,
}

impl RouteGuideImpl {
    pub fn new_and_load_db() -> RouteGuideImpl {
        RouteGuideImpl {
            saved_features: Arc::new(load_features(Path::new(ROUTE_GUIDE_DB_PATH))),
            route_notes: Default::default(),
        }
    }
}

impl RouteGuide for RouteGuideImpl {
    fn get_feature(
        &self,
        _o: ServerHandlerContext,
        req: ServerRequestSingle<Point>,
        resp: ServerResponseUnarySink<Feature>,
    ) -> grpc::Result<()> {
        for feature in &*self.saved_features {
            if feature.get_location() == &req.message {
                return resp.finish(feature.clone());
            }
        }

        resp.finish(Feature {
            location: Some(req.message).into(),
            ..Default::default()
        })
    }

    fn list_features(
        &self,
        o: ServerHandlerContext,
        mut req: ServerRequestSingle<Rectangle>,
        mut resp: ServerResponseSink<Feature>,
    ) -> grpc::Result<()> {
        let req = req.take_message();
        let saved_features = self.saved_features.clone();
        o.spawn(|| {
            for feature in &saved_features[..] {
                resp.ready().await?;
                resp.send_data(feature.clone())?;
            }
            resp.send_trailers(Metadata::new())
        });
        Ok(())
    }

    fn record_route(
        &self,
        o: ServerHandlerContext,
        req: ServerRequest<Point>,
        resp: ServerResponseUnarySink<RouteSummary>,
    ) -> grpc::Result<()> {
        let start_time = Instant::now();

        struct State {
            point_count: u32,
            feature_count: u32,
            distance: u32,
            last_point: Option<Point>,
        }

        let mut state = State {
            point_count: 0,
            feature_count: 0,
            distance: 0,
            last_point: None,
        };

        let saved_features = self.saved_features.clone();

        o.spawn(|| {
            let mut stream = req.into_stream();
            while let Some(point) = stream.next() {
                let point = point?;
                state.point_count += 1;
                for feature in &saved_features[..] {
                    if feature.get_location() == &point {
                        state.feature_count += 1;
                    }
                }
                if let Some(last_point) = &state.last_point {
                    state.distance += calc_distance(last_point, &point);
                }
                state.last_point = Some(point);
            }

            resp.finish(RouteSummary {
                point_count: state.point_count as i32,
                feature_count: state.feature_count as i32,
                distance: state.distance as i32,
                elapsed_time: start_time.elapsed().as_secs() as i32,
                ..Default::default()
            })
        });

        Ok(())
    }

    fn route_chat(
        &self,
        o: ServerHandlerContext,
        req: ServerRequest<RouteNote>,
        mut resp: ServerResponseSink<RouteNote>,
    ) -> grpc::Result<()> {
        let mut req = req.into_stream();

        let route_notes_map = self.route_notes.clone();

        o.spawn_poll_fn(move |cx| {
            loop {
                // Wait until resp is writable
                if let Poll::Pending = resp.poll(cx)? {
                    return Poll::Pending;
                }

                match req.poll_next_unpin(cx)? {
                    Poll::Pending => return Poll::Pending,
                    Poll::Ready(Some(note)) => {
                        let key = serialize(note.get_location());

                        let mut route_notes_map = route_notes_map.lock().unwrap();

                        let route_notes = route_notes_map.entry(key).or_insert(Vec::new());
                        route_notes.push(note);

                        for note in route_notes {
                            resp.send_data(note.clone())?;
                        }
                    }
                    Poll::Ready(None) => {
                        resp.send_trailers(Metadata::new())?;
                        return Poll::Ready(Ok(()));
                    }
                }
            }
        });
        Ok(())
    }
}

fn in_range(point: &Point, rect: &Rectangle) -> bool {
    let left = f64::min(
        rect.get_lo().longitude as f64,
        rect.get_hi().longitude as f64,
    );
    let right = f64::max(
        rect.get_lo().longitude as f64,
        rect.get_hi().longitude as f64,
    );
    let top = f64::max(rect.get_lo().latitude as f64, rect.get_hi().latitude as f64);
    let bottom = f64::min(rect.get_lo().latitude as f64, rect.get_hi().latitude as f64);

    point.longitude as f64 >= left
        && point.longitude as f64 <= right
        && point.latitude as f64 >= bottom
        && point.latitude as f64 <= top
}

fn to_radians(num: f64) -> f64 {
    num * f64::consts::PI / 180.
}

fn calc_distance(p1: &Point, p2: &Point) -> u32 {
    let cord_factor: f64 = 1e7;
    let r = 6371000.; // earth radius in metres
    let lat1 = to_radians(p1.latitude as f64 / cord_factor);
    let lat2 = to_radians(p2.latitude as f64 / cord_factor);
    let lng1 = to_radians(p1.longitude as f64 / cord_factor);
    let lng2 = to_radians(p2.longitude as f64 / cord_factor);
    let dlat = lat2 - lat1;
    let dlng = lng2 - lng1;

    let a = f64::sin(dlat / 2.) * f64::sin(dlat / 2.)
        + f64::cos(lat1) * f64::cos(lat2) * f64::sin(dlng / 2.) * f64::sin(dlng / 2.);
    let c = 2. * f64::atan2(f64::sqrt(a), f64::sqrt(1. - a));

    let distance = r * c;
    distance as u32
}

fn serialize(point: &Point) -> String {
    format!("{} {}", point.latitude, point.longitude)
}

const ROUTE_GUIDE_DB_PATH: &str = "testdata/route_guide_db.json";

fn load_features(path: &Path) -> Vec<Feature> {
    let mut file = fs::File::open(path).expect("open");
    let mut s = String::new();
    file.read_to_string(&mut s).expect("read");

    // TODO: use protobuf mapper when new version is released

    let json_value = json::parse(&s).expect("parse json");
    let array = match json_value {
        JsonValue::Array(array) => array,
        _ => panic!(),
    };

    array
        .into_iter()
        .map(|item| {
            let object = match item {
                JsonValue::Object(object) => object,
                _ => panic!(),
            };

            let location = match object.get("location").expect("location") {
                JsonValue::Object(object) => object,
                _ => panic!(),
            };

            Feature {
                name: object
                    .get("name")
                    .expect("name")
                    .as_str()
                    .expect("unwrap")
                    .to_owned(),
                location: Some(Point {
                    latitude: location
                        .get("latitude")
                        .expect("latitude")
                        .as_i32()
                        .unwrap(),
                    longitude: location
                        .get("longitude")
                        .expect("longitude")
                        .as_i32()
                        .unwrap(),
                    ..Default::default()
                })
                .into(),
                ..Default::default()
            }
        })
        .collect()
}

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

    #[test]
    fn test_load_features() {
        let features = load_features(Path::new(ROUTE_GUIDE_DB_PATH));
        assert!(features.len() > 0);
    }
}
<commit_msg>Fix compilation<commit_after>use crate::route_guide::Feature;
use crate::route_guide::Point;
use crate::route_guide::Rectangle;
use crate::route_guide::RouteNote;
use crate::route_guide::RouteSummary;
use crate::route_guide_grpc::RouteGuide;

use futures::stream::StreamExt;

use grpc::Metadata;
use grpc::ServerHandlerContext;
use grpc::ServerRequest;
use grpc::ServerRequestSingle;
use grpc::ServerResponseSink;
use grpc::ServerResponseUnarySink;
use json::JsonValue;
use std::collections::HashMap;
use std::f64;
use std::fs;
use std::io::Read;
use std::path::Path;
use std::sync::Arc;
use std::sync::Mutex;
use std::task::Poll;
use std::time::Instant;

// https://github.com/grpc/grpc-go/blob/master/examples/route_guide/server/server.go
#[derive(Default)]
pub struct RouteGuideImpl {
    saved_features: Arc<Vec<Feature>>,
    route_notes: Arc<Mutex<HashMap<String, Vec<RouteNote>>>>,
}

impl RouteGuideImpl {
    pub fn new_and_load_db() -> RouteGuideImpl {
        RouteGuideImpl {
            saved_features: Arc::new(load_features(Path::new(ROUTE_GUIDE_DB_PATH))),
            route_notes: Default::default(),
        }
    }
}

impl RouteGuide for RouteGuideImpl {
    fn get_feature(
        &self,
        _o: ServerHandlerContext,
        req: ServerRequestSingle<Point>,
        resp: ServerResponseUnarySink<Feature>,
    ) -> grpc::Result<()> {
        for feature in &*self.saved_features {
            if feature.get_location() == &req.message {
                return resp.finish(feature.clone());
            }
        }

        resp.finish(Feature {
            location: Some(req.message).into(),
            ..Default::default()
        })
    }

    fn list_features(
        &self,
        o: ServerHandlerContext,
        mut req: ServerRequestSingle<Rectangle>,
        mut resp: ServerResponseSink<Feature>,
    ) -> grpc::Result<()> {
        let req = req.take_message();
        let saved_features = self.saved_features.clone();
        o.spawn(async move {
            for feature in &saved_features[..] {
                if in_range(feature.get_location(), &req) {
                    resp.ready().await?;
                    resp.send_data(feature.clone())?;
                }
            }
            resp.send_trailers(Metadata::new())
        });
        Ok(())
    }

    fn record_route(
        &self,
        o: ServerHandlerContext,
        req: ServerRequest<Point>,
        resp: ServerResponseUnarySink<RouteSummary>,
    ) -> grpc::Result<()> {
        let start_time = Instant::now();

        let saved_features = self.saved_features.clone();

        let mut stream = req.into_stream();

        o.spawn(async move {
            struct State {
                point_count: u32,
                feature_count: u32,
                distance: u32,
                last_point: Option<Point>,
            }

            let mut state = State {
                point_count: 0,
                feature_count: 0,
                distance: 0,
                last_point: None,
            };

            while let Some(point) = stream.next().await {
                let point = point?;
                state.point_count += 1;
                for feature in &saved_features[..] {
                    if feature.get_location() == &point {
                        state.feature_count += 1;
                    }
                }
                if let Some(last_point) = &state.last_point {
                    state.distance += calc_distance(last_point, &point);
                }
                state.last_point = Some(point);
            }

            resp.finish(RouteSummary {
                point_count: state.point_count as i32,
                feature_count: state.feature_count as i32,
                distance: state.distance as i32,
                elapsed_time: start_time.elapsed().as_secs() as i32,
                ..Default::default()
            })
        });

        Ok(())
    }

    fn route_chat(
        &self,
        o: ServerHandlerContext,
        req: ServerRequest<RouteNote>,
        mut resp: ServerResponseSink<RouteNote>,
    ) -> grpc::Result<()> {
        let mut req = req.into_stream();

        let route_notes_map = self.route_notes.clone();

        o.spawn_poll_fn(move |cx| {
            loop {
                // Wait until resp is writable
                if let Poll::Pending = resp.poll(cx)? {
                    return Poll::Pending;
                }

                match req.poll_next_unpin(cx)? {
                    Poll::Pending => return Poll::Pending,
                    Poll::Ready(Some(note)) => {
                        let key = serialize(note.get_location());

                        let mut route_notes_map = route_notes_map.lock().unwrap();

                        let route_notes = route_notes_map.entry(key).or_insert(Vec::new());
                        route_notes.push(note);

                        for note in route_notes {
                            resp.send_data(note.clone())?;
                        }
                    }
                    Poll::Ready(None) => {
                        resp.send_trailers(Metadata::new())?;
                        return Poll::Ready(Ok(()));
                    }
                }
            }
        });
        Ok(())
    }
}

fn in_range(point: &Point, rect: &Rectangle) -> bool {
    let left = f64::min(
        rect.get_lo().longitude as f64,
        rect.get_hi().longitude as f64,
    );
    let right = f64::max(
        rect.get_lo().longitude as f64,
        rect.get_hi().longitude as f64,
    );
    let top = f64::max(rect.get_lo().latitude as f64, rect.get_hi().latitude as f64);
    let bottom = f64::min(rect.get_lo().latitude as f64, rect.get_hi().latitude as f64);

    point.longitude as f64 >= left
        && point.longitude as f64 <= right
        && point.latitude as f64 >= bottom
        && point.latitude as f64 <= top
}

fn to_radians(num: f64) -> f64 {
    num * f64::consts::PI / 180.
}

fn calc_distance(p1: &Point, p2: &Point) -> u32 {
    let cord_factor: f64 = 1e7;
    let r = 6371000.; // earth radius in metres
    let lat1 = to_radians(p1.latitude as f64 / cord_factor);
    let lat2 = to_radians(p2.latitude as f64 / cord_factor);
    let lng1 = to_radians(p1.longitude as f64 / cord_factor);
    let lng2 = to_radians(p2.longitude as f64 / cord_factor);
    let dlat = lat2 - lat1;
    let dlng = lng2 - lng1;

    let a = f64::sin(dlat / 2.) * f64::sin(dlat / 2.)
        + f64::cos(lat1) * f64::cos(lat2) * f64::sin(dlng / 2.) * f64::sin(dlng / 2.);
    let c = 2. * f64::atan2(f64::sqrt(a), f64::sqrt(1. - a));

    let distance = r * c;
    distance as u32
}

fn serialize(point: &Point) -> String {
    format!("{} {}", point.latitude, point.longitude)
}

const ROUTE_GUIDE_DB_PATH: &str = "testdata/route_guide_db.json";

fn load_features(path: &Path) -> Vec<Feature> {
    let mut file = fs::File::open(path).expect("open");
    let mut s = String::new();
    file.read_to_string(&mut s).expect("read");

    // TODO: use protobuf mapper when new version is released

    let json_value = json::parse(&s).expect("parse json");
    let array = match json_value {
        JsonValue::Array(array) => array,
        _ => panic!(),
    };

    array
        .into_iter()
        .map(|item| {
            let object = match item {
                JsonValue::Object(object) => object,
                _ => panic!(),
            };

            let location = match object.get("location").expect("location") {
                JsonValue::Object(object) => object,
                _ => panic!(),
            };

            Feature {
                name: object
                    .get("name")
                    .expect("name")
                    .as_str()
                    .expect("unwrap")
                    .to_owned(),
                location: Some(Point {
                    latitude: location
                        .get("latitude")
                        .expect("latitude")
                        .as_i32()
                        .unwrap(),
                    longitude: location
                        .get("longitude")
                        .expect("longitude")
                        .as_i32()
                        .unwrap(),
                    ..Default::default()
                })
                .into(),
                ..Default::default()
            }
        })
        .collect()
}

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

    #[test]
    fn test_load_features() {
        let features = load_features(Path::new(ROUTE_GUIDE_DB_PATH));
        assert!(features.len() > 0);
    }
}
<|endoftext|>


<commit_before>use std::marker::PhantomData;

use primitives::{ConsumedResult, Error, Info, ParseError, Parser, RangeStream, StreamError,
                 StreamOnce, Tracked};
use primitives::FastResult::*;

pub struct Range(I::Range)
where
    I: RangeStream;

impl Parser for Range
where
    I: RangeStream,
    I::Range: PartialEq + ::primitives::Range,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, mut input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        use primitives::Range;
        let position = input.position();
        match input.uncons_range(self.0.len()) {
            Ok(other) => if other == self.0 {
                ConsumedOk((other, input))
            } else {
                EmptyErr(ParseError::empty(position).into())
            },
            Err(err) => EmptyErr(ParseError::new(position, err).into()),
        }
    }
    fn add_error(&mut self, errors: &mut Tracked<StreamError<Self::Input>>) {
        // TODO Add unexpected message?
        errors
            .error
            .add_error(Error::Expected(Info::Range(self.0.clone())));
    }
}

pub struct RangeOf
(P);

impl
 Parser for RangeOf

where
    P: Parser,
    P::Input: RangeStream,

::Range: ::primitives::Range,
{
    type Input = P::Input;
    type Output =
::Range;

    #[inline]
    fn parse_lazy(&mut self, input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        let (_, new_input) = ctry!(self.0.parse_lazy(input.clone()));
        let distance = input.distance(&new_input.into_inner());
        take(distance).parse_lazy(input)
    }
    fn add_error(&mut self, errors: &mut Tracked<StreamError<Self::Input>>) {
        self.0.add_error(errors)
    }
}

/// Zero-copy parser which reads a range of length `i.len()` and succeds if `i` is equal to that
/// range.
///
/// ```
/// # extern crate combine;
/// # use combine::range::range_of;
/// # use combine::char::letter;
/// # use combine::*;
/// # fn main() {
/// let mut parser = range_of(skip_many1(letter()));
/// assert_eq!(parser.parse("hello world"), Ok(("hello", " world")));
/// assert!(parser.parse("!").is_err());
/// # }
/// ```
#[inline(always)]
pub fn range_of
(parser: P) -> RangeOf

where
    P: Parser,
{
    RangeOf(parser)
}

/// Zero-copy parser which reads a range of length `i.len()` and succeds if `i` is equal to that
/// range.
///
/// ```
/// # extern crate combine;
/// # use combine::range::range;
/// # use combine::*;
/// # fn main() {
/// let mut parser = range("hello");
/// let result = parser.parse("hello world");
/// assert_eq!(result, Ok(("hello", " world")));
/// let result = parser.parse("hel world");
/// assert!(result.is_err());
/// # }
/// ```
#[inline(always)]
pub fn range(i: I::Range) -> Range
where
    I: RangeStream,
    I::Range: PartialEq + ::primitives::Range,
{
    Range(i)
}

pub struct Take(usize, PhantomData<fn(I) -> I>);
impl Parser for Take
where
    I: RangeStream,
    I::Range: ::primitives::Range,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, mut input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        let position = input.position();
        match input.uncons_range(self.0) {
            Ok(x) => ConsumedOk((x, input)),
            Err(err) => EmptyErr(ParseError::new(position, err).into()),
        }
    }
}

/// Zero-copy parser which reads a range of length `n`.
///
/// ```
/// # extern crate combine;
/// # use combine::range::take;
/// # use combine::*;
/// # fn main() {
/// let mut parser = take(1);
/// let result = parser.parse("1");
/// assert_eq!(result, Ok(("1", "")));
/// let mut parser = take(4);
/// let result = parser.parse("123abc");
/// assert_eq!(result, Ok(("123a", "bc")));
/// let result = parser.parse("abc");
/// assert!(result.is_err());
/// # }
/// ```
#[inline(always)]
pub fn take(n: usize) -> Take
where
    I: RangeStream,
    I::Range: ::primitives::Range,
{
    Take(n, PhantomData)
}

pub struct TakeWhile(F, PhantomData<fn(I) -> I>);
impl Parser for TakeWhile
where
    I: RangeStream,
    I::Range: ::primitives::Range,
    F: FnMut(I::Item) -> bool,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        ::primitives::uncons_while(input, &mut self.0)
    }
}

/// Zero-copy parser which reads a range of 0 or more tokens which satisfy `f`.
///
/// ```
/// # extern crate combine;
/// # use combine::range::take_while;
/// # use combine::*;
/// # fn main() {
/// let mut parser = take_while(|c: char| c.is_digit(10));
/// let result = parser.parse("123abc");
/// assert_eq!(result, Ok(("123", "abc")));
/// let result = parser.parse("abc");
/// assert_eq!(result, Ok(("", "abc")));
/// # }
/// ```
#[inline(always)]
pub fn take_while(f: F) -> TakeWhile
where
    I: RangeStream,
    F: FnMut(I::Item) -> bool,
{
    TakeWhile(f, PhantomData)
}

pub struct TakeWhile1(F, PhantomData<fn(I) -> I>);
impl Parser for TakeWhile1
where
    I: RangeStream,
    I::Range: ::primitives::Range,
    F: FnMut(I::Item) -> bool,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        match ::primitives::uncons_while(input, &mut self.0) {
            ConsumedOk((v, input)) => ConsumedOk((v, input)),
            EmptyOk((_, input)) => {
                let position = input.position();
                EmptyErr(ParseError::empty(position).into())
            }
            EmptyErr(err) => EmptyErr(err),
            ConsumedErr(err) => ConsumedErr(err),
        }
    }
}

/// Zero-copy parser which reads a range of 1 or more tokens which satisfy `f`.
///
/// ```
/// # extern crate combine;
/// # use combine::range::take_while1;
/// # use combine::*;
/// # fn main() {
/// let mut parser = take_while1(|c: char| c.is_digit(10));
/// let result = parser.parse("123abc");
/// assert_eq!(result, Ok(("123", "abc")));
/// let result = parser.parse("abc");
/// assert!(result.is_err());
/// # }
/// ```
#[inline(always)]
pub fn take_while1(f: F) -> TakeWhile1
where
    I: RangeStream,
    I::Range: ::primitives::Range,
    F: FnMut(I::Item) -> bool,
{
    TakeWhile1(f, PhantomData)
}

pub struct TakeUntilRange(I::Range)
where
    I: RangeStream;
impl Parser for TakeUntilRange
where
    I: RangeStream,
    I::Range: PartialEq + ::primitives::Range,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, mut input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        use primitives::Range;

        let len = self.0.len();
        let mut to_consume = 0;
        let mut look_ahead_input = input.clone();

        loop {
            match look_ahead_input.clone().uncons_range(len) {
                Ok(xs) => {
                    if xs == self.0 {
                        if let Ok(consumed) = input.uncons_range(to_consume) {
                            if to_consume == 0 {
                                return EmptyOk((consumed, input));
                            } else {
                                return ConsumedOk((consumed, input));
                            }
                        }

                        // We are guaranteed able to uncons to_consume characters here
                        // because we've already done it on look_ahead_input.
                        unreachable!();
                    } else {
                        to_consume += 1;
                        if look_ahead_input.uncons().is_err() {
                            unreachable!();
                        }
                    }
                }
                Err(e) => return EmptyErr(ParseError::new(look_ahead_input.position(), e).into()),
            };
        }
    }
}

/// Zero-copy parser which reads a range of 0 or more tokens until `r` is found.
///
/// The range `r` will not be consumed. If `r` is not found, the parser will
/// return an error.
///
/// ```
/// # extern crate combine;
/// # use combine::range::{range, take_until_range};
/// # use combine::*;
/// # fn main() {
/// let mut parser = take_until_range("\r\n");
/// let result = parser.parse("To: user@example.com\r\n");
/// assert_eq!(result, Ok(("To: user@example.com", "\r\n")));
/// let result = parser.parse("Hello, world\n");
/// assert!(result.is_err());
/// # }
/// ```
#[inline(always)]
pub fn take_until_range(r: I::Range) -> TakeUntilRange
where
    I: RangeStream,
{
    TakeUntilRange(r)
}

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

    #[test]
    fn take_while_test() {
        let result = take_while(|c: char| c.is_digit(10)).parse("123abc");
        assert_eq!(result, Ok(("123", "abc")));
        let result = take_while(|c: char| c.is_digit(10)).parse("abc");
        assert_eq!(result, Ok(("", "abc")));
    }

    #[test]
    fn take_while1_test() {
        let result = take_while1(|c: char| c.is_digit(10)).parse("123abc");
        assert_eq!(result, Ok(("123", "abc")));
        let result = take_while1(|c: char| c.is_digit(10)).parse("abc");
        assert!(result.is_err());
    }

    #[test]
    fn range_string_no_char_boundary_error() {
        let mut parser = range("hello");
        let result = parser.parse("hell\u{00EE} world");
        assert!(result.is_err());
    }
}
<commit_msg>refactor: Rename range_of to recognize<commit_after>use std::marker::PhantomData;

use primitives::{ConsumedResult, Error, Info, ParseError, Parser, RangeStream, StreamError,
                 StreamOnce, Tracked};
use primitives::FastResult::*;

pub struct Range(I::Range)
where
    I: RangeStream;

impl Parser for Range
where
    I: RangeStream,
    I::Range: PartialEq + ::primitives::Range,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, mut input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        use primitives::Range;
        let position = input.position();
        match input.uncons_range(self.0.len()) {
            Ok(other) => if other == self.0 {
                ConsumedOk((other, input))
            } else {
                EmptyErr(ParseError::empty(position).into())
            },
            Err(err) => EmptyErr(ParseError::new(position, err).into()),
        }
    }
    fn add_error(&mut self, errors: &mut Tracked<StreamError<Self::Input>>) {
        // TODO Add unexpected message?
        errors
            .error
            .add_error(Error::Expected(Info::Range(self.0.clone())));
    }
}

pub struct Recognize
(P);

impl
 Parser for Recognize

where
    P: Parser,
    P::Input: RangeStream,

::Range: ::primitives::Range,
{
    type Input = P::Input;
    type Output =
::Range;

    #[inline]
    fn parse_lazy(&mut self, input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        let (_, new_input) = ctry!(self.0.parse_lazy(input.clone()));
        let distance = input.distance(&new_input.into_inner());
        take(distance).parse_lazy(input)
    }
    fn add_error(&mut self, errors: &mut Tracked<StreamError<Self::Input>>) {
        self.0.add_error(errors)
    }
}

/// Zero-copy parser which reads a range of length `i.len()` and succeds if `i` is equal to that
/// range.
///
/// ```
/// # extern crate combine;
/// # use combine::range::recognize;
/// # use combine::char::letter;
/// # use combine::*;
/// # fn main() {
/// let mut parser = recognize(skip_many1(letter()));
/// assert_eq!(parser.parse("hello world"), Ok(("hello", " world")));
/// assert!(parser.parse("!").is_err());
/// # }
/// ```
#[inline(always)]
pub fn recognize
(parser: P) -> Recognize

where
    P: Parser,
{
    Recognize(parser)
}

/// Zero-copy parser which reads a range of length `i.len()` and succeds if `i` is equal to that
/// range.
///
/// ```
/// # extern crate combine;
/// # use combine::range::range;
/// # use combine::*;
/// # fn main() {
/// let mut parser = range("hello");
/// let result = parser.parse("hello world");
/// assert_eq!(result, Ok(("hello", " world")));
/// let result = parser.parse("hel world");
/// assert!(result.is_err());
/// # }
/// ```
#[inline(always)]
pub fn range(i: I::Range) -> Range
where
    I: RangeStream,
    I::Range: PartialEq + ::primitives::Range,
{
    Range(i)
}

pub struct Take(usize, PhantomData<fn(I) -> I>);
impl Parser for Take
where
    I: RangeStream,
    I::Range: ::primitives::Range,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, mut input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        let position = input.position();
        match input.uncons_range(self.0) {
            Ok(x) => ConsumedOk((x, input)),
            Err(err) => EmptyErr(ParseError::new(position, err).into()),
        }
    }
}

/// Zero-copy parser which reads a range of length `n`.
///
/// ```
/// # extern crate combine;
/// # use combine::range::take;
/// # use combine::*;
/// # fn main() {
/// let mut parser = take(1);
/// let result = parser.parse("1");
/// assert_eq!(result, Ok(("1", "")));
/// let mut parser = take(4);
/// let result = parser.parse("123abc");
/// assert_eq!(result, Ok(("123a", "bc")));
/// let result = parser.parse("abc");
/// assert!(result.is_err());
/// # }
/// ```
#[inline(always)]
pub fn take(n: usize) -> Take
where
    I: RangeStream,
    I::Range: ::primitives::Range,
{
    Take(n, PhantomData)
}

pub struct TakeWhile(F, PhantomData<fn(I) -> I>);
impl Parser for TakeWhile
where
    I: RangeStream,
    I::Range: ::primitives::Range,
    F: FnMut(I::Item) -> bool,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        ::primitives::uncons_while(input, &mut self.0)
    }
}

/// Zero-copy parser which reads a range of 0 or more tokens which satisfy `f`.
///
/// ```
/// # extern crate combine;
/// # use combine::range::take_while;
/// # use combine::*;
/// # fn main() {
/// let mut parser = take_while(|c: char| c.is_digit(10));
/// let result = parser.parse("123abc");
/// assert_eq!(result, Ok(("123", "abc")));
/// let result = parser.parse("abc");
/// assert_eq!(result, Ok(("", "abc")));
/// # }
/// ```
#[inline(always)]
pub fn take_while(f: F) -> TakeWhile
where
    I: RangeStream,
    F: FnMut(I::Item) -> bool,
{
    TakeWhile(f, PhantomData)
}

pub struct TakeWhile1(F, PhantomData<fn(I) -> I>);
impl Parser for TakeWhile1
where
    I: RangeStream,
    I::Range: ::primitives::Range,
    F: FnMut(I::Item) -> bool,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        match ::primitives::uncons_while(input, &mut self.0) {
            ConsumedOk((v, input)) => ConsumedOk((v, input)),
            EmptyOk((_, input)) => {
                let position = input.position();
                EmptyErr(ParseError::empty(position).into())
            }
            EmptyErr(err) => EmptyErr(err),
            ConsumedErr(err) => ConsumedErr(err),
        }
    }
}

/// Zero-copy parser which reads a range of 1 or more tokens which satisfy `f`.
///
/// ```
/// # extern crate combine;
/// # use combine::range::take_while1;
/// # use combine::*;
/// # fn main() {
/// let mut parser = take_while1(|c: char| c.is_digit(10));
/// let result = parser.parse("123abc");
/// assert_eq!(result, Ok(("123", "abc")));
/// let result = parser.parse("abc");
/// assert!(result.is_err());
/// # }
/// ```
#[inline(always)]
pub fn take_while1(f: F) -> TakeWhile1
where
    I: RangeStream,
    I::Range: ::primitives::Range,
    F: FnMut(I::Item) -> bool,
{
    TakeWhile1(f, PhantomData)
}

pub struct TakeUntilRange(I::Range)
where
    I: RangeStream;
impl Parser for TakeUntilRange
where
    I: RangeStream,
    I::Range: PartialEq + ::primitives::Range,
{
    type Input = I;
    type Output = I::Range;

    #[inline]
    fn parse_lazy(&mut self, mut input: Self::Input) -> ConsumedResult<Self::Output, Self::Input> {
        use primitives::Range;

        let len = self.0.len();
        let mut to_consume = 0;
        let mut look_ahead_input = input.clone();

        loop {
            match look_ahead_input.clone().uncons_range(len) {
                Ok(xs) => {
                    if xs == self.0 {
                        if let Ok(consumed) = input.uncons_range(to_consume) {
                            if to_consume == 0 {
                                return EmptyOk((consumed, input));
                            } else {
                                return ConsumedOk((consumed, input));
                            }
                        }

                        // We are guaranteed able to uncons to_consume characters here
                        // because we've already done it on look_ahead_input.
                        unreachable!();
                    } else {
                        to_consume += 1;
                        if look_ahead_input.uncons().is_err() {
                            unreachable!();
                        }
                    }
                }
                Err(e) => return EmptyErr(ParseError::new(look_ahead_input.position(), e).into()),
            };
        }
    }
}

/// Zero-copy parser which reads a range of 0 or more tokens until `r` is found.
///
/// The range `r` will not be consumed. If `r` is not found, the parser will
/// return an error.
///
/// ```
/// # extern crate combine;
/// # use combine::range::{range, take_until_range};
/// # use combine::*;
/// # fn main() {
/// let mut parser = take_until_range("\r\n");
/// let result = parser.parse("To: user@example.com\r\n");
/// assert_eq!(result, Ok(("To: user@example.com", "\r\n")));
/// let result = parser.parse("Hello, world\n");
/// assert!(result.is_err());
/// # }
/// ```
#[inline(always)]
pub fn take_until_range(r: I::Range) -> TakeUntilRange
where
    I: RangeStream,
{
    TakeUntilRange(r)
}

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

    #[test]
    fn take_while_test() {
        let result = take_while(|c: char| c.is_digit(10)).parse("123abc");
        assert_eq!(result, Ok(("123", "abc")));
        let result = take_while(|c: char| c.is_digit(10)).parse("abc");
        assert_eq!(result, Ok(("", "abc")));
    }

    #[test]
    fn take_while1_test() {
        let result = take_while1(|c: char| c.is_digit(10)).parse("123abc");
        assert_eq!(result, Ok(("123", "abc")));
        let result = take_while1(|c: char| c.is_digit(10)).parse("abc");
        assert!(result.is_err());
    }

    #[test]
    fn range_string_no_char_boundary_error() {
        let mut parser = range("hello");
        let result = parser.parse("hell\u{00EE} world");
        assert!(result.is_err());
    }
}
<|endoftext|>


<commit_before>use approxeq::ApproxEq;
use num_traits::Float;
use trig::Trig;
use {TypedRotation3D, TypedTransform3D, TypedVector3D, UnknownUnit};

/// A rigid transformation. All lengths are preserved under such a transformation.
///
///
/// Internally, this is a rotation and a translation, with the rotation
/// applied first (i.e. `Rotation * Translation`, in row-vector notation)
///
/// This can be more efficient to use over full matrices, especially if you
/// have to deal with the decomposed quantities often.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[repr(C)]
pub struct TypedRigidTransform3D<T, Src, Dst> {
    pub rotation: TypedRotation3D<T, Src, Dst>,
    pub translation: TypedVector3D<T, Dst>,
}

pub type RigidTransform3D<T> = TypedRigidTransform3D<T, UnknownUnit, UnknownUnit>;

// All matrix multiplication in this file is in row-vector notation,
// i.e. a vector `v` is transformed with `v * T`, and if you want to apply `T1`
// before `T2` you use `T1 * T2`

impl<T: Float + ApproxEq<T>, Src, Dst> TypedRigidTransform3D<T, Src, Dst> {
    /// Construct a new rigid transformation, where the `rotation` applies first
    #[inline]
    pub fn new(rotation: TypedRotation3D<T, Src, Dst>, translation: TypedVector3D<T, Dst>) -> Self {
        Self {
            rotation,
            translation,
        }
    }

    /// Construct an identity transform
    #[inline]
    pub fn identity() -> Self {
        Self {
            rotation: TypedRotation3D::identity(),
            translation: TypedVector3D::zero(),
        }
    }

    /// Construct a new rigid transformation, where the `translation` applies first
    #[inline]
    pub fn new_from_reversed(
        translation: TypedVector3D<T, Src>,
        rotation: TypedRotation3D<T, Src, Dst>,
    ) -> Self {
        // T * R
        //   = (R * R^-1) * T * R
        //   = R * (R^-1 * T * R)
        //   = R * T'
        //
        // T' = (R^-1 * T * R) is also a translation matrix
        // It is equivalent to the translation matrix obtained by rotating the
        // translation by R

        let translation = rotation.rotate_vector3d(&translation);
        Self {
            rotation,
            translation,
        }
    }

    #[inline]
    pub fn from_rotation(rotation: TypedRotation3D<T, Src, Dst>) -> Self {
        Self {
            rotation,
            translation: TypedVector3D::zero(),
        }
    }

    #[inline]
    pub fn from_translation(translation: TypedVector3D<T, Dst>) -> Self {
        Self {
            translation,
            rotation: TypedRotation3D::identity(),
        }
    }

    /// Decompose this into a translation and an rotation to be applied in the opposite order
    ///
    /// i.e., the translation is applied _first_
    #[inline]
    pub fn decompose_reversed(&self) -> (TypedVector3D<T, Src>, TypedRotation3D<T, Src, Dst>) {
        // self = R * T
        //      = R * T * (R^-1 * R)
        //      = (R * T * R^-1) * R)
        //      = T' * R
        //
        // T' = (R^ * T * R^-1) is T rotated by R^-1

        let translation = self.rotation.inverse().rotate_vector3d(&self.translation);
        (translation, self.rotation)
    }

    /// Returns the multiplication of the two transforms such that
    /// other's transformation applies after self's transformation.
    ///
    /// i.e., this produces `self * other` in row-vector notation
    #[inline]
    pub fn post_mul<Dst2>(
        &self,
        other: &TypedRigidTransform3D<T, Dst, Dst2>,
    ) -> TypedRigidTransform3D<T, Src, Dst2> {
        // self = R1 * T1
        // other = R2 * T2
        // result = R1 * T1 * R2 * T2
        //        = R1 * T1 * (R1^-1 * R1) * R2 * T2
        //        = (R1 * T1 * R1^-1) * R1 * R2 * T2
        //        = T' * R' * T2
        //        = (R' * R'^-1) * T' * R' * T2
        //        = R' * (R'^-1 * T' * R') * T2
        //        = R' * T'' * T2
        //        = R' * T'''
        //
        // (R1 * T1 * R1^-1) = T' = T1 rotated by R1^-1
        // R1 * R2  = R'
        // (R'^-1 * T' * R') = T'' = T' rotated by R'
        // T'' * T2 = T''' = vector addition of translations T2 and T''

        let t_prime = self
            .rotation
            .inverse()
            .rotate_vector3d(&self.translation);
        let r_prime = self.rotation.post_rotate(&other.rotation);
        let t_prime2 = r_prime.rotate_vector3d(&t_prime);
        let t_prime3 = t_prime2 + other.translation;
        TypedRigidTransform3D {
            rotation: r_prime,
            translation: t_prime3,
        }
    }

    /// Returns the multiplication of the two transforms such that
    /// self's transformation applies after other's transformation.
    ///
    /// i.e., this produces `other * self` in row-vector notation
    #[inline]
    pub fn pre_mul<Src2>(
        &self,
        other: &TypedRigidTransform3D<T, Src2, Src>,
    ) -> TypedRigidTransform3D<T, Src2, Dst> {
        other.post_mul(&self)
    }

    /// Inverts the transformation
    #[inline]
    pub fn inverse(&self) -> TypedRigidTransform3D<T, Dst, Src> {
        // result = (self)^-1
        //        = (R * T)^-1
        //        = T^-1 * R^-1
        //        = (R^-1 * R) * T^-1 * R^-1
        //        = R^-1 * (R * T^-1 * R^-1)
        //        = R' * T'
        //
        // T' = (R * T^-1 * R^-1) = (-T) rotated by R^-1
        // R' = R^-1
        //
        // An easier way of writing this is to use new_from_reversed() with R^-1 and T^-1

        TypedRigidTransform3D::new_from_reversed(
            -self.translation,
            self.rotation.inverse(),
        )
    }

    pub fn to_transform(&self) -> TypedTransform3D<T, Src, Dst>
    where
        T: Trig,
    {
        self.translation
            .to_transform()
            .pre_mul(&self.rotation.to_transform())
    }
}

impl<T: Float + ApproxEq<T>, Src, Dst> From<TypedRotation3D<T, Src, Dst>>
    for TypedRigidTransform3D<T, Src, Dst>
{
    fn from(rot: TypedRotation3D<T, Src, Dst>) -> Self {
        Self::from_rotation(rot)
    }
}

impl<T: Float + ApproxEq<T>, Src, Dst> From<TypedVector3D<T, Dst>>
    for TypedRigidTransform3D<T, Src, Dst>
{
    fn from(t: TypedVector3D<T, Dst>) -> Self {
        Self::from_translation(t)
    }
}

#[cfg(test)]
mod test {
    use super::RigidTransform3D;
    use {Rotation3D, TypedTransform3D, Vector3D};

    #[test]
    fn test_rigid_construction() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);

        let rigid = RigidTransform3D::new(rotation, translation);
        assert!(rigid
            .to_transform()
            .approx_eq(&translation.to_transform().pre_mul(&rotation.to_transform())));

        let rigid = RigidTransform3D::new_from_reversed(translation, rotation);
        assert!(rigid.to_transform().approx_eq(
            &translation
                .to_transform()
                .post_mul(&rotation.to_transform())
        ));
    }

    #[test]
    fn test_rigid_decomposition() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);

        let rigid = RigidTransform3D::new(rotation, translation);
        let (t2, r2) = rigid.decompose_reversed();
        assert!(rigid
            .to_transform()
            .approx_eq(&t2.to_transform().post_mul(&r2.to_transform())));
    }

    #[test]
    fn test_rigid_inverse() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);

        let rigid = RigidTransform3D::new(rotation, translation);
        let inverse = rigid.inverse();
        assert!(rigid
            .post_mul(&inverse)
            .to_transform()
            .approx_eq(&TypedTransform3D::identity()));
        assert!(inverse
            .to_transform()
            .approx_eq(&rigid.to_transform().inverse().unwrap()));
    }

    #[test]
    fn test_rigid_multiply() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
        let translation2 = Vector3D::new(9.3, -3.9, 1.1);
        let rotation2 = Rotation3D::unit_quaternion(0.1, 0.2, 0.3, -0.4);
        let rigid = RigidTransform3D::new(rotation, translation);
        let rigid2 = RigidTransform3D::new(rotation2, translation2);

        assert!(rigid
            .post_mul(&rigid2)
            .to_transform()
            .approx_eq(&rigid.to_transform().post_mul(&rigid2.to_transform())));
        assert!(rigid
            .pre_mul(&rigid2)
            .to_transform()
            .approx_eq(&rigid.to_transform().pre_mul(&rigid2.to_transform())));
    }
}
<commit_msg>Simplify rigid transform premultiplication math<commit_after>use approxeq::ApproxEq;
use num_traits::Float;
use trig::Trig;
use {TypedRotation3D, TypedTransform3D, TypedVector3D, UnknownUnit};

/// A rigid transformation. All lengths are preserved under such a transformation.
///
///
/// Internally, this is a rotation and a translation, with the rotation
/// applied first (i.e. `Rotation * Translation`, in row-vector notation)
///
/// This can be more efficient to use over full matrices, especially if you
/// have to deal with the decomposed quantities often.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[repr(C)]
pub struct TypedRigidTransform3D<T, Src, Dst> {
    pub rotation: TypedRotation3D<T, Src, Dst>,
    pub translation: TypedVector3D<T, Dst>,
}

pub type RigidTransform3D<T> = TypedRigidTransform3D<T, UnknownUnit, UnknownUnit>;

// All matrix multiplication in this file is in row-vector notation,
// i.e. a vector `v` is transformed with `v * T`, and if you want to apply `T1`
// before `T2` you use `T1 * T2`

impl<T: Float + ApproxEq<T>, Src, Dst> TypedRigidTransform3D<T, Src, Dst> {
    /// Construct a new rigid transformation, where the `rotation` applies first
    #[inline]
    pub fn new(rotation: TypedRotation3D<T, Src, Dst>, translation: TypedVector3D<T, Dst>) -> Self {
        Self {
            rotation,
            translation,
        }
    }

    /// Construct an identity transform
    #[inline]
    pub fn identity() -> Self {
        Self {
            rotation: TypedRotation3D::identity(),
            translation: TypedVector3D::zero(),
        }
    }

    /// Construct a new rigid transformation, where the `translation` applies first
    #[inline]
    pub fn new_from_reversed(
        translation: TypedVector3D<T, Src>,
        rotation: TypedRotation3D<T, Src, Dst>,
    ) -> Self {
        // T * R
        //   = (R * R^-1) * T * R
        //   = R * (R^-1 * T * R)
        //   = R * T'
        //
        // T' = (R^-1 * T * R) is also a translation matrix
        // It is equivalent to the translation matrix obtained by rotating the
        // translation by R

        let translation = rotation.rotate_vector3d(&translation);
        Self {
            rotation,
            translation,
        }
    }

    #[inline]
    pub fn from_rotation(rotation: TypedRotation3D<T, Src, Dst>) -> Self {
        Self {
            rotation,
            translation: TypedVector3D::zero(),
        }
    }

    #[inline]
    pub fn from_translation(translation: TypedVector3D<T, Dst>) -> Self {
        Self {
            translation,
            rotation: TypedRotation3D::identity(),
        }
    }

    /// Decompose this into a translation and an rotation to be applied in the opposite order
    ///
    /// i.e., the translation is applied _first_
    #[inline]
    pub fn decompose_reversed(&self) -> (TypedVector3D<T, Src>, TypedRotation3D<T, Src, Dst>) {
        // self = R * T
        //      = R * T * (R^-1 * R)
        //      = (R * T * R^-1) * R)
        //      = T' * R
        //
        // T' = (R^ * T * R^-1) is T rotated by R^-1

        let translation = self.rotation.inverse().rotate_vector3d(&self.translation);
        (translation, self.rotation)
    }

    /// Returns the multiplication of the two transforms such that
    /// other's transformation applies after self's transformation.
    ///
    /// i.e., this produces `self * other` in row-vector notation
    #[inline]
    pub fn post_mul<Dst2>(
        &self,
        other: &TypedRigidTransform3D<T, Dst, Dst2>,
    ) -> TypedRigidTransform3D<T, Src, Dst2> {
        // self = R1 * T1
        // other = R2 * T2
        // result = R1 * T1 * R2 * T2
        //        = R1 * (R2 * R2^-1) * T1 * R2 * T2
        //        = (R1 * R2) * (R2^-1 * T1 * R2) * T2
        //        = R' * T' * T2
        //        = R' * T''
        //
        // (R2^-1 * T2 * R2^) = T' = T2 rotated by R2
        // R1 * R2  = R'
        // T' * T2 = T'' = vector addition of translations T2 and T'

        let t_prime = other
            .rotation
            .rotate_vector3d(&self.translation);
        let r_prime = self.rotation.post_rotate(&other.rotation);
        let t_prime2 = t_prime + other.translation;
        TypedRigidTransform3D {
            rotation: r_prime,
            translation: t_prime2,
        }
    }

    /// Returns the multiplication of the two transforms such that
    /// self's transformation applies after other's transformation.
    ///
    /// i.e., this produces `other * self` in row-vector notation
    #[inline]
    pub fn pre_mul<Src2>(
        &self,
        other: &TypedRigidTransform3D<T, Src2, Src>,
    ) -> TypedRigidTransform3D<T, Src2, Dst> {
        other.post_mul(&self)
    }

    /// Inverts the transformation
    #[inline]
    pub fn inverse(&self) -> TypedRigidTransform3D<T, Dst, Src> {
        // result = (self)^-1
        //        = (R * T)^-1
        //        = T^-1 * R^-1
        //        = (R^-1 * R) * T^-1 * R^-1
        //        = R^-1 * (R * T^-1 * R^-1)
        //        = R' * T'
        //
        // T' = (R * T^-1 * R^-1) = (-T) rotated by R^-1
        // R' = R^-1
        //
        // An easier way of writing this is to use new_from_reversed() with R^-1 and T^-1

        TypedRigidTransform3D::new_from_reversed(
            -self.translation,
            self.rotation.inverse(),
        )
    }

    pub fn to_transform(&self) -> TypedTransform3D<T, Src, Dst>
    where
        T: Trig,
    {
        self.translation
            .to_transform()
            .pre_mul(&self.rotation.to_transform())
    }
}

impl<T: Float + ApproxEq<T>, Src, Dst> From<TypedRotation3D<T, Src, Dst>>
    for TypedRigidTransform3D<T, Src, Dst>
{
    fn from(rot: TypedRotation3D<T, Src, Dst>) -> Self {
        Self::from_rotation(rot)
    }
}

impl<T: Float + ApproxEq<T>, Src, Dst> From<TypedVector3D<T, Dst>>
    for TypedRigidTransform3D<T, Src, Dst>
{
    fn from(t: TypedVector3D<T, Dst>) -> Self {
        Self::from_translation(t)
    }
}

#[cfg(test)]
mod test {
    use super::RigidTransform3D;
    use {Rotation3D, TypedTransform3D, Vector3D};

    #[test]
    fn test_rigid_construction() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);

        let rigid = RigidTransform3D::new(rotation, translation);
        assert!(rigid
            .to_transform()
            .approx_eq(&translation.to_transform().pre_mul(&rotation.to_transform())));

        let rigid = RigidTransform3D::new_from_reversed(translation, rotation);
        assert!(rigid.to_transform().approx_eq(
            &translation
                .to_transform()
                .post_mul(&rotation.to_transform())
        ));
    }

    #[test]
    fn test_rigid_decomposition() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);

        let rigid = RigidTransform3D::new(rotation, translation);
        let (t2, r2) = rigid.decompose_reversed();
        assert!(rigid
            .to_transform()
            .approx_eq(&t2.to_transform().post_mul(&r2.to_transform())));
    }

    #[test]
    fn test_rigid_inverse() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);

        let rigid = RigidTransform3D::new(rotation, translation);
        let inverse = rigid.inverse();
        assert!(rigid
            .post_mul(&inverse)
            .to_transform()
            .approx_eq(&TypedTransform3D::identity()));
        assert!(inverse
            .to_transform()
            .approx_eq(&rigid.to_transform().inverse().unwrap()));
    }

    #[test]
    fn test_rigid_multiply() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
        let translation2 = Vector3D::new(9.3, -3.9, 1.1);
        let rotation2 = Rotation3D::unit_quaternion(0.1, 0.2, 0.3, -0.4);
        let rigid = RigidTransform3D::new(rotation, translation);
        let rigid2 = RigidTransform3D::new(rotation2, translation2);

        assert!(rigid
            .post_mul(&rigid2)
            .to_transform()
            .approx_eq(&rigid.to_transform().post_mul(&rigid2.to_transform())));
        assert!(rigid
            .pre_mul(&rigid2)
            .to_transform()
            .approx_eq(&rigid.to_transform().pre_mul(&rigid2.to_transform())));
    }
}
<|endoftext|>