mechutil 0.8.1

//
// Copyright (C) 2024 Automated Design Corp.. All Rights Reserved.
// Created Date: 2024-09-06 07:32:01
// -----
// Last Modified: 2024-11-13 15:52:10
// -----
//
//

//!

use std::{
    sync::{
        Arc,
        atomic::{AtomicBool, Ordering},
    },
    time::Duration,
};

use crate::command::{MechFsmCommandTuple, MechFsmControl, MechFsmState, MechFsmStatusTuple};

use async_trait::async_trait;
use log::{debug, error, info};
use tokio::sync::{mpsc, oneshot};

use anyhow::anyhow;
use thiserror::Error;

pub enum CommandFsmMessage {
    ChangeState {
        state: MechFsmState,
        status: MechFsmStatusTuple,
    },
    ReadState {
        respond_to: oneshot::Sender<MechFsmState>,
    },
    Command {
        sender: mpsc::Sender<CommandFsmMessage>,
        respond_to: oneshot::Sender<MechFsmStatusTuple>,
        cmd: MechFsmCommandTuple,
    },
    Heartbeat {
        heartbeat: i64,
    },
    Shutdown {
        respond_to: oneshot::Sender<bool>,
    },
}

#[derive(Debug, Clone, Copy)]
pub enum CommandFsmResult {
    /// The command completed.
    Done,
    /// The command is running and will signal when completed
    Running,
}

#[derive(Error, Debug)]
pub enum StateChangeError {
    /// This is a minor error, and will not cause the FSM to go to the error state.
    #[error("Rejected. Not ready to change state. {0}")]
    Rejected(String),
    /// A return of this error will cause the FSM to go into the error state.
    #[error("A fault occurred changing state: {0}")]
    Fault(String),
    /// A return of this error will cause the FSM to go into the error state.
    #[error("unknown data store error")]
    Unknown,
}

/// A trait that state machines can implement to handle custom command execution.
#[async_trait]
pub trait CommandFsmHandler {
    /// Read the command for this FSM from the client/master.
    /// This is the first function called on every tick.
    ///
    /// If an error is returned, the FSM will shift to an error state. If Ok(()), the FSM proceeds
    /// normally, although commands with a 0 for transaction_id or control code will be ignored.
    async fn read_command(
        &mut self,
        command: &mut MechFsmCommandTuple,
    ) -> Result<(), anyhow::Error>;

    /// Process the state machine. This will be called on a regular tick.
    /// The current state of the state machine is represented in status.state.
    ///
    /// A default process is provided that should work for most purposes.
    async fn process(
        &mut self,
        sender: &mpsc::Sender<CommandFsmMessage>,
        command: &MechFsmCommandTuple,
        status: &mut MechFsmStatusTuple,
        //registers: &mut Vec<Vec<MechCommandRegisterValue>>
    ) -> Result<(), anyhow::Error> {
        match status.state {
            MechFsmState::Invalid => {
                // not a valid state. Push the FSM into the startup phase.
                status.state = MechFsmState::StartUp;
                return Ok(());
            }
            MechFsmState::StartUp => {
                return self.on_startup(status).await;
            }
            MechFsmState::Init => {
                match self.on_init(command, status).await {
                    Err(err) => {
                        return Err(err);
                    }
                    _ => {
                        if command.transaction_id != status.transaction_id && command.is_valid() {
                            //
                            // In Init, there is only possible control: move to Idle. Everything else is ignored.
                            //
                            if command.control == MechFsmControl::Idle {
                                match self.change_state(&MechFsmState::Idle, status).await {
                                    Err(err) => {
                                        return Err(anyhow!(
                                            "Failed to change from INIT to RESET: {}",
                                            err
                                        ));
                                    }
                                    _ => {
                                        return Ok(());
                                    }
                                }
                            } else {
                                return Ok(());
                            }
                        } else {
                            return Ok(());
                        }
                    }
                }
            }
            MechFsmState::Idle => {
                // if let Err(err) = self.on_idle(command, status, registers) {
                //     return Err(err);
                // }
                // else {
                //     return Ok(());
                // }
                return self
                    .on_idle(sender, command, status /* , registers */)
                    .await;
            }
            MechFsmState::Executing => {
                // We are running a long-term command. The derived instance will push
                // us into CmdDone when it has completed its work. Until then, we do
                // nothing.
                return Ok(());
            }
            MechFsmState::CmdDone => {
                if command.is_valid() {
                    match command.control {
                        MechFsmControl::AckDone => {
                            if let Err(err) = self.change_state(&MechFsmState::Idle, status).await {
                                return Err(anyhow!(
                                    "Failed to change from CmdDone to IDLE: {}",
                                    err
                                ));
                            }
                        }
                        MechFsmControl::Restart => {
                            // the external client wants a reset
                            if let Err(err) =
                                self.change_state(&MechFsmState::StartUp, status).await
                            {
                                return Err(anyhow!(
                                    "Failed to change from CmdDone to INIT: {}",
                                    err
                                ));
                            }
                        }
                        _ => {
                            // do nothing
                        }
                    }
                }

                return Ok(());
            }
            MechFsmState::Error => {
                if command.is_valid() {
                    match command.control {
                        MechFsmControl::AckDone => {
                            if let Err(err) = self.change_state(&MechFsmState::Idle, status).await {
                                return Err(anyhow!(
                                    "Failed to change from Error to IDLE: {}",
                                    err
                                ));
                            }
                        }
                        MechFsmControl::Restart => {
                            // the external client wants a reset
                            if let Err(err) =
                                self.change_state(&MechFsmState::StartUp, status).await
                            {
                                return Err(anyhow!(
                                    "Failed to change from Error to INIT: {}",
                                    err
                                ));
                            }
                        }
                        _ => {
                            // do nothing
                        }
                    }
                }

                return Ok(());
            }
        }
    }

    /// This state allows the FSM to handle any internal startup details, like opening/re-opening communications.
    /// Like Init, it can be called more than once, if the external client requests a hard reset, though that
    /// should not be considered normal operation, but rather a response to an error.
    ///
    /// The FSM will not attempt to read commands or write status while in the StartUp state. The decision
    /// to move from StartUp to Init is made internally, with no consideration for the remote client. The
    /// FSM should move from StartUp into Init as soon as possible. The CommandFsmActor will never do this
    /// automatically; the on_startup function is exected to call self.change state to move into Init.
    ///
    /// No command argument is passed with the function; the StartUp phase is intended to be internal and
    /// independent from the external fsm client.
    ///
    /// Default implementation simply moves into the Init state.
    async fn on_startup(&mut self, status: &mut MechFsmStatusTuple) -> Result<(), anyhow::Error> {
        match self.change_state(&MechFsmState::Init, status).await {
            Err(err) => {
                return Err(anyhow!("Failed to change to Init state: {:?}", err));
            }
            _ => {
                return Ok(());
            }
        }
    }

    /// Called when the state init status.
    /// This state is the entry-point of the process, and is inteded to intialize process variables.
    /// If the remote client wants to reset the process or a command, it will send an Init request,
    /// jumping the FSM back to this state.
    ///
    /// This state can be called more than once sequentially, as the external client may not want the
    /// process to run, or perhaps because of a safety issue.
    ///
    /// /// Default implementation simply moves into the Idle state.
    async fn on_init(
        &mut self,
        _command: &MechFsmCommandTuple,
        status: &mut MechFsmStatusTuple,
    ) -> Result<(), anyhow::Error> {
        match self.change_state(&MechFsmState::Idle, status).await {
            Err(err) => {
                return Err(anyhow!("Failed to change to Idle state: {:?}", err));
            }
            _ => {
                return Ok(());
            }
        }
    }

    /// Called when the FSM is in Reset stastus.
    /// Reset may be called many times over the lifetime of this instance, and often many times sequentially.

    /// Check the command tuple for a control code and execute accordingly.
    /// The current state of the state machine is represented in status.state.
    ///
    /// A default process is provided that should work for most purposes.
    async fn on_idle(
        &mut self,
        sender: &mpsc::Sender<CommandFsmMessage>,
        command: &MechFsmCommandTuple,
        status: &mut MechFsmStatusTuple,
        //registers : &mut Vec<Vec<MechCommandRegisterValue>>
    ) -> Result<(), anyhow::Error> {
        if command.transaction_id == status.transaction_id || !command.is_valid() {
            // Nothing to evaluate
            return Ok(());
        }

        // Indicate the transaction ID we're currently working on.
        status.transaction_id = command.transaction_id;

        match command.control {
            MechFsmControl::NoOp => {
                // No operation : ignore
            }
            MechFsmControl::Restart => {
                if let Err(err) = self.change_state(&MechFsmState::StartUp, status).await {
                    return Err(anyhow!("Failed to change to INIT state: {:?}", err));
                }
            }
            MechFsmControl::Idle => {
                // already here. Nothing to do.
            }
            MechFsmControl::Exec => {
                // The upstream struct needs to execute the command.

                if let Err(err) = self.handle_exec_command(sender, command, status).await {
                    let _ = self.change_state(&MechFsmState::Error, status).await;
                    return Err(anyhow!("Failed to change to execute command: {:?}", err));
                }

                // else {
                //     if let Err(err) = self.change_state(&MechFsmState::CmdDone, status).await {
                //         return Err(anyhow!("Failed to change to CmdDone state: {:?}", err));
                //     }
                // }
            }
            MechFsmControl::Write => {
                match self
                    .on_write_register(command, status /*,  registers */)
                    .await
                {
                    Err(err) => {
                        let _ = self.change_state(&MechFsmState::Error, status).await;
                        log::debug!("Error writing register: {:?}", err);
                        return Err(anyhow!("Error writing register: {:?}", err));
                    }
                    _ => {
                        if let Err(err) = self.change_state(&MechFsmState::CmdDone, status).await {
                            return Err(anyhow!("Failed to change to CmdDone state: {:?}", err));
                        }
                    }
                }
            }
            MechFsmControl::Read => match self
                .on_read_register(command, status /* , registers */)
                .await
            {
                Err(err) => {
                    let _ = self.change_state(&MechFsmState::Error, status).await;
                    log::debug!("Error writing register: {:?}", err);
                    return Err(anyhow!("Error writing register: {:?}", err));
                }
                _ => {
                    if let Err(err) = self.change_state(&MechFsmState::CmdDone, status).await {
                        return Err(anyhow!("Failed to change to CmdDone state: {:?}", err));
                    }
                }
            },
            _ => {
                // Nothing else has meaning here.
            }
        }

        return Ok(());
    }

    /// Process the `Exec` command received by the state machine.
    async fn handle_exec_command(
        &mut self,
        sender: &mpsc::Sender<CommandFsmMessage>,
        command: &MechFsmCommandTuple,
        status: &mut MechFsmStatusTuple,
    ) -> Result<(), anyhow::Error> {
        match self.on_exec_command(sender, command, status).await {
            Err(err) => {
                let _ = self.change_state(&MechFsmState::Error, status).await;
                return Err(err);
            }
            Ok(res) => {
                match res {
                    CommandFsmResult::Done => {
                        match self.change_state(&MechFsmState::CmdDone, status).await {
                            Ok(()) => {
                                return Ok(());
                            }
                            Err(err) => {
                                match err {
                                    StateChangeError::Rejected(res) => {
                                        // do not change state, but don't set error
                                        debug!("State change rejected: {}", res);
                                        return Ok(());
                                    }
                                    _ => {
                                        let _ =
                                            self.change_state(&MechFsmState::Error, status).await;
                                        return Err(anyhow!("State change cancelled by handler."));
                                    }
                                }
                            }
                        }
                    }
                    CommandFsmResult::Running => {
                        // change state to Executing
                        let _ = self.change_state(&MechFsmState::Executing, status).await;
                        return Ok(());
                    }
                }
            }
        }
    }

    /// This function will attempt to change the state of the FSM. If gives the upstream struct
    /// a callback and will cancel the state change if that callback returns an error, at which
    /// point the FSM is shifted into t
    async fn change_state(
        &mut self,
        new_state: &MechFsmState,
        status: &mut MechFsmStatusTuple,
    ) -> Result<(), StateChangeError> {
        log::debug!("request change state: {} -> {}", status.state, new_state);

        match self.on_state_change(new_state).await {
            Err(err) => {
                match &err {
                    StateChangeError::Rejected(res) => {
                        debug!("State change rejected: {}", res);
                        return Ok(());
                    }
                    StateChangeError::Fault(res) => {
                        error!("State change fault: {}", res);
                    }
                    StateChangeError::Unknown => {
                        error!("State change Unknown Error. ");
                    }
                }

                let _ = self.on_state_change(&MechFsmState::Error);
                status.state = MechFsmState::Error;
                return Err(err);
            }
            _ => {
                status.state = *new_state;
                return Ok(());
            }
        }
    }

    /// This will be called as the last step in changing the state.
    /// This gives the upstream struct a chance to handle any business or reject the state change.
    /// If an Error is returned, the state is not changed and, depending upon the error type, the state
    /// may be shifted into the error state.
    ///
    /// If the new_state argument is Error, then the FSM will be shifted into Error, no matter then
    /// return value.
    ///
    /// The default implementation does nothing.
    async fn on_state_change(&mut self, _new_state: &MechFsmState) -> Result<(), StateChangeError> {
        return Ok(());
    }

    /// Called when the state machine receives an `Exec` command.
    ///
    /// This is where the specific logic for executing the command will be defined
    /// in the implementing state machine.
    ///
    /// This function cannot block for more than the tick interval! Long-running commands
    /// need to span a task, then use the sender to change the state.
    ///
    /// When executing a long running command, the return value should be Ok(CommandFsmResult::Running).
    /// The state machine will enter the Executing state and wait for the sender to change the state out.
    /// The tick will keep updating, though, so other tasks, like the heartbeat, will continue.
    ///
    /// If running a command that returns immediately, return OK(CommandFsmResult::Done).
    /// The state machine will forward itself to the CmdDone state and complete the command.
    ///
    /// Example of a command that returns immediately.
    /// ```ignore
    /// async fn on_exec_command(
    ///     &mut self,
    ///     sender: &mpsc::Sender<CommandFsmMessage>,
    ///     _command: &MechFsmCommandTuple,
    ///     status : &mut MechFsmStatusTuple
    /// ) -> Result<CommandFsmResult, anyhow::Error> {
    ///     
    ///     println!("This command returns immediately...");
    ///     
    ///     // Feel free to write some data to the status
    ///     let mut v = MechCommandRegisterValue::new();
    ///     v.data[0] = 1;
    ///     status_clone.data = v;
    ///
    ///     // Return the Done code so that the engine knows it can complete immediately.
    ///     // The engine will transition to the CmdDone state.
    ///     Ok(CommandFsmResult::Done)
    /// }
    /// ```
    ///
    /// Example of a long-running command.
    /// ```ignore
    /// async fn on_exec_command(
    ///     &mut self,
    ///     sender: &mpsc::Sender<CommandFsmMessage>,
    ///     _command: &MechFsmCommandTuple,
    ///     status : &mut MechFsmStatusTuple
    /// ) -> Result<CommandFsmResult, anyhow::Error> {
    ///     
    ///     // Get a sender and clone it.
    ///     let tx = sender.clone();
    ///     let mut status_clone = status.clone();
    ///     
    ///     tokio::spawn( async move {
    ///         println!("Taking forever to execute command...");
    ///         tokio::time::sleep(Duration::from_millis(7000)).await;  
    ///     
    ///         println!("Sending message that CmdDone...");
    ///     
    ///         // Feel free to write some data to the status
    ///         let mut v = MechCommandRegisterValue::new();
    ///          v.data[0] = 1;
    ///         status_clone.data = v;
    ///     
    ///         if let Err(err) = tx.send(                
    ///             CommandFsmMessage::ChangeState { state: MechFsmState::CmdDone, status: status_clone}
    ///         ).await {
    ///             error!("Failed to send change state command: {}", err);
    ///         }
    ///     });
    ///     
    ///     // Return the Running code so that the engine knows this command will take awhile.
    ///     // The engine will transition to the Executing state. Other background functions will
    ///     // continue, but the state machine will stay in the Executing state until receiving the
    ///     // ChangeState message above.
    ///     Ok(CommandFsmResult::Running)
    /// }
    /// ```
    async fn on_exec_command(
        &mut self,
        sender: &mpsc::Sender<CommandFsmMessage>,
        command: &MechFsmCommandTuple,
        status: &mut MechFsmStatusTuple,
    ) -> Result<CommandFsmResult, anyhow::Error>;

    /// Default implementation for writing a register. Returns an error.
    /// A struct implmenting CommandFsmHandler must reimplement this function so that
    /// it has access to the register data.
    async fn on_write_register(
        &mut self,
        _command: &MechFsmCommandTuple,
        _status: &mut MechFsmStatusTuple,
    ) -> Result<(), anyhow::Error> {
        return Err(anyhow!("This FSM does not support writing registers."));
    }

    /// Default implementation of reading an internal register. Returns an error.
    /// A struct implmenting CommandFsmHandler must reimplement this function so that
    /// it has access to the register data.
    async fn on_read_register(
        &mut self,
        _command: &MechFsmCommandTuple,
        _status: &mut MechFsmStatusTuple,
    ) -> Result<(), anyhow::Error> {
        return Err(anyhow!("This FSM does not support reading registers."));
    }

    // /// Default implementation of writing an internal register.
    // async fn on_write_register(
    //     &mut self,
    //     command: &MechFsmCommandTuple,
    //     status: &mut MechFsmStatusTuple,
    //     registers: &mut Vec<Vec<MechCommandRegisterValue>>
    // ) -> Result<(), anyhow::Error> {

    //     if command.index > 0 {

    //         let group;
    //         if (command.index as usize) >= registers.len() {
    //             return Err(anyhow!("command.index {} out of range of registers.", command.index));
    //         }
    //         else {
    //             group = &mut registers[command.index as usize];
    //         }

    //         let target;
    //         if (command.sub_index as usize) >= group.len() {
    //             return Err(anyhow!("command.sub_index {} out of range of group {}",
    //                 command.sub_index,
    //                 command.index
    //             ));
    //         }
    //         else {
    //             target = &mut group[command.sub_index as usize];
    //         }

    //         *target = command.data.clone();

    //         return Ok(());
    //     }

    //     return Ok(());

    // }

    // /// Default implementation of reading an internal register.
    // async fn on_read_register(
    //     &mut self,
    //     command: &MechFsmCommandTuple,
    //     status: &mut MechFsmStatusTuple,
    //     registers: &mut Vec<Vec<MechCommandRegisterValue>>
    // ) -> Result<(), anyhow::Error> {

    //     if command.index > 0 {

    //         let group;
    //         if (command.index as usize) >= registers.len() {
    //             return Err(anyhow!("command.index {} out of range of registers.", command.index));
    //         }
    //         else {
    //             group = &registers[command.index as usize];
    //         }

    //         let target;
    //         if (command.sub_index as usize) >= group.len() {
    //             return Err(anyhow!("command.sub_index {} out of range of group {}",
    //                 command.sub_index,
    //                 command.index
    //             ));
    //         }
    //         else {
    //             target = &group[command.sub_index as usize];
    //         }

    //         status.data = target.clone();

    //         return Ok(());
    //     }

    //     return Ok(());

    // }

    /// Write the FSM status back to the client/master.
    /// The upstream struct is welcome to modify the status tuple as needed.
    /// This is the last function called on every tick.
    ///
    /// If an error is returned, the FSM will shift to an error state. If Ok(()), the FSM proceeds
    /// normally.
    async fn write_status(&mut self, status: &mut MechFsmStatusTuple) -> Result<(), anyhow::Error>;

    /// Write the latest heartbeat count to the client/master. This is called independent
    /// of the tick so that long operations don't interrupt the heartbeat.
    ///
    /// If an error is returned, the FSM will shift to an error state. If Ok(()), the FSM proceeds
    /// normally.
    ///
    /// The default implementation does nothing, but usage of the heartbeat is recommended.
    async fn write_heartbeat(&mut self, _heartbeat_count: i64) -> Result<(), anyhow::Error> {
        return Ok(());
    }

    /// Called only once as part of the shutdown process.
    ///
    /// Default implementation does nothing.
    async fn on_shutdown(&mut self, _status: &mut MechFsmStatusTuple) -> Result<(), anyhow::Error> {
        return Ok(());
    }
}

struct CommandFsmActor {
    /// Current status of this instance, as of the last Tick.
    current_status: MechFsmStatusTuple,
    /// This is the upstream struct that implements this CommandFsmActor
    handler: Box<dyn CommandFsmHandler + Send>,

    /// Local value registers that the external client can use.
    /// There are commands to read and write values, and the upstream
    /// struct can use these as properties or arguments to functions, as needed.
    // registers : Vec<Vec<MechCommandRegisterValue>>,

    /// Interval at which the hearbeat is ticked.
    heartbeat_interval: Duration,
    /// Last counter value sent with the last heartbeat. Should increment every time.
    last_heartbeat: i64,
}

impl CommandFsmActor {
    pub fn new(handler: Box<dyn CommandFsmHandler + Send>, /*, num_registers : usize */) -> Self {
        // let registers : Vec<Vec<MechCommandRegisterValue>> = vec![0;num_registers];

        Self {
            current_status: MechFsmStatusTuple::new(),
            handler: handler,
            // registers : registers,
            heartbeat_interval: Duration::from_millis(1000),
            last_heartbeat: 0,
        }
    }

    async fn handle_message(&mut self, msg: CommandFsmMessage) {
        match msg {
            CommandFsmMessage::Command {
                sender,
                respond_to,
                cmd,
            } => {
                let _ = self
                    .handler
                    .process(
                        &sender,
                        &cmd,
                        &mut self.current_status, /* , &mut self.registers */
                    )
                    .await;
                let _ = respond_to.send(self.current_status);
            }
            CommandFsmMessage::Heartbeat { heartbeat } => {
                let _ = self.handler.write_heartbeat(heartbeat).await;
            }
            CommandFsmMessage::ChangeState { state, status } => {
                self.current_status = status;
                let _ = self
                    .handler
                    .change_state(&state, &mut self.current_status)
                    .await;
            }
            CommandFsmMessage::ReadState { respond_to } => {
                let _ = respond_to.send(self.current_status.state);
            }
            _ => {
                // Unknown message. Ignore.
            }
        }
    }

    // /// Wait for an acknowledgment after command execution.
    // async fn wait_for_ack(
    //     &mut self,
    //     command: &MechFsmCommandTuple,
    //     status: &mut MechFsmStatusTuple,
    // ) -> Result<(), anyhow::Error> {
    //     // Polling or waiting logic for acknowledgment
    //     // Transition to CmdDone state once received
    //     // Logic here can be standardized for most state machines

    //     if command.transaction_id == status.transaction_id
    //         && command.control == MechFsmControl::AckDone
    //     {
    //         if let Err(err) = self.handler.on_state_change(&MechFsmState::Idle).await {
    //             info!("on_state_change rejected: {}", err);
    //             status.state = MechFsmState::Error;
    //             let _ = self.handler.on_state_change(&MechFsmState::Error);
    //             return Err(anyhow!("on_state_change rejected: {}", err));
    //         } else {
    //             status.state = MechFsmState::Idle;
    //         }
    //     } else if command.control == MechFsmControl::Restart {
    //         status.state = MechFsmState::Error;
    //         let _ = self.handler.on_state_change(&MechFsmState::Error);
    //         // Not returning an error here. We're responding to a request.
    //     }

    //     return Ok(());
    // }

    /// Tick the state machine. Should be called on regular intervals.
    async fn tick(&mut self, sender: &mpsc::Sender<CommandFsmMessage>) {
        let mut command = MechFsmCommandTuple::new();

        if self.current_status.state == MechFsmState::Invalid {
            self.current_status.state = MechFsmState::StartUp;
        }

        // Store the new entry state. Don't transition into startup and then
        // write out the command in the same scan.
        let entry_state = self.current_status.state;

        if entry_state != MechFsmState::StartUp {
            if let Err(err) = self.handler.read_command(&mut command).await {
                log::error!(
                    "Error reading commmand tuple: {} in state {}",
                    err,
                    entry_state
                );

                if self.current_status.state != MechFsmState::Error {
                    let _ = self
                        .handler
                        .change_state(&MechFsmState::Error, &mut self.current_status)
                        .await;
                }
            }
        }

        // Process the state machine.
        if let Err(err) = self
            .handler
            .process(
                sender,
                &command,
                &mut self.current_status,
                // &mut self.registers
            )
            .await
        {
            log::error!("CommandFsmActor::process error: {}", err);
        }

        // Update the status in the remote client.
        if entry_state != MechFsmState::StartUp {
            self.current_status.update_crc32();
            if let Err(err) = self.handler.write_status(&mut self.current_status).await {
                log::error!("Error writing status tuple: {}", err);

                if self.current_status.state != MechFsmState::Error {
                    let _ = self
                        .handler
                        .change_state(&MechFsmState::Error, &mut self.current_status)
                        .await;
                }
            }
        }
    }

    /// Ticks writing the heartbeat in the derived child.
    async fn tick_heartbeat(&mut self) {
        // If we're not past these states, then we don't have a communication pipeline with which to
        // write the heartbeat.
        if self.current_status.state != MechFsmState::Invalid
            && self.current_status.state != MechFsmState::StartUp
        {
            self.last_heartbeat += 1;
            if let Err(err) = self.handler.write_heartbeat(self.last_heartbeat).await {
                log::error!("CommandFsmActor Error writing heartbeat: {}", err);
                let _ = self
                    .handler
                    .change_state(&MechFsmState::Error, &mut self.current_status)
                    .await;
            }
        }
    }
}

/// Responsible for ticking the Actor. This is the loop that ticks the FSM until
/// the Shutdown message is received.
async fn run_command_fsm(
    sender: mpsc::Sender<CommandFsmMessage>,
    mut actor: Box<CommandFsmActor>,
    mut receiver: mpsc::Receiver<CommandFsmMessage>,
    interval: Duration,
) {
    // Initialize state and timers
    actor.current_status = MechFsmStatusTuple::new();
    actor.current_status.state = MechFsmState::StartUp;

    let mut last_tick_time = tokio::time::Instant::now();
    let mut last_heartbeat_time = tokio::time::Instant::now();
    let heartbeat_interval = actor.heartbeat_interval;
    let mut fin: Option<oneshot::Sender<bool>> = None;

    //
    // Regarding this loop: Originally, we used tokio::select!, but quickly ran into
    // performance and overhead problem. Spawning another task when running a command
    // would take 2 - 4 seconds, which isn't acceptable.
    //
    // We were also using multiple tickers, one for the main tick and one for the
    // heartbeat tick. That had additional overhead in and of itself.
    //
    // In this version, we simply have a single loop, no overhead of the select! macro
    // or the tickers. We execute tick or hearbeat_tick based upon elapsed time.
    // Spawning additional tasks from within a command is now instantaneous. The overhead
    // of this loop is much lower.
    //
    loop {
        // FSM tick based on interval
        if last_tick_time.elapsed() >= interval {
            actor.tick(&sender).await;
            last_tick_time = tokio::time::Instant::now();
        }

        // Heartbeat tick based on heartbeat interval
        if last_heartbeat_time.elapsed() >= heartbeat_interval {
            actor.tick_heartbeat().await;
            last_heartbeat_time = tokio::time::Instant::now();
        }

        // Check and handle incoming messages
        if let Ok(msg) = receiver.try_recv() {
            match msg {
                CommandFsmMessage::Shutdown { respond_to } => {
                    fin = Some(respond_to);
                    break;
                }
                _ => actor.handle_message(msg).await,
            }
        }

        // Sleep briefly to yield, preventing the loop from consuming too much CPU
        // todo: this should be adjustable.
        tokio::time::sleep(Duration::from_millis(10)).await;
    }

    debug!("run_command_fsm done exited loop. Shutting down actor...");

    // Finalize shutdown
    if let Some(sender) = fin {
        let _ = sender.send(true);
    }

    if let Err(err) = actor.handler.on_shutdown(&mut actor.current_status).await {
        error!("Error shutting down command fsm: {}", err);
        return;
    }

    info!("run_command_fsm shutdown completed successfully.");
}

/// Handle that contains, starts and runs the CommandFsmActor.
pub struct CommandFsmHandle {
    sender: mpsc::Sender<CommandFsmMessage>,
    running: Arc<AtomicBool>,
}

impl CommandFsmHandle {
    /// Creates a new `CommandFsmActor` that ticks at the specified `interval`.
    ///
    /// # Arguments
    ///
    /// * `interval` - The time duration between each tick.
    ///
    /// # Examples
    ///
    /// ```ignore
    /// use mechutil::CommandFsmHandle;
    /// use std::time::Duration;
    ///
    /// let handle = CommandFsmHandle::new(Duration::from_secs(1));
    /// ```    
    pub fn new(handler: Box<dyn CommandFsmHandler + Send>, interval: Duration) -> Self {
        let (sender, receiver) = mpsc::channel(8);
        let actor = Box::new(CommandFsmActor::new(handler));

        let running = Arc::new(AtomicBool::new(true));

        tokio::spawn(run_command_fsm(sender.clone(), actor, receiver, interval));
        // tokio::spawn(run_heartbeat(sender.clone(), running.clone()));

        Self {
            sender: sender,
            running: running,
        }
    }

    /// Pass a command to the CommandFsmActor to be processed immediately; it does not
    /// wait for the tick to occur.
    pub async fn command(&self, cmd: &MechFsmCommandTuple) -> Result<(), anyhow::Error> {
        let mut cmd_out = cmd.clone();
        cmd_out.transaction_id = u32::MAX;
        cmd_out.update_crc32();

        let (send, recv) = oneshot::channel();
        let msg = CommandFsmMessage::Command {
            sender: self.sender.clone(),
            respond_to: send,
            cmd: cmd_out,
        };

        // Ignore send errors. If this send fails, so does the
        // recv.await below. There's no reason to check for the
        // same failure twice.
        let _ = self.sender.send(msg).await;
        match recv.await {
            Ok(res) => {
                if res.state == MechFsmState::Executing {
                    loop {
                        tokio::time::sleep(Duration::from_millis(20)).await;

                        let (read_state_send, read_state_recv) = oneshot::channel();
                        let _ = self
                            .sender
                            .send(CommandFsmMessage::ReadState {
                                respond_to: read_state_send,
                            })
                            .await;
                        match read_state_recv.await {
                            Ok(current_state) => {
                                if current_state != MechFsmState::Executing {
                                    break;
                                }
                            }
                            Err(err) => {
                                error!("Error checking state while FSM was Executing: {}", err);
                                // return Err(anyhow!("Error checking state while FSM was Executing: {}", err));
                            }
                        }
                    }
                }

                Ok(())
            }
            Err(err) => return Err(anyhow!("Error sending command: {}", err)),
        }
    }

    /// Shuts down the Actor.
    ///
    /// # Examples
    ///
    /// ```ignore
    /// let _ = actor.shutdown().await;
    /// ```    
    pub async fn shutdown(&self) -> Result<(), anyhow::Error> {
        let (send, recv) = oneshot::channel();
        match self
            .sender
            .send(CommandFsmMessage::Shutdown { respond_to: send })
            .await
        {
            Ok(_) => {
                if let Err(err) = recv.await {
                    return Err(anyhow!("Actor error processing shutdown: {}", err));
                }

                println!("Shut down party town");
                self.running.store(false, Ordering::Relaxed);
                Ok(())
            }
            Err(err) => return Err(anyhow!("Error shutting down Actor: {}", err)),
        }
    }
}