From f5c520816e377fa5ea53602821a5cafd273eada8 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Fri, 18 Jan 2019 01:43:35 -0800
Subject: [PATCH] even better command stuff

---
 src/command.rs | 376 ++++++++++++++++++++++++++-----------------------
 src/robot.rs   |  43 +++---
 2 files changed, 220 insertions(+), 199 deletions(-)

diff --git a/src/command.rs b/src/command.rs
index 53075d1..c7d4cd4 100644
--- a/src/command.rs
+++ b/src/command.rs
@@ -11,154 +11,246 @@ impl CommandId for u64 {
     }
 }
 
-pub trait WithCommandData {
-    fn command_data(&self) -> &CommandData;
+pub trait WithCommandInfo {
+    fn command_info(&self) -> &CommandInfo;
 }
 
-pub trait CommandInfo: CommandId {
+pub trait GetCommandInfo: CommandId {
     fn name(&self) -> &str;
 }
 
-pub struct CommandData {
+pub struct CommandInfo {
     command_id: u64,
     name: String,
 }
 
-impl CommandData {
+impl CommandInfo {
     pub fn new<TName: Into<String>>(command_id: u64, name: TName) -> Self {
-        CommandData {
+        Self {
             command_id,
             name: name.into(),
         }
     }
 }
 
-impl CommandId for CommandData {
+impl CommandId for CommandInfo {
     fn command_id(&self) -> u64 {
         self.command_id
     }
 }
 
-impl CommandInfo for CommandData {
+impl GetCommandInfo for CommandInfo {
     fn name(&self) -> &str {
         &self.name
     }
 }
 
-impl<T: WithCommandData> CommandId for T {
+impl<T: WithCommandInfo> CommandId for T {
     fn command_id(&self) -> u64 {
-        self.command_data().command_id
+        self.command_info().command_id
     }
 }
 
-impl<T: WithCommandData> CommandInfo for T {
+impl<T: WithCommandInfo> GetCommandInfo for T {
     fn name(&self) -> &str {
-        &self.command_data().name
+        &self.command_info().name
     }
 }
 
-#[derive(Debug, PartialEq, Clone, Copy)]
+#[derive(Debug, PartialEq, Eq, Clone, Copy)]
 pub enum CommandResult {
     Continue,
     Done,
     Cancel,
 }
 
-pub trait Command: WithCommandData {
-    fn init(&mut self);
-    fn step(&mut self) -> CommandResult;
-    fn finish(&mut self, result: CommandResult);
-}
-
-pub struct GenericCommand<TConfig, TState> {
-    command_data: CommandData,
-    pub config: TConfig,
-    pub state: TState,
+#[derive(Debug, PartialEq, Eq, Clone, Copy)]
+pub enum CommandStatus {
+    Stopped,
+    Running,
+    Ended,
+    Cancelling,
+    Cancelled,
 }
 
-impl<TConfig, TState> GenericCommand<TConfig, TState>
+pub struct CommandContext<T>
 where
-    TConfig: 'static,
-    GenericCommand<TConfig, TState>: Command,
-    TState: Default + 'static,
+    T: CommandImp,
 {
-    pub fn new(command_data: CommandData, config: TConfig) -> Self {
+    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 {
-            command_data,
-            config,
-            state: TState::default(),
+            context: CommandContext {
+                info,
+                status: CommandStatus::Stopped,
+                state: T::State::default(),
+            },
+            imp,
         }
     }
 
-    pub fn new_ref(command_data: CommandData, config: TConfig) -> CommandRef {
-        Rc::new(RefCell::new(Self::new(command_data, config)))
+    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<TConfig, TState> WithCommandData for GenericCommand<TConfig, TState> {
-    fn command_data(&self) -> &CommandData {
-        &self.command_data
+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>;
-        $init_self:ident => $init:block;
-        $step_self:ident => $step:block;
-        $finish_self:ident, $finish_result:ident => $finish:block
+        $slf:ident, $ctx:ident => $init:block;
+        $step:block;
+        $finish:block
     ) => {
-        pub type $command = GenericCommand<$tconfig, $tstate>;
+        impl $crate::command::CommandImp for $tconfig {
+            type State = $tstate;
 
-        impl $crate::command::Command for $command {
-            fn init(&mut $init_self) {
-                use $crate::command::CommandInfo;
+            fn init(&$slf, $ctx: &mut $crate::command::CommandContext<Self>) {
+                use $crate::command::GetCommandInfo;
                 debug!(
                     "{}({}) init",
-                    stringify!($command_st),
-                    $init_self.name()
+                    stringify!($command),
+                    $ctx.name()
                 );
                 $init;
             }
 
-            fn step(&mut $step_self) -> $crate::command::CommandResult {
-                use $crate::command::CommandInfo;
+            fn step(&$slf, $ctx: &mut $crate::command::CommandContext<Self>) -> $crate::command::CommandResult {
+                use $crate::command::GetCommandInfo;
                 debug!(
                     "{}({}) step",
-                    stringify!($command_st),
-                    $step_self.name()
+                    stringify!($command),
+                    $ctx.name()
                 );
                 $step
             }
 
-            fn finish(&mut $finish_self, $finish_result: $crate::command::CommandResult) {
-                use $crate::command::CommandInfo;
+            fn finish(&$slf, $ctx: &mut $crate::command::CommandContext<Self>) {
+                use $crate::command::GetCommandInfo;
                 debug!(
-                    "{}({}) finish({:?}",
-                    stringify!($command_st),
-                    $finish_self.name(),
-                    $finish_result,
+                    "{}({}) finish({:?})",
+                    stringify!($command),
+                    $ctx.name(),
+                    $ctx.status(),
                 );
                 $finish;
             }
-
         }
+        pub type $command = $crate::command::GenericCommand<$tconfig>;
     };
     (
         $command:ident<$tconfig:ty,$tstate:ty>;
-        $init_self:ident => $init:block;
-        $step_self:ident => $step:block;
-        $finish_self:ident => $finish:block
+        $slf:ident => $init:block;
+        $step:block;
+        $finish:block
     ) => {
         command_impl! { $command<$tconfig,$tstate>;
-        $init_self => $init; $step_self => $step; $finish_self, res => {} }
+        $init_self, cts => $init; $step; $finish }
     };
     (
         $command:ident<$tconfig:ty>;
-        $init_self:ident => $init:block
+        $slf:ident => $init:block
     ) => {
         command_impl! { $command<$tconfig,()>;
-        $init_self => $init; self => { $crate::command::CommandResult::Done }; self => {} }
+        $slf, ctx => $init; { $crate::command::CommandResult::Done }; {} }
     };
 }
 
@@ -175,134 +267,84 @@ impl CommandGroupConfig {
 }
 
 pub struct CommandGroupState {
-    current_idx: Option<usize>,
-    has_initialized: bool,
+    current_idx: usize,
     has_cancelled: bool,
 }
 
 impl Default for CommandGroupState {
     fn default() -> Self {
         CommandGroupState {
-            current_idx: None,
-            has_initialized: false,
+            current_idx: 0,
             has_cancelled: false,
         }
     }
 }
 
-pub type CommandGroup = GenericCommand<CommandGroupConfig, CommandGroupState>;
+pub type CommandGroup = GenericCommand<CommandGroupConfig>;
 
-impl Command for CommandGroup {
-    fn init(&mut self) {
-        if self.state.current_idx.is_some() {
-            error!("CommandGroup init called twice");
-        }
-        self.state.current_idx = Some(0);
-        self.state.has_initialized = false;
-        self.state.has_cancelled = false;
-        debug!("CommandGroup {} init", self.command_data().name());
+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(&mut self) -> CommandResult {
+    fn step(&self, ctx: &mut CommandContext<Self>) -> CommandResult {
         use self::CommandResult::*;
-        let state = &mut self.state;
-        let commands = &self.config.commands;
+        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 mut current_idx = if let Some(c) = state.current_idx {
-            c
-        } else {
-            error!("CommandGroup step called before init");
-            state.has_cancelled = true;
-            return CommandResult::Cancel;
-        };
-        let mut result: CommandResult;
+        let current_idx = &mut state.current_idx;
         loop {
-            if current_idx >= commands.len() {
+            if *current_idx >= commands.len() {
                 debug!("CommandGroup out of commands");
                 return CommandResult::Done;
             }
-            let mut command = commands[current_idx].borrow_mut();
-            if !state.has_initialized {
-                debug!(
-                    "CommandGroup init {0} ({1})",
-                    current_idx,
-                    command.command_data().name()
-                );
-                command.init();
-                state.has_initialized = true;
-            }
-            result = command.step();
-            if result != Continue {
-                debug!(
-                    "CommandGroup {2:?} {0} ({1}) ",
-                    current_idx,
-                    command.command_data().name(),
-                    result
-                );
-                command.finish(result)
-            }
-            match result {
-                Done => {
-                    current_idx += 1;
-                    state.has_initialized = false;
+            let mut command = commands[*current_idx].borrow_mut();
+            let status = command.execute();
+            match status {
+                Ended => {
+                    command.reset();
+                    *current_idx += 1;
                 }
-                Cancel => {
+                Cancelled => {
+                    command.reset();
                     state.has_cancelled = true;
-                    break;
+                    return Cancel;
+                }
+                Running => return Continue,
+                other => {
+                    warn!("CommandGroup unhandled status {:?}", other);
+                    return Continue;
                 }
-                _ => break,
             }
         }
-        state.current_idx = Some(current_idx);
-        CommandResult::Continue
     }
 
-    fn finish(&mut self, result: CommandResult) {
-        let state = &mut self.state;
-        let commands = &mut self.config.commands;
-        if result == CommandResult::Cancel {
-            if let Some(command) = state
-                .current_idx
-                .and_then(|current_idx| commands.get(current_idx))
-            {
-                command.borrow_mut().finish(result);
+    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();
             }
         }
-        state.current_idx = None;
     }
 }
 
 pub struct CommandRun {
-    initialized: bool,
-    cancelled: bool,
     command: CommandRef,
 }
 
 impl CommandId for CommandRun {
     fn command_id(&self) -> u64 {
-        self.command.borrow().command_data().command_id()
-    }
-}
-
-impl CommandRun {
-    pub fn initialized(&self) -> bool {
-        self.initialized
-    }
-
-    pub fn cancelled(&self) -> bool {
-        self.cancelled
-    }
-
-    pub fn cancel(&mut self) -> bool {
-        if self.cancelled {
-            false
-        } else {
-            self.cancelled = true;
-            true
-        }
+        self.command.borrow().command_info().command_id()
     }
 }
 
@@ -346,50 +388,28 @@ impl CommandScheduler {
      * 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_data().command_id()) {
+        if self.is_running(command.borrow().command_info().command_id()) {
             false
         } else {
-            self.running_commands.push(CommandRun {
-                initialized: false,
-                cancelled: false,
-                command,
-            });
+            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.cancelled = true;
+            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();
-            if !command_run.initialized && !command_run.cancelled {
-                {
-                    let command_data = command.command_data();
-                    debug!(
-                        "initialzing command {} (id {})",
-                        command_data.name(),
-                        command_data.command_id()
-                    );
-                }
-                command.init();
-                command_run.initialized = true;
-            }
-            let result = if command_run.initialized {
-                command.step()
-            } else {
-                CommandResult::Cancel
-            };
-            if result != CommandResult::Continue {
-                command.finish(result);
-                true
-            } else {
-                false
+            match command.execute() {
+                Ended | Cancelled => true,
+                Stopped | Running | Cancelling => false,
             }
         });
     }
diff --git a/src/robot.rs b/src/robot.rs
index 1316a4e..6e77bd7 100644
--- a/src/robot.rs
+++ b/src/robot.rs
@@ -3,13 +3,13 @@ use std::rc::Rc;
 use std::thread;
 use std::time::Duration;
 
-use command::{CommandData, CommandGroup, CommandGroupConfig, CommandRef, CommandScheduler};
+use command::{CommandGroup, CommandGroupConfig, CommandInfo, CommandRef, CommandScheduler};
 use drive::{Drive, DriveRef};
 
 mod cmds {
-    use command::{Command, CommandResult, GenericCommand};
-    use drive::DriveRef;
     use self::CommandResult::*;
+    use command::CommandResult;
+    use drive::DriveRef;
 
     pub struct DriveCommandConfig {
         pub drive: DriveRef,
@@ -25,28 +25,28 @@ mod cmds {
         }
     }
 
-    pub type DriveCommand = GenericCommand<DriveCommandConfig, DriveCommandState>;
-
-    impl Command for DriveCommand {
-        fn init(&mut self) {
-            self.state = DriveCommandState::default();
-        }
-        fn step(&mut self) -> CommandResult {
-            self.config.drive.borrow_mut().drive_powers(1.0, 1.0);
-            self.state.iterations += 1;
-            if self.state.iterations >= 10 {
+    command_impl! (
+        DriveCommand<DriveCommandConfig, DriveCommandState>;
+        self, ctx => {
+            *ctx.state() = DriveCommandState::default();
+        }; {
+            let state = ctx.state();
+            self.drive.borrow_mut().drive_powers(1.0, 1.0);
+            state.iterations += 1;
+            if state.iterations >= 10 {
                 Done
             } else {
                 Continue
             }
+        }; {
+            self.drive.borrow_mut().drive_powers(0.0, 0.0);
         }
-        fn finish(&mut self, res: CommandResult) {
-            self.config.drive.borrow_mut().drive_powers(0.0, 0.0);
-        }
-    }
+    );
+
+    pub struct LogCommandConfig;
 
     command_impl! {
-        LogCmd<()>; self => { info!("{}", "LogCmd") }
+        LogCmd<LogCommandConfig>; self => { info!("{}", "LogCmd") }
     }
 }
 
@@ -69,14 +69,15 @@ impl Robot {
         warn!("Starting RobotRS");
 
         let command = cmds::DriveCommand::new_ref(
-            CommandData::new(0, "DriveCommand"),
+            CommandInfo::new(0, "DriveCommand"),
             cmds::DriveCommandConfig {
                 drive: self.drive.clone(),
             },
         );
-        let log_command = cmds::LogCmd::new_ref(CommandData::new(2, "LogCommand"), ());
+        let log_command =
+            cmds::LogCmd::new_ref(CommandInfo::new(2, "LogCommand"), cmds::LogCommandConfig);
         let command_group = CommandGroup::new(
-            CommandData::new(1, "Group"),
+            CommandInfo::new(1, "Group"),
             CommandGroupConfig::new(vec![command, log_command]),
         );
         let command_ref: CommandRef = Rc::new(RefCell::new(command_group));