robotrs/src/command.rs

use std::cell::RefCell;
use std::rc::Rc;

pub trait CommandId {
    fn command_id(&self) -> u64;
}

impl CommandId for u64 {
    fn command_id(&self) -> u64 {
        *self
    }
}

pub trait WithCommandInfo {
    fn command_info(&self) -> &CommandInfo;
}

pub trait GetCommandInfo: CommandId {
    fn name(&self) -> &str;
}

pub struct CommandInfo {
    command_id: u64,
    name: String,
}

impl CommandInfo {
    pub fn new<TName: Into<String>>(command_id: u64, name: TName) -> Self {
        Self {
            command_id,
            name: name.into(),
        }
    }
}

impl CommandId for CommandInfo {
    fn command_id(&self) -> u64 {
        self.command_id
    }
}

impl GetCommandInfo for CommandInfo {
    fn name(&self) -> &str {
        &self.name
    }
}

impl<T: WithCommandInfo> CommandId for T {
    fn command_id(&self) -> u64 {
        self.command_info().command_id
    }
}

impl<T: WithCommandInfo> GetCommandInfo for T {
    fn name(&self) -> &str {
        &self.command_info().name
    }
}

#[derive(Debug, PartialEq, Eq, Clone, Copy)]
pub enum CommandResult {
    Continue,
    Done,
    Cancel,
}

#[derive(Debug, PartialEq, Eq, Clone, Copy)]
pub enum CommandStatus {
    Stopped,
    Running,
    Ended,
    Cancelling,
    Cancelled,
}

pub struct CommandContext<T>
where
    T: CommandImp,
{
    info: CommandInfo,
    status: CommandStatus,
    state: <T as CommandImp>::State,
}

impl<T: CommandImp> CommandContext<T> {
    pub fn status(&self) -> CommandStatus {
        self.status
    }

    pub fn state(&mut self) -> &mut T::State {
        &mut self.state
    }
}

impl<T: CommandImp> WithCommandInfo for CommandContext<T> {
    fn command_info(&self) -> &CommandInfo {
        &self.info
    }
}

pub trait CommandImp {
    type State: Default;

    fn init(&self, ctx: &mut CommandContext<Self>)
    where
        Self: Sized;
    fn step(&self, ctx: &mut CommandContext<Self>) -> CommandResult
    where
        Self: Sized;
    fn finish(&self, ctx: &mut CommandContext<Self>)
    where
        Self: Sized;
}

pub trait Command: WithCommandInfo {
    fn reset(&mut self);
    fn status(&self) -> CommandStatus;
    fn execute(&mut self) -> CommandStatus;
    fn cancel(&mut self);
}

pub struct GenericCommand<T: CommandImp> {
    context: CommandContext<T>,
    imp: T,
}

impl<T: CommandImp + 'static> GenericCommand<T> {
    pub fn new(info: CommandInfo, imp: T) -> Self {
        Self {
            context: CommandContext {
                info,
                status: CommandStatus::Stopped,
                state: T::State::default(),
            },
            imp,
        }
    }

    pub fn new_ref(info: CommandInfo, imp: T) -> CommandRef {
        Rc::new(RefCell::new(Self::new(info, imp)))
    }
}

impl<T: CommandImp> WithCommandInfo for GenericCommand<T> {
    fn command_info(&self) -> &CommandInfo {
        &self.context.info
    }
}

impl<T: CommandImp> Command for GenericCommand<T> {
    fn status(&self) -> CommandStatus {
        self.context.status
    }

    fn execute(&mut self) -> CommandStatus {
        use self::CommandResult::*;
        use self::CommandStatus::*;
        let do_cancel = |me: &mut Self| {
            me.context.status = Cancelling;
            me.imp.finish(&mut me.context);
            Cancelled
        };
        let do_step = |me: &mut Self| {
            let result = me.imp.step(&mut me.context);
            match result {
                Continue => Running,
                Done => {
                    me.imp.finish(&mut me.context);
                    Ended
                }
                Cancel => do_cancel(me),
            }
        };
        let new_status = match self.status() {
            Stopped => {
                self.imp.init(&mut self.context);
                do_step(self)
            }
            Running => do_step(self),
            Ended => Ended,
            Cancelling => do_cancel(self),
            Cancelled => Cancelled,
        };
        self.context.status = new_status;
        new_status
    }

    fn reset(&mut self) {
        self.context.status = CommandStatus::Stopped;
    }
    fn cancel(&mut self) {
        self.context.status = CommandStatus::Cancelling;
    }
}

macro_rules! command_impl {
    (
        $command:ident<$tconfig:ty,$tstate:ty>;
        $slf:ident, $ctx:ident => $init:block;
        $step:block;
        $finish:block
    ) => {
        impl $crate::command::CommandImp for $tconfig {
            type State = $tstate;

            fn init(&$slf, $ctx: &mut $crate::command::CommandContext<Self>) {
                use $crate::command::GetCommandInfo;
                debug!(
                    "{}({}) init",
                    stringify!($command),
                    $ctx.name()
                );
                $init;
            }

            fn step(&$slf, $ctx: &mut $crate::command::CommandContext<Self>) -> $crate::command::CommandResult {
                use $crate::command::GetCommandInfo;
                debug!(
                    "{}({}) step",
                    stringify!($command),
                    $ctx.name()
                );
                $step
            }

            fn finish(&$slf, $ctx: &mut $crate::command::CommandContext<Self>) {
                use $crate::command::GetCommandInfo;
                debug!(
                    "{}({}) finish({:?})",
                    stringify!($command),
                    $ctx.name(),
                    $ctx.status(),
                );
                $finish;
            }
        }
        pub type $command = $crate::command::GenericCommand<$tconfig>;
    };
    (
        $command:ident<$tconfig:ty,$tstate:ty>;
        $slf:ident => $init:block;
        $step:block;
        $finish:block
    ) => {
        command_impl! { $command<$tconfig,$tstate>;
        $init_self, cts => $init; $step; $finish }
    };
    (
        $command:ident<$tconfig:ty>;
        $slf:ident => $init:block
    ) => {
        command_impl! { $command<$tconfig,()>;
        $slf, ctx => $init; { $crate::command::CommandResult::Done }; {} }
    };
}

pub type CommandRef = Rc<RefCell<dyn Command>>;

pub struct CommandGroupConfig {
    pub commands: Vec<CommandRef>,
}

impl CommandGroupConfig {
    pub fn new(commands: Vec<CommandRef>) -> Self {
        Self { commands }
    }
}

pub struct CommandGroupState {
    current_idx: usize,
    has_cancelled: bool,
}

impl Default for CommandGroupState {
    fn default() -> Self {
        CommandGroupState {
            current_idx: 0,
            has_cancelled: false,
        }
    }
}

pub type CommandGroup = GenericCommand<CommandGroupConfig>;

impl CommandImp for CommandGroupConfig {
    type State = CommandGroupState;

    fn init(&self, ctx: &mut CommandContext<Self>) {
        *ctx.state() = Default::default();
        debug!("CommandGroup {} init", ctx.name());
    }

    fn step(&self, ctx: &mut CommandContext<Self>) -> CommandResult {
        use self::CommandResult::*;
        use self::CommandStatus::*;
        let state = ctx.state();
        let commands = &self.commands;
        if state.has_cancelled {
            debug!("CommandGroup step called after cancelled");
            return CommandResult::Cancel;
        }
        let current_idx = &mut state.current_idx;
        loop {
            if *current_idx >= commands.len() {
                debug!("CommandGroup out of commands");
                return CommandResult::Done;
            }
            let mut command = commands[*current_idx].borrow_mut();
            let status = command.execute();
            match status {
                Ended => {
                    command.reset();
                    *current_idx += 1;
                }
                Cancelled => {
                    command.reset();
                    state.has_cancelled = true;
                    return Cancel;
                }
                Running => return Continue,
                other => {
                    warn!("CommandGroup unhandled status {:?}", other);
                    return Continue;
                }
            }
        }
    }

    fn finish(&self, ctx: &mut CommandContext<Self>) {
        let commands = &self.commands;
        if ctx.status() == CommandStatus::Cancelling {
            if let Some(command) = commands.get(ctx.state().current_idx) {
                let mut command = command.borrow_mut();
                command.cancel();
                command.execute();
            }
        }
    }
}

pub struct CommandRun {
    command: CommandRef,
}

impl CommandId for CommandRun {
    fn command_id(&self) -> u64 {
        self.command.borrow().command_info().command_id()
    }
}

pub struct CommandScheduler {
    running_commands: Vec<CommandRun>,
}

impl CommandScheduler {
    pub fn new() -> Self {
        CommandScheduler {
            running_commands: Vec::new(),
        }
    }

    pub fn get_command_run<T: CommandId>(&self, id: T) -> Option<&CommandRun> {
        let id = id.command_id();
        for command_run in &self.running_commands {
            if command_run.command_id() == id {
                return Some(command_run);
            }
        }
        None
    }

    fn get_command_run_mut<T: CommandId>(&mut self, id: T) -> Option<&mut CommandRun> {
        let id = id.command_id();
        for command_run in &mut self.running_commands {
            if command_run.command_id() == id {
                return Some(command_run);
            }
        }
        None
    }

    pub fn is_running<T: CommandId>(&self, id: T) -> bool {
        self.get_command_run(id).is_some()
    }

    /**
     * Starts running a command.
     * Returns true if the command was started, false if it was not started because it was already running
     */
    pub fn start(&mut self, command: CommandRef) -> bool {
        if self.is_running(command.borrow().command_info().command_id()) {
            false
        } else {
            self.running_commands.push(CommandRun { command });
            true
        }
    }

    pub fn cancel<T: CommandId>(&mut self, id: T) -> bool {
        self.get_command_run_mut(id).map_or(false, |command_run| {
            command_run.command.borrow_mut().cancel();
            true
        })
    }

    pub fn execute(&mut self) {
        use self::CommandStatus::*;
        self.running_commands.drain_filter(|command_run| {
            let mut command = command_run.command.borrow_mut();
            match command.execute() {
                Ended | Cancelled => true,
                Stopped | Running | Cancelling => false,
            }
        });
    }
}