libghost 0.4.6

use core::{borrow::BorrowMut, cell::RefCell, ops::Deref, panic, time::Duration};
use alloc::format;
use pros::devices::competition::CompetitionMode;
use pros::panic::panic;
use pros::{
    devices::{controller::JoystickAxis, Controller, competition},
    prelude::*,
};

extern crate alloc;
use alloc::{
    borrow::ToOwned,
    boxed::Box,
    string::{String, ToString},
    vec::Vec,
};

const COORDINATE_PLANE: &'static [u32; 4] = &[0, 0, 200, 200];

fn abs(x: f32) -> f32 {
    if x < 0.0 {
        -x
    } else {
        x
    }
}

fn signum(x: f32) -> f32 {
    if x < 0.0 {
        -1.0
    } else if x > 0.0 {
        1.0
    } else {
        0.0
    }
}

fn number_to_port(periph: &mut DynamicPeripherals, port: u8) -> SmartPort {
    periph.take_smart_port(port).unwrap()
}

type GhostMotor = (u8, bool);

/// Defines your drivetrain motors' ratio and wheel size.
pub struct DriveMotorAttributes {
    pub ratio: f32,
    pub wheel_size: f32,
}

/// Distance struct
/// Used for distance measurement
#[allow(dead_code)]
#[derive(Clone)]
pub struct Distance {
    inches: f32,
}

#[allow(dead_code)]
impl Distance {
    /// Create a new Distance struct from a set number of inches
    pub fn from_in(inches: f32) -> Self {
        Self { inches }
    }

    /// Create a new Distance struct from a set number of centimeters
    pub fn from_cm(cm: f32) -> Self {
        Self { inches: cm / 2.54 }
    }

    /// Create a new Distance struct from a set number of feet
    pub fn from_ft(ft: f32) -> Self {
        Self { inches: ft * 12.0 }
    }

    /// Initialize a new Distance object which takes in inches
    pub fn new(inches: f32) -> Self {
        Self { inches }
    }

    /// Convert the Distance object to inches
    pub fn to_in(&self) -> f32 {
        self.inches
    }

    /// Convert the Distance object to centimeters
    pub fn to_cm(&self) -> f32 {
        self.inches * 2.54
    }
}

impl Copy for Distance {}

// The turn direction
#[derive(PartialEq, Eq, Clone, Copy)]
#[allow(dead_code)]
pub enum Direction {
    Left,
    Right,
}

#[derive(PartialEq, Eq)]
#[allow(dead_code)]
pub enum TankType {
    Single,
    Double,
}

#[derive(PartialEq, Eq)]
#[allow(dead_code)]
pub enum OmniType {
    Mecanum,
    Arcade,
}

#[derive(PartialEq, Eq)]
#[allow(dead_code)]
pub enum Hand {
    Left,
    Right,
}

#[derive(PartialEq, Eq, Clone, Copy)]
#[allow(dead_code)]
pub enum Yoyo {
    True,
    False,
}

#[derive(PartialEq, Eq, Clone, Copy)]
#[allow(dead_code)]
pub enum Reverse {
    True,
    False,
}

const KEYS: &'static &[&str; 16] = &&[
    "UP", "DOWN", "LEFT", "RIGHT", "X", "B", "Y", "A", "LeftX", "LeftY", "RightX", "RightY", "L1",
    "L2", "R1", "R2",
];

#[allow(dead_code)]
fn parse_combination(comb: &'static str) -> Vec<&str> {
    let mut com = comb.split("+").map(|x| x.trim()).collect::<Vec<&str>>();
    for i in com.iter_mut() {
        let s: &&str = &i;
        if !KEYS.contains(s) {
            panic!("When creating keybind: The keybind {} is not a valid keybind!\nThis is due to values for keys being specified that are invalid or not implemented.", i);
        }
    }

    com
}

#[derive(PartialEq, Eq)]
enum MovementType {
    Drive,
    Turn,
    Timeline,
}

fn u8_to_adi_port(periph: &mut DynamicPeripherals, port: u8) -> AdiPort {
    periph.take_adi_port(port).unwrap()
}

pub struct Inertial {
    pub port: u8,
}

#[derive(Clone, Copy)]
pub struct Step {
    is_turn: bool,
    is_timed: bool,
    degrees: Option<f64>,
    distance: Option<Distance>,
    speed: Option<f32>,
    direction: Option<Direction>,
    duration: Option<Duration>,
    reverse: Option<bool>,
}

impl Step {
    pub fn new() -> Self {
        Self {
            is_turn: false,
            is_timed: false,
            degrees: None,
            distance: None,
            speed: None,
            direction: None,
            duration: None,
            reverse: None,
        }
    }

    pub fn turn(speed: f32, degrees: f64, direction: Direction) -> Self {
        Self {
            is_turn: true,
            is_timed: false,
            degrees: Some(degrees),
            distance: None,
            speed: Some(speed),
            direction: Some(direction),
            duration: None,
            reverse: None,
        }
    }

    pub fn drive(speed: f32, distance: Distance, reverse: Reverse) -> Self {
        Self {
            is_turn: false,
            is_timed: false,
            degrees: None,
            distance: Some(distance),
            speed: Some(speed),
            direction: None,
            duration: None,
            reverse: Some(reverse == Reverse::True),
        }
    }
}

#[derive(Clone)]
struct Timeline {
    steps: Vec<Step>,
    reverse: Option<Reverse>,
    yoyo: Option<Yoyo>,
}

impl Timeline {
    fn new() -> Self {
        Self {
            steps: Vec::new(),
            reverse: None,
            yoyo: None,
        }
    }

    fn add_step(&mut self, step: Step) {
        self.steps.push(step);
    }
}

// Drivetrain: What controls the robot's movement
pub struct Drivetrain {
    left: alloc::vec::Vec<(Motor, Option<AdiEncoder>)>,
    right: alloc::vec::Vec<(Motor, Option<AdiEncoder>)>,
    imu: InertialSensor,
    pids: Vec<(&'static str, PidController)>,
    motor_attrs: DriveMotorAttributes,
    controller: Controller,
    arcade: Option<(TankType, Hand)>,
    holonom: Option<(OmniType, Hand)>,
    binds: Option<Box<Vec<Box<(&'static str, &'static dyn Fn(Box<RefCell<&mut Drivetrain>>))>>>>,
    last_drive: Option<(f32, Distance, bool)>,
    last_turn: Option<(f32, f64, Direction)>,
    last_movement: Option<MovementType>,
    last_timeline: Option<String>,
    timelines: Option<Vec<(String, Timeline)>>,
    screen: Screen,
    time_scale: f32,
}

// Drivetrain implementation
#[allow(dead_code)]
impl Drivetrain {
    /// Initialize your drivetrain
    ///
    /// # Arguments
    ///
    /// * `leftx` - A Box containing an array of tuples defining the left side's motors. The first value in the tuple is the port number of the motor, and the second value is a boolean that determines if the motor is reversed.
    /// * `rightx` - A Box containing an array of tuples defining the right side's motors. The first value in the tuple is the port number of the motor, and the second value is a boolean that determines if the motor is reversed.
    /// * `imu` - The port number of the inertial sensor
    /// * `pid_data` - A Box containing an array of tuples defining the PID values for the drivetrain. The first value in the tuple is the name of the PID value, and the second value is a tuple containing the kP, kI, and kD values.
    /// * `wheels` - The attributes of the drivetrain's wheels. The ratio is the ratio of the drivetrain's gearset, and the wheel_size is the diameter of the wheels.
    ///
    /// # Example
    /// ```
    /// let train = Drivetrain::new(
    ///     Vec::from([
    ///         ("left",
    ///             Vec::from([
    ///                 ((7, true), (0, 0)),
    ///                 ((1, false), (0, 0)),
    ///                 ((8, true), (0, 0))
    ///             ])
    ///         ),
    ///         ("right",
    ///             Vec::from([
    ///                 ((9, false), (0, 0)),
    ///                 ((2, true), (0, 0)),
    ///                 ((10, false), (0, 0))
    ///             ])
    ///         )
    ///     ]),
    ///     Inertial {
    ///         port: 5
    ///     },
    ///     Vec::from([
    ///         ("drive",
    ///             (
    ///                 50.0,
    ///                 5.0,
    ///                 0.0
    ///             )
    ///         ),
    ///         ("turn",
    ///             (
    ///                 50.0,
    ///                 5.0,
    ///                 0.0
    ///             )
    ///         )
    ///     ]),
    ///     DriveMotorAttributes {
    ///         wheel_size: 4.0,
    ///         ratio: 0.6
    ///     }
    /// ).await;
    pub async fn new(
        mut peripherals: Box<DynamicPeripherals>,
        controller: Controller,
        motors: Vec<(&'static str, Vec<(GhostMotor, (u8, u8))>)>,
        imu: Inertial,
        pid_data: Vec<(&'static str, (f32, f32, f32))>,
        wheels: DriveMotorAttributes,
    ) -> Self {
        let mut leftx: Vec<(GhostMotor, (u8, u8))> = Vec::new();
        let mut rightx: Vec<(GhostMotor, (u8, u8))> = Vec::new();
        for i in motors.iter() {
            if i.0 == "left" {
                leftx = i.1.to_vec();
            }
            if i.0 == "right" {
                rightx = i.1.to_vec();
            }
        }
        let mut lefts = Vec::new();
        let mut rights = Vec::new();
        for i in leftx.iter() {
            let port = peripherals.as_mut().take_smart_port(i.0 .0).unwrap();
            let mut mtr = Motor::new(port, BrakeMode::Brake).unwrap();
            let mut enc: Option<AdiEncoder> = None;
            if i.1 .0 > 0 && i.1 .1 > 0 {
                enc = Some(
                    AdiEncoder::new(
                        (
                            u8_to_adi_port(&mut peripherals, i.1 .0),
                            u8_to_adi_port(&mut peripherals, i.1 .1),
                        ),
                        i.0 .1,
                    )
                    .unwrap(),
                );
            }
            mtr.set_reversed(i.0 .1).unwrap();
            lefts.push((mtr, enc));
        }
        for i in rightx.iter() {
            let port = number_to_port(&mut peripherals, i.0 .0);
            let mut mtr = Motor::new(port, BrakeMode::Brake).unwrap();
            let mut enc: Option<AdiEncoder> = None;
            if i.1 .0 > 0 && i.1 .1 > 0 {
                enc = Some(
                    AdiEncoder::new(
                        (
                            u8_to_adi_port(&mut peripherals, i.1 .0),
                            u8_to_adi_port(&mut peripherals, i.1 .1),
                        ),
                        i.0 .1,
                    )
                    .unwrap(),
                );
            }
            mtr.set_reversed(i.0 .1).unwrap();
            rights.push((mtr, enc));
        }
        if imu.port > 21 {
            panic!("When you constructed Drivetrain: The IMU port was invalid (greater than 21)!");
        }
        let mut imu = InertialSensor::new(number_to_port(&mut peripherals, imu.port));
        if imu.port_index() > 21 {
            panic!("**INTERNAL ERROR** When user constructed Drivetrain: The IMU port, *after construction*, was invalid (greater than 21)!\nReport to developers immediately!");
        }
        imu.calibrate().await.unwrap();
        let mut drive_exists = false;
        let mut turn_exists = false;
        let mut pid_vals = Vec::new();
        for i in pid_data.into_iter() {
            pid_vals.push((i.0, PidController::new(i.1 .0, i.1 .1, i.1 .2)));
        }
        for i in pid_vals.iter() {
            if i.0 == "drive" {
                drive_exists = true;
            }
            if i.0 == "turn" {
                turn_exists = true;
            }
        }
        if !drive_exists {
            panic!("When you constructed Drivetrain: The PID values for \"drive\" were not found!");
        }
        if !turn_exists {
            panic!("When you constructed Drivetrain: The PID values for \"turn\" were not found!");
        }
        Self {
            screen: peripherals.take_screen().unwrap(),
            left: lefts,
            right: rights,
            imu,
            pids: pid_vals,
            motor_attrs: wheels,
            controller,
            arcade: None,
            holonom: None,
            binds: None,
            last_drive: None,
            last_turn: None,
            last_movement: None,
            last_timeline: None,
            timelines: None,
            time_scale: 1.0,
        }
    }

    /// Get the drivetrain's `Screen`` struct.
    pub fn screen(&mut self) -> &mut Screen {
        &mut self.screen
    }

    /// Brake all available motors specified in the drivetrain.
    pub fn brake(&mut self) {
        for i in self.left.iter_mut().chain(self.right.iter_mut()) {
            i.0.brake().unwrap();
        }
    }

    /// Find the average voltage of each motor
    pub fn voltage(&mut self) -> i32 {
        let mut avg: i32 = 0;
        let mut i = 1;
        for wheel in self.left.iter_mut().chain(self.right.iter_mut()) {
            avg += wheel.0.voltage().unwrap() as i32;
            i += 1;
        }
        return (avg as i32) / i;
    }

    /// Universally applies a voltage (speed) to every motor.
    /// Moves your bot straight.
    /// It is recommended to use `timed_drive` for timed drive instead, however this is useful for setting raw speed values.
    /// 
    /// # Arguments
    /// 
    /// * `voltage` - The voltage (-127.0 to 127.0) to drive at.
    /// * `reverse` - Specifies if you are driving in reverse or not.
    pub fn set_drive(&mut self, voltage: f32, reverse: Reverse) {
        let volt = abs(voltage);
        for wheel in self.left.iter_mut().chain(self.right.iter_mut()) {
            wheel
                .0
                .set_voltage(volt * if reverse.eq(&Reverse::True) { -1.0 } else { 1.0 })
                .unwrap();
        }
    }

    /// Applies a turning force in volts to every motor.
    /// This turns your bot in a specified direction.
    /// It is recommended to use `timed_turn` for timed turn instead, however this is useful for setting raw speed values.
    ///
    /// # Arguments
    ///
    /// * `voltage` - The voltage (-127.0 to 127.0) to turn at.
    /// * `direction` - The direction (Left or Right) to turn towards.
    pub fn set_turn(&mut self, voltage: f32, direction: Direction) {
        if direction.eq(&Direction::Left) {
            for i in self.left.iter_mut() {
                let mtr = i.borrow_mut();
                mtr.0.set_voltage(-voltage).unwrap();
            }
            for i in self.right.iter_mut() {
                let mtr = i.borrow_mut();
                mtr.0.set_voltage(voltage).unwrap();
            }
        } else {
            for i in self.left.iter_mut() {
                let mtr = i.borrow_mut();
                mtr.0.set_voltage(voltage).unwrap();
            }
            for i in self.right.iter_mut() {
                let mtr = i.borrow_mut();
                mtr.0.set_voltage(-voltage).unwrap();
            }
        }
    }

    /// Reset the voltage of every motor.
    /// Use this to reset your speed after a raw drive or turn setting.
    pub fn reset_voltage(&mut self) {
        for wheel in self.left.iter_mut().chain(self.right.iter_mut()) {
            wheel.0.set_voltage(0.0).unwrap();
        }
    }

    fn find_pid_val_by_key(&self, key: &'static str) -> PidController {
        for i in self.pids.iter() {
            if i.0 == key {
                return i.1;
            }
        }
        panic!("**INTERNAL ERROR** PID value for {} was not found (value verification bypassed by user somehow?)!\nReport to the developers immediately!", key);
    }

    /// This is the PID-Controlled drive function as defined in the README.
    ///
    /// # Arguments
    ///
    /// * `speed` - The speed at which the robot should drive
    /// * `distance` - The distance the robot should drive
    ///
    /// # Example
    /// ```
    /// drivetrain.drive(
    ///     127.0,
    ///     Distance::from_ft(1.0),
    ///     Reverse::False
    /// )
    ///     .await;
    pub async fn drive(
        &mut self,
        speedx: f32,
        distance: Distance,
        reverse: Reverse,
    ) -> &mut Drivetrain {
        let mut pid_drive = self.find_pid_val_by_key("drive");
        let mut target = distance.to_in();
        if reverse.eq(&Reverse::True) {
            target = -target;
        }
    
        let speed = abs(speedx) * (self.time_scale / 100.0);
    
        for wheel in self.left.iter_mut().chain(self.right.iter_mut()) {
            let mtr = wheel.borrow_mut();
            mtr.0.zero().unwrap();
            mtr.0.set_voltage(speed).unwrap();
        }
    
        let mut dist = if self.left[0].1.is_none() {
            self.left[0].0.position().unwrap().into_degrees() / 360.0
                * (self.motor_attrs.wheel_size as f64)
        } else {
            self.left[0].1.as_ref().unwrap().position().unwrap() as f64 / 360.0
                * (self.motor_attrs.wheel_size as f64)
        };
        if reverse.eq(&Reverse::True) {
            dist = -dist;
        }
        let mut error = (target as f64) - dist;
    
        while abs(error as f32) > 0.1 {
            let output = pid_drive.update(target as f32, dist as f32);
            let adjusted_speed = speed + output;
    
            for wheel in self.left.iter_mut().chain(self.right.iter_mut()) {
                let mtr = wheel.borrow_mut();
                mtr.0.set_voltage(adjusted_speed).unwrap();
            }
    
            if self.left[0].1.is_none() {
                dist = self.left[0].0.position().unwrap().into_degrees() / 360.0
                    * (self.motor_attrs.wheel_size as f64);
            } else {
                dist = self.left[0].1.as_ref().unwrap().position().unwrap() as f64 / 360.0
                    * (self.motor_attrs.wheel_size as f64);
            }
            if reverse.eq(&Reverse::True) {
                dist = -dist;
            }
            error = (target as f64) - dist;
        }
    
        for wheel in self.left.iter_mut().chain(self.right.iter_mut()) {
            let mtr = wheel.borrow_mut();
            mtr.0.set_voltage(0.0).unwrap();
        }
    
        self.last_movement = Some(MovementType::Drive);
        self.last_drive = Some((speed, distance, reverse == Reverse::True));
    
        self
    }

    /// This is the PID-Controlled turn function as defined in the README.
    ///
    /// # Arguments
    ///
    /// * `speed` - The speed at which the robot should turn
    /// * `angle` - The angle the robot should turn
    /// * `direction` - The direction the robot should turn
    ///
    /// # Example
    /// ```
    /// drivetrain.turn(
    ///     127.0,
    ///     90.0,
    ///     Direction::Left
    /// )
    ///     .await;
    #[allow(unused_assignments)]
    pub async fn turn(&mut self, speed: f32, angle: f64, direction: Direction) -> &mut Drivetrain {
        let mut pid_data = self.find_pid_val_by_key("turn");
        let scale = self.time_scale;
        let mut new_speed = speed * (scale / 100.0);
        let target = angle;
        let mut error = target - self.imu.yaw().unwrap();
        for i in self.left.iter_mut().chain(self.right.iter_mut()) {
            let mtr = i.borrow_mut();
            mtr.0.set_voltage(speed).unwrap();
        }
        while abs(error as f32) > 0.1 {
            let output = pid_data.update(target as f32, self.imu.yaw().unwrap() as f32);
            new_speed = output;
            if direction.eq(&Direction::Left) {
                for i in self.left.iter_mut() {
                    let mtr = i.borrow_mut();
                    mtr.0.set_voltage(-new_speed).unwrap();
                }
                for i in self.right.iter_mut() {
                    let mtr = i.borrow_mut();
                    mtr.0.set_voltage(new_speed).unwrap();
                }
            } else {
                for i in self.left.iter_mut() {
                    let mtr = i.borrow_mut();
                    mtr.0.set_voltage(new_speed).unwrap();
                }
                for i in self.right.iter_mut() {
                    let mtr = i.borrow_mut();
                    mtr.0.set_voltage(-new_speed).unwrap();
                }
            }
            error = target - self.imu.yaw().unwrap();
        }
        for i in self.left.iter_mut().chain(self.right.iter_mut()) {
            let mtr = i.borrow_mut();
            mtr.0.set_voltage(0.0).unwrap();
        }

        self.last_movement = Some(MovementType::Turn);
        self.last_turn = Some((speed, angle, direction));

        self
    }

    /// Reverses the last action.
    ///
    /// # Arguments
    ///
    /// * `speed` - The speed at which the robot should reverse
    ///
    /// # Example
    /// ```
    /// drivetrain
    ///     .reverse(127.0)
    ///     .await;
    pub async fn reverse(&mut self, speed: f32) -> &mut Drivetrain {
        let lasttype = self.last_movement.as_ref().unwrap();
        if lasttype.eq(&MovementType::Drive) {
            let last = self.last_drive.as_ref().unwrap();
            self.drive(speed, last.1, Reverse::True).await;
        } else if lasttype.eq(&MovementType::Turn) {
            let last = self.last_turn.as_ref().unwrap();
            self.turn(
                speed,
                last.1,
                if last.2.eq(&Direction::Left) {
                    Direction::Right
                } else {
                    Direction::Left
                },
            )
            .await;
        } else if lasttype.eq(&MovementType::Timeline) {
            let last = self.last_timeline.as_mut().unwrap().clone();
            let timelines = Vec::clone(self.timelines.as_mut().unwrap());
            for i in timelines
                .iter()
                .find(|x| x.0.eq(&last))
                .unwrap()
                .1
                .steps
                .iter()
            {
                if i.is_turn {
                    let direction = if i.direction.unwrap().eq(&Direction::Left) {
                        Direction::Right
                    } else {
                        Direction::Left
                    };
                    self.turn(speed, i.degrees.unwrap(), direction).await;
                } else {
                    self.drive(speed, i.distance.unwrap(), Reverse::True).await;
                }
            }
        } else {
            panic!("When calling reverse: No movement was previously made!");
        }
        self
    }

    /// DO NOT USE THIS OUTSIDE OF CHAINED DRIVETRAIN STATEMENTS!
    /// If you do not use this within chains, this function is pointless. Use Rust's `sleep` instead.
    ///
    /// # Arguments
    /// 
    /// * `duration` - The duration to wait for.
    ///
    /// # Example
    ///
    /// ```
    /// drivetrain
    ///     .wait(Duration::from_secs(2))
    ///     .await;
    /// ```
    pub async fn wait(&mut self, duration: Duration) -> &mut Drivetrain {
        let scale = self.time_scale / 100.0;
        sleep(duration * (scale as u32));

        self
    }

    /// This is the timed drive function as defined in the README.
    ///
    /// # Arguments
    ///
    /// * `speed` - The speed at which the robot should drive
    /// * `duration` - The duration the robot should drive
    ///
    /// # Example
    /// ```
    /// drivetrain
    ///     .timed_drive(127.0, Duration::from_secs(5))
    ///     .await;
    pub async fn timed_drive(
        &mut self,
        speedx: f32,
        duration: Duration,
        reverse: Reverse,
    ) -> &mut Drivetrain {
        let mut speed = abs(speedx) * (self.time_scale / 100.0);
        if reverse.eq(&Reverse::True) {
            speed = -speed;
        }
        for i in self.left.iter_mut() {
            let mtr = i.borrow_mut();
            mtr.0.set_voltage(speed).unwrap();
        }
        for i in self.right.iter_mut() {
            let mtr = i.borrow_mut();
            mtr.0.set_voltage(speed).unwrap();
        }
        sleep(duration);
        for i in self.left.iter_mut() {
            let mtr = i.borrow_mut();
            mtr.0.set_voltage(0.0).unwrap();
        }
        for i in self.right.iter_mut() {
            let mtr = i.borrow_mut();
            mtr.0.set_voltage(0.0).unwrap();
        }
        self
    }

    /// This is the timed turn function as defined in the README.
    ///
    /// # Arguments
    ///
    /// * `speed` - The speed at which the robot should turn
    /// * `duration` - The duration the robot should turn
    /// * `direction` - The direction the robot should turn
    ///
    /// # Example
    /// ```
    /// drivetrain.timed_turn(
    ///     127.0,
    ///     Duration::from_secs(5),
    ///     Direction::Left
    /// )
    ///     .await;
    pub async fn timed_turn(
        &mut self,
        speed: f32,
        duration: Duration,
        direction: Direction,
    ) -> &mut Drivetrain {
        if direction.eq(&Direction::Left) {
            for i in self.left.iter_mut() {
                let mtr = i.borrow_mut();
                mtr.0.set_voltage(-speed).unwrap();
            }
            for i in self.right.iter_mut() {
                let mtr = i.borrow_mut();
                mtr.0.set_voltage(speed).unwrap();
            }
        } else {
            for i in self.left.iter_mut() {
                let mtr = i.borrow_mut();
                mtr.0.set_voltage(speed).unwrap();
            }
            for i in self.right.iter_mut() {
                let mtr = i.borrow_mut();
                mtr.0.set_voltage(-speed).unwrap();
            }
        }
        sleep(duration);
        for i in self.left.iter_mut() {
            let mtr = i.borrow_mut();
            mtr.0.set_voltage(0.0).unwrap();
        }
        for i in self.right.iter_mut() {
            let mtr = i.borrow_mut();
            mtr.0.set_voltage(0.0).unwrap();
        }
        self
    }

    /// Sets the type of tank drive and hand.
    pub fn tank(&mut self, arcade_type: TankType, hand: Hand) {
        if self.holonom.is_some() {
            panic!("When calling tank: Drivetrain already initialized with Holonomic driving!\nAre you sure you have a set of omni wheels that pulls up and switches to tank drive?\nI hope not!");
        }
        self.arcade = Some((arcade_type, hand));
    }

    /// Sets the type of omni drive and hand.
    pub fn omni(&mut self, holonom_type: OmniType, hand: Hand) {
        if self.arcade.is_some() {
            panic!("When calling omni: Drivetrain already initialized with Arcade driving!\nAre you sure you have a set of omni wheels that drops down and switches to omni drive?\nI hope not!");
        }
        self.holonom = Some((holonom_type, hand));
    }

    /// Controls the drivetrain, used for a loop in opcontrol.
    /// Handles driver control and keybinds.
    pub fn control(&mut self) {
        if competition::mode() != CompetitionMode::Opcontrol {
            panic!("When calling control: Control activated when not in opcontrol mode!");
        }
        let me = self;
        let do_arcade = me.arcade.is_some();
        let do_holonom = me.arcade.is_some();
        let controller = me.controller;
        let binds = me.binds.as_mut().unwrap();
        let binds_iter = binds.iter_mut();

        const KEYS: &'static &[&str; 12] = &&[
            "UP", "DOWN", "LEFT", "RIGHT", "X", "B", "Y", "A", "L1", "L2", "R1", "R2",
        ];
        const JOYSTICKS: &'static &[&str; 4] = &&["LeftX", "LeftY", "RightX", "RightY"];
        const KEYS_MAP: &'static &[pros::devices::controller::ControllerButton; 12] = &&[
            pros::devices::controller::ControllerButton::Up,
            pros::devices::controller::ControllerButton::Down,
            pros::devices::controller::ControllerButton::Left,
            pros::devices::controller::ControllerButton::Right,
            pros::devices::controller::ControllerButton::X,
            pros::devices::controller::ControllerButton::B,
            pros::devices::controller::ControllerButton::Y,
            pros::devices::controller::ControllerButton::A,
            pros::devices::controller::ControllerButton::LeftTrigger1,
            pros::devices::controller::ControllerButton::LeftTrigger2,
            pros::devices::controller::ControllerButton::RightTrigger1,
            pros::devices::controller::ControllerButton::RightTrigger2,
        ];
        const JOYSTICKS_MAP: &'static &[pros::devices::controller::JoystickAxis; 4] = &&[
            pros::devices::controller::JoystickAxis::LeftX,
            pros::devices::controller::JoystickAxis::LeftY,
            pros::devices::controller::JoystickAxis::RightX,
            pros::devices::controller::JoystickAxis::RightY,
        ];
        let good: &mut Vec<bool> = &mut Vec::new();
        for i in binds_iter {
            let keybind = i.0;
            let combination = parse_combination(keybind);
            let mut pressed = false;
            for i in combination.iter() {
                if KEYS.contains(i) {
                    let button = KEYS_MAP[KEYS.iter().position(|&x| &x == i).unwrap()];
                    if controller.button(button).unwrap() {
                        pressed = true;
                    } else {
                        pressed = false;
                    }
                } else if JOYSTICKS.contains(i) {
                    let axis = JOYSTICKS_MAP[JOYSTICKS.iter().position(|&x| &x == i).unwrap()];
                    if controller.joystick_axis(axis).unwrap() != 0.0 {
                        pressed = true;
                    } else {
                        pressed = false;
                    }
                }
                for i in combination.iter() {
                    if KEYS.contains(i) {
                        let button = KEYS_MAP[KEYS.iter().position(|&x| &x == i).unwrap()];
                        if controller.button(button).unwrap() {
                            pressed = true;
                        } else {
                            pressed = false;
                        }
                    } else if JOYSTICKS.contains(i) {
                        let axis = JOYSTICKS_MAP[JOYSTICKS.iter().position(|&x| &x == i).unwrap()];
                        if controller.joystick_axis(axis).unwrap() != 0.0 {
                            pressed = true;
                        } else {
                            pressed = false;
                        }
                    }
                    for i in combination.iter() {
                        if KEYS.contains(i) {
                            let button = KEYS_MAP[KEYS.iter().position(|&x| &x == i).unwrap()];
                            if controller.button(button).unwrap() {
                                pressed = true;
                            } else {
                                pressed = false;
                            }
                        } else if JOYSTICKS.contains(i) {
                            let axis =
                                JOYSTICKS_MAP[JOYSTICKS.iter().position(|&x| &x == i).unwrap()];
                            if controller.joystick_axis(axis).unwrap() != 0.0 {
                                pressed = true;
                            } else {
                                pressed = false;
                            }
                        }
                    }
                    good.push(pressed);
                }
            }
            if do_arcade {
                if me.arcade.as_mut().unwrap().0.eq(&TankType::Single) {
                    if me.arcade.as_mut().unwrap().1.eq(&Hand::Left) {
                        let l_y = controller.joystick_axis(JoystickAxis::LeftY).unwrap();
                        let l_x = controller.joystick_axis(JoystickAxis::LeftX).unwrap();
                        let left_speed = l_y + l_x;
                        let right_speed = l_y - l_x;
                        for i in me.left.iter_mut() {
                            let mtr = i.borrow_mut();
                            mtr.0.set_voltage(left_speed).unwrap();
                        }
                        for i in me.right.iter_mut() {
                            let mtr = i.borrow_mut();
                            mtr.0.set_voltage(right_speed).unwrap();
                        }
                    } else {
                        let r_y = controller.joystick_axis(JoystickAxis::RightY).unwrap();
                        let r_x = controller.joystick_axis(JoystickAxis::RightX).unwrap();
                        let left_speed = r_y + r_x;
                        let right_speed = r_y - r_x;
                        for i in me.left.iter_mut() {
                            let mtr = i.borrow_mut();
                            mtr.0.set_voltage(left_speed).unwrap();
                        }
                        for i in me.right.iter_mut() {
                            let mtr = i.borrow_mut();
                            mtr.0.set_voltage(right_speed).unwrap();
                        }
                    }
                } else if me.arcade.as_mut().unwrap().0.eq(&TankType::Double) {
                    if me.arcade.as_mut().unwrap().1.eq(&Hand::Left) {
                        let l_y = controller.joystick_axis(JoystickAxis::LeftY).unwrap();
                        let l_x = controller.joystick_axis(JoystickAxis::RightX).unwrap();
                        let left_speed = l_y + l_x;
                        let right_speed = l_y - l_x;
                        for i in me.left.iter_mut() {
                            let mtr = i.borrow_mut();
                            mtr.0.set_voltage(left_speed).unwrap();
                        }
                        for i in me.right.iter_mut() {
                            let mtr = i.borrow_mut();
                            mtr.0.set_voltage(right_speed).unwrap();
                        }
                    } else {
                        let r_y = controller.joystick_axis(JoystickAxis::RightY).unwrap();
                        let r_x = controller.joystick_axis(JoystickAxis::LeftX).unwrap();
                        let left_speed = r_y + r_x;
                        let right_speed = r_y - r_x;
                        for i in me.left.iter_mut() {
                            let mtr = i.borrow_mut();
                            mtr.0.set_voltage(left_speed).unwrap();
                        }
                        for i in me.right.iter_mut() {
                            let mtr = i.borrow_mut();
                            mtr.0.set_voltage(right_speed).unwrap();
                        }
                    }
                }
            } else if do_holonom {
                if me.holonom.as_mut().unwrap().0.eq(&OmniType::Mecanum) {
                    if me.holonom.as_mut().unwrap().1.eq(&Hand::Left) {
                        let left = me.controller.joystick_axis(JoystickAxis::LeftX).unwrap();
                        let right = me.controller.joystick_axis(JoystickAxis::RightY).unwrap();
                        let sideways = me.controller.joystick_axis(JoystickAxis::LeftY).unwrap();

                        me.right[0].0.set_voltage(right - sideways).unwrap();
                        me.left[0].0.set_voltage(left + sideways).unwrap();
                        me.right[1].0.set_voltage(right + sideways).unwrap();
                        me.left[1].0.set_voltage(left - sideways).unwrap();
                    } else if me.holonom.as_mut().unwrap().1.eq(&Hand::Right) {
                        let left = me.controller.joystick_axis(JoystickAxis::RightX).unwrap();
                        let right = me.controller.joystick_axis(JoystickAxis::LeftY).unwrap();
                        let sideways = me.controller.joystick_axis(JoystickAxis::RightY).unwrap();

                        me.right[0].0.set_voltage(right - sideways).unwrap();
                        me.left[0].0.set_voltage(left + sideways).unwrap();
                        me.right[1].0.set_voltage(right + sideways).unwrap();
                        me.left[1].0.set_voltage(left - sideways).unwrap();
                    }
                } else if me.holonom.as_mut().unwrap().0.eq(&OmniType::Arcade) {
                    if me.holonom.as_mut().unwrap().1.eq(&Hand::Left) {
                        let forward = me.controller.joystick_axis(JoystickAxis::LeftX).unwrap();
                        let sideways = me.controller.joystick_axis(JoystickAxis::LeftY).unwrap();
                        let turn = me.controller.joystick_axis(JoystickAxis::RightX).unwrap();

                        me.right[0]
                            .0
                            .set_voltage(forward - sideways + turn)
                            .unwrap();
                        me.left[0].0.set_voltage(forward + sideways - turn).unwrap();
                        me.right[1]
                            .0
                            .set_voltage(forward + sideways + turn)
                            .unwrap();
                        me.left[1].0.set_voltage(forward - sideways - turn).unwrap();
                    } else if me.holonom.as_mut().unwrap().1.eq(&Hand::Right) {
                        let forward = me.controller.joystick_axis(JoystickAxis::RightX).unwrap();
                        let sideways = me.controller.joystick_axis(JoystickAxis::RightY).unwrap();
                        let turn = me.controller.joystick_axis(JoystickAxis::LeftX).unwrap();

                        me.right[0]
                            .0
                            .set_voltage(forward - sideways + turn)
                            .unwrap();
                        me.left[0].0.set_voltage(forward + sideways - turn).unwrap();
                        me.right[1]
                            .0
                            .set_voltage(forward + sideways + turn)
                            .unwrap();
                        me.left[1].0.set_voltage(forward - sideways - turn).unwrap();
                    }
                }
            }
        }
        me.handle_binds(good.to_vec());
    }

    fn handle_binds(&mut self, binds: Vec<bool>) {
        for i in binds.iter() {
            if *i {
                let bind = self.binds.as_mut().unwrap().pop().unwrap();
                bind.1(Box::new(RefCell::new(self)));
            }
        }
    }

    /// Sets the speed scale, where 1.0 is the minimum (0.01x) and 200.0 is the maximum (2x).
    /// This affects motor voltage and delay speed. The motors will not go above or below |127.0| in any case.
    pub async fn speed(&mut self, scale: f32) -> &mut Drivetrain {
        if scale < 1.0 {
            panic!("When setting time (speed) scale to {}%: Speed scale cannot be less than 1.0 (1%)!", scale);
        } else if scale > 200.0 {
            panic!("When setting time (speed) scale to {}%: Speed scale cannot be less than 200.0 (200%)!", scale);
        }
        self.time_scale = scale;
        self
    }

    /// Bind a key to a function.
    ///
    /// # Arguments
    ///
    /// * `name` - The key combinations - One of `LeftX`, `LeftY`, `RightX`, `RightY` for joysticks.
    /// * `bind` - The function to call.
    ///
    /// # Example
    ///
    /// ```
    /// drivetrain.bind("L2", &|dt| {
    ///     let train: &mut Drivetrain = dt.into_inner();
    ///     train.tank(TankType::Single, Hand::Left);
    /// });
    /// ``` 
    pub fn bind(&mut self, name: &'static str, bind: &'static dyn Fn(Box<RefCell<&mut Self>>)) {
        self.binds.as_mut().unwrap().push(Box::new((name, bind)));
    }

    /// Register a new timeline.
    ///
    /// # Arguments
    ///
    /// * `name` - The name of the timeline.
    /// * `steps` - The steps in the timeline.
    /// * `reverse` - Should the timeline play backwards?
    /// * `yoyo` - Should the timeline play forwards and then backwards (affected by `reverse`)?
    ///
    /// # Example
    ///
    /// ```
    /// dt.timeline(
    ///     "back_turn",
    ///     Vec::from([
    ///         Step::drive(127.0, Distance::from_in(12.0), Reverse::False),
    ///         Step::turn(127.0, 45.0, Direction::Left),
    ///         Step::drive(127.0, Distance::from_in(12.0), Reverse::False),
    ///     ]),
    ///     Reverse::False,
    ///     Yoyo::True
    /// );
    /// ```
    pub fn timeline(
        &mut self,
        name: &'static str,
        steps: Vec<Step>,
        reverse: Reverse,
        yoyo: Yoyo,
    ) -> &mut Drivetrain {
        let mut steps = steps.to_vec();
        let mut cloned_steps = Vec::clone(&steps);
        if reverse.eq(&Reverse::True) {
            for (index, i) in cloned_steps.iter_mut().rev().enumerate() {
                let value: Step = if !i.is_turn {
                    Step::drive(i.speed.unwrap(), i.distance.unwrap(), if !i.reverse.unwrap() { Reverse::True } else { Reverse::False })
                } else if i.is_turn {
                    Step::turn(
                        i.speed.unwrap(),
                        i.degrees.unwrap(),
                        if i.direction.unwrap().eq(&Direction::Left) {
                            Direction::Right
                        } else {
                            Direction::Left
                        },
                    )
                } else {
                    panic!("**FATAL** The step type specified by the user was made invalid. This should not happen under any normal circumstances.\nPlease report this to the developers immediately!");
                };
                steps[index] = value;
            }
        }
        if yoyo.eq(&Yoyo::True) {
            let mut cloned_steps = Vec::clone(&steps);
            for i in cloned_steps.iter_mut().rev() {
                let value: Step = if !i.is_turn {
                    Step::drive(i.speed.unwrap(), i.distance.unwrap(), if !i.reverse.unwrap() { Reverse::True } else { Reverse::False })
                } else if i.is_turn {
                    Step::turn(
                        i.speed.unwrap(),
                        i.degrees.unwrap(),
                        if i.direction.unwrap().eq(&Direction::Left) {
                            Direction::Right
                        } else {
                            Direction::Left
                        },
                    )
                } else {
                    panic!("**FATAL** The step type specified by the user was made invalid. This should not happen under any normal circumstances.\nPlease report this to the developers immediately!");
                };
                steps.push(value);
            }
        }
        self.timelines.as_mut().unwrap().push((
            name.to_string(),
            Timeline {
                steps: Vec::from(steps),
                reverse: Some(reverse),
                yoyo: Some(yoyo),
            },
        ));
        self
    }

    /// Clears the list of timelines.
    pub fn clear_timelines(&mut self) {
        self.timelines.as_mut().unwrap().clear();
    }

    /// Runs a timeline
    ///
    /// # Arguments
    /// 
    /// * `name` - The name of the timeline, registered when you registered your timeline.
    /// * `reverse` - Should the timeline play in reverse individually? This does affect the initial reverse or yoyo states.
    ///
    /// # Example
    ///
    /// ```
    /// drivetrain
    ///    .run("back_turn", Reverse::False)
    ///    .await;
    /// ```
    pub async fn run(&mut self, name: &'static str, reverse: Reverse) -> &mut Drivetrain {
        let this = self.borrow_mut();
        let timeline_steps: &mut Vec<Step> = &mut Vec::clone(
            &this
                .timelines
                .as_mut()
                .unwrap()
                .iter_mut()
                .find(|x| x.0.eq(&name))
                .unwrap()
                .1
                .steps,
        );
        let mut cloned_steps = Vec::clone(timeline_steps);
        if reverse.eq(&Reverse::True) {
            for (index, i) in cloned_steps.iter_mut().rev().enumerate() {
                let value: Step = if !i.is_turn {
                    Step::drive(i.speed.unwrap(), i.distance.unwrap(), if !i.reverse.unwrap() { Reverse::True } else { Reverse::False })
                } else if i.is_turn {
                    Step::turn(
                        i.speed.unwrap(),
                        i.degrees.unwrap(),
                        if i.direction.unwrap().eq(&Direction::Left) {
                            Direction::Right
                        } else {
                            Direction::Left
                        },
                    )
                } else {
                    panic!("**FATAL** The step type specified by the user was made invalid. This should not happen under any normal circumstances.\nPlease report this to the developers immediately!");
                };
                timeline_steps[index] = value;
            }
        }
        self.last_timeline = Some(name.to_string());

        for i in timeline_steps.iter() {
            let this = self.borrow_mut();
            if i.is_turn {
                if reverse.eq(&Reverse::False) {
                    this.turn(i.speed.unwrap(), i.degrees.unwrap(), i.direction.unwrap())
                        .await;
                } else {
                    this.turn(
                        i.speed.unwrap(),
                        i.degrees.unwrap(),
                        if i.direction.unwrap().eq(&Direction::Left) {
                            Direction::Right
                        } else {
                            Direction::Left
                        },
                    )
                    .await;
                }
            } else {
                if reverse.eq(&Reverse::False) {
                    this.drive(i.speed.unwrap(), i.distance.unwrap(), if i.reverse.unwrap() { Reverse::True } else { Reverse::False })
                        .await;
                } else {
                    this.drive(i.speed.unwrap(), i.distance.unwrap(), if i.reverse.unwrap() { Reverse::False } else { Reverse::True })
                        .await;
                }
            }
            if i.is_timed {
                this.wait(i.duration.unwrap()).await;
            }
        }
        self
    }
}