# Using Rust to Control a Raspberry Pi Zero W Rover

fn main() {
    println!("Hello World!");
}

use sysfs_pwm;

error_chain!{
    foreign_links {
        PWM(sysfs_pwm::Error);
    }
}

// The dependencies we are going to use.
extern crate sysfs_pwm;
#[macro_use]
extern crate error_chain;
#[macro_use]
extern crate clap;

// Include our error module so we can use it.
mod error;

// Import everything from our error module.
use error::*;

// A stub function we will implement our application logic in later.
fn run() -> Result<()> {
    bail!("Not yet implemented")
}

fn main() {
    // Run the run function and print any errors that it returns.
    if let Err(ref e) = run() {
        use std::io::Write;
        let stderr = &mut ::std::io::stderr();
        // Error message for when we cannot write to stderr
        let errmsg = "Error writing to stderr";

        // Print out the error that occurred.
        writeln!(stderr, "error: {}", e).expect(errmsg);

        // And what caused it.
        for e in e.iter().skip(1) {
            writeln!(stderr, "caused by: {}", e).expect(errmsg);
        }

        // As well as any backtrace if they are enabled.
        if let Some(backtrace) = e.backtrace() {
            writeln!(stderr, "backtrace: {:?}", backtrace).expect(errmsg);
        }

        ::std::process::exit(1);
    }
}

use error::*;
use sysfs_pwm::Pwm;

const PERIOD: u32 = 20_000_000;
const MAX_DUTY_CYCLE: u32 = 2_000_000;
const MIN_DUTY_CYCLE: u32 = 1_000_000;

// Holds the left and right motor that the functions below will act upon.
pub struct Rover {
    left: Pwm,
    right: Pwm,
}

impl Rover {
    // Creates a new rovers with both motors ready to be enabled. The motors
    // will be disabled and the underlying pwm drivers unexported when the
    // rover is dropped. `enable(true)` must be called before the motor will
    // move.
    pub fn new(chip: u32, left_pin: u32, right_pin: u32) -> Result<Rover> {
        let left = Pwm::new(chip, left_pin).chain_err(|| "failed to create left motor")?;
        let right = Pwm::new(chip, right_pin).chain_err(|| "failed to create right motor")?;
        left.export().chain_err(|| "failed to export the left motor pwm channel")?;
        right.export().chain_err(|| "failed to export the right motor pwm channel")?;
        left.set_period_ns(PERIOD).chain_err(|| "failed to set period on left motor")?;
        right.set_period_ns(PERIOD).chain_err(|| "failed to set period on right motor")?;
        Ok(Rover {
            left: left,
            right: right,
        })
    }

    // Enables/disables the motor. When disabled they keep their current
    // speed and their speed can still be set but they will not move until
    // enabled.
    pub fn enable(&self, enabled: bool) -> Result<()> {
        self.left.enable(enabled).chain_err(|| "failed to enable left motor")?;
        self.right.enable(enabled).chain_err(|| "failed to enable right motor")
    }

    // Converts a speed between -100 (full reverse) and 100 (full forward)
    // to a duty cycle which we can pass to the Pwm struct from sysfs_pwm.
    // The idea is to map values from -100, 100 to 1_000_000, 2_000_000 where
    // 0 is 1500000 (the neutral point for servos). It also caps the return
    // value to be within this range.
    fn speed_to_duty_cycle(speed: i8) -> u32 {
        let duty_cycle = (((speed as i32 * 10000) + MIN_DUTY_CYCLE as i32) as u32 / 2) +
                         MIN_DUTY_CYCLE;
        if duty_cycle > MAX_DUTY_CYCLE {
            return MAX_DUTY_CYCLE;
        }
        if duty_cycle < MIN_DUTY_CYCLE {
            return MIN_DUTY_CYCLE;
        }
        duty_cycle
    }

    // Sets the speed of the left motor. Can be any value between -100 (full
    // reverse) and 100 (full forward), values above or below these limits will
    // be to to the limit.
    pub fn set_left_speed(&self, speed: i8) -> Result<()> {
        self.left
            .set_duty_cycle_ns(Rover::speed_to_duty_cycle(-speed))
            .chain_err(|| "failed to set duty on left motor")
    }

    // Sets the speed of the right motor. Can be any value between -100 (full
    // reverse) and 100 (full forward), values above or below these limits will
    // be to to the limit.
    pub fn set_right_speed(&self, speed: i8) -> Result<()> {
        self.right
            .set_duty_cycle_ns(Rover::speed_to_duty_cycle(speed))
            .chain_err(|| "failed to set duty on left motor")
    }

    // Stops both the motors, equlivent to setting their speeds to 0.
    pub fn stop(&self) -> Result<()> {
        self.set_left_speed(0)?;
        self.set_right_speed(0)
    }

    // Sets the speed of left and right motor. Can be any value between -100 (full
    // reverse) and 100 (full forward), values above or below these limits will
    // be to to the limit.
    pub fn set_speed(&self, left: i8, right: i8) -> Result<()> {
        self.set_left_speed(left)?;
        self.set_right_speed(right)
    }

    // Unexports the motors so they can no longer be used. Note that we use
    // `self` rather than `&self` as we want this function to consume the
    // rover stopping any future calls to it (which will cause a compile time
    // error)
    pub fn unexport(self) -> Result<()> {
        self.left.enable(false).chain_err(|| "failed to disable left motor")?;
        self.right.enable(false).chain_err(|| "failed to disable right motor")?;
        self.left.unexport().chain_err(|| "failed to unexport left motor")?;
        self.right.unexport().chain_err(|| "failed to unexport right motor")
    }
}

mod rover;

const PWM_CHIP: u32 = 0;
const LEFT_PWM: u32 = 0;
const RIGHT_PWM: u32 = 1;

fn run() -> Result<()> {
    use clap::App;
    use rover::Rover;

    let yaml = load_yaml!("cli.yml");
    let matches = App::from_yaml(yaml).version(crate_version!()).get_matches();

    let rover = Rover::new(PWM_CHIP, LEFT_PWM, RIGHT_PWM)?;

    // find out which subcommand was given, get_matches will print a help
    // message and exit if an unknown command is given.
    if let Some(_) = matches.subcommand_matches("disable") {
        rover.enable(false)
    } else if let Some(_) = matches.subcommand_matches("enable") {
        rover.enable(true)
    } else if let Some(_) = matches.subcommand_matches("stop") {
        rover.stop()
    } else if let Some(matches) = matches.subcommand_matches("speed") {
        // left is required so it will always be set here, otherwise
        // get_matches above will print a help message and exit.
        let left = matches.value_of("LEFT").unwrap();
        // if right is not set then use the left value
        let right = matches.value_of("RIGHT").unwrap_or(left);
        // parse the values into i8s and return an error if this fails.
        let left: i8 = left.parse::<i8>().chain_err(|| "failed to parse left speed")?;
        let right: i8 = right.parse::<i8>().chain_err(|| "failed to parse right speed")?;

        rover.set_speed(left, right)?;
        if !matches.is_present("dont-enable") {
            rover.enable(true)?;
        }
        Ok(())
    } else if let Some(_) = matches.subcommand_matches("unexport") {
        rover.unexport()
    } else {
        // If no command was specified print the help message
        println!("{}", matches.usage());
        Ok(())
    }
}