itla 0.1.2

//! Non-blocking library to interface with ITLA lasers based upon embedded-hal
//! See https://www.oiforum.com/wp-content/uploads/2019/01/OIF-ITLA-MSA-01.3.pdf
//!   for all informations concerning ITLA lasers.
//! This library also includes special vendor-specific commands for the PPCL200
//!   laser used by PolyOrbite
//!
//! For any questions, please contact Xavier L'Heureux at xavier.lheureux@polymtl.ca

// ITLA-rs
// Copyright (C) 2021  PolyOrbite
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published
// by the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
//

#![cfg_attr(not(test), no_std)]

use core::convert::TryInto;
use core::fmt;
#[cfg(not(test))]
use defmt::*;
use embedded_hal::{
    digital::v2::{InputPin, OutputPin},
    serial::{Read, Write},
};
use heapless::Vec;
#[cfg(test)]
use log::*;

#[cfg(not(test))]
use defmt as logger;
#[cfg(test)]
use std as logger;

// Registers that are available for requests to the ITLA laser
pub mod registers;
#[cfg(test)]
pub mod tests;

pub use registers::{CommandStatus, ReadRegister, Request, Response, WriteRegister};

// Used by `wait_for_clear` to determine what to do after a request
#[derive(Debug, Clone, Copy, defmt::Format, PartialEq, Eq)]
pub enum AfterRequest {
    Continue,
    Exit,
}

#[derive(Debug, Clone, Copy, defmt::Format, PartialEq, Eq)]
enum LaserTransferState {
    // Sending a command
    Command(Request),
    // Wait state when commands return pending
    WaitForClear(Response),
    // Requesting more info when commands fail
    ReadErrorStatus(Request),
    // Ready for new commands
    Ready,
}

pub enum Error<S: Read<u8> + Write<u8>, I: InputPin, R: OutputPin, M: OutputPin> {
    Tx(<S as Write<u8>>::Error),
    Rx(<S as Read<u8>>::Error),
    SrqPin(<I as InputPin>::Error),
    ResetPin(<R as OutputPin>::Error),
    CommResetPin(<M as OutputPin>::Error),
    Srq(Response),
    CommandError(Request, Response),
    TooManyRetries(u32),
}

impl<S: Read<u8> + Write<u8>, I: InputPin, R: OutputPin, M: OutputPin> fmt::Debug
    for Error<S, I, R, M>
where
    <S as Write<u8>>::Error: fmt::Debug,
    <S as Read<u8>>::Error: fmt::Debug,
    <I as InputPin>::Error: fmt::Debug,
    <R as OutputPin>::Error: fmt::Debug,
    <M as OutputPin>::Error: fmt::Debug,
{
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            Self::Tx(tx) => core::write!(f, "Error while transmitting: {:?}", tx),
            Self::Rx(rx) => core::write!(f, "Error while receiving: {:?}", rx),
            Self::SrqPin(srq) => core::write!(f, "Could not read srq pin: {:?}", srq),
            Self::ResetPin(reset) => core::write!(f, "Could not reset pin: {:?}", reset),
            Self::CommResetPin(comms) => {
                core::write!(f, "Could not change communication reset pin: {:?}", comms)
            }
            Self::Srq(_response) => core::write!(f, "Requested attention"),
            Self::CommandError(req, resp) => {
                core::write!(f, "Command {:?} returned error {:?}", req, resp)
            }
            Self::TooManyRetries(retries) => {
                core::write!(f, "The laser was not ready after {} retries", retries)
            }
        }
    }
}

#[derive(Debug)]
pub struct Laser<R, D, M, I, S, F> {
    // Pins for physical communication (See page 21 of RM)
    n_reset: R,
    n_disable: D,
    n_ms: M,
    srq: I,
    serial: S,
    // Each command/response is 4 bytes long (See page 26 of RM)
    tx_buf: Vec<u8, 4>,
    rx_buf: Vec<u8, 4>,
    state: LaserTransferState,
    // Was the last command/response ok? (indicates retry)
    last_ok: bool,
    // to add delay between successive waiting
    after_request: Option<F>,
    retries: u32,
}

#[derive(Debug, defmt::Format, Clone, Copy, PartialEq, Eq)]
#[non_exhaustive]
// Execution error during communication with the laser
// See *Execution Error Field Conditions* at page 19 of RM
pub enum LastCommandError {
    Ok = 0x00,
    RegisterNotImplemented = 0x01,
    RegisterNotWriteable = 0x02,
    RegisterValueRangeError = 0x03,
    Pending = 0x04,
    Initializing = 0x05,
    AddressInvalid = 0x06,
    AddressReadOnly = 0x07,
    ExecutionFailure = 0x08,
    OutputEnabled = 0x09,
    InvalidConfig = 0x0A,
    Vendor = 0x0F,
}

impl<R, D, M, I, S> Laser<R, D, M, I, S, fn(u32) -> AfterRequest>
where
    R: OutputPin,
    D: OutputPin,
    M: OutputPin,
    I: InputPin,
    S: Read<u8> + Write<u8>,
{
    // Reset and establish communication with a new ITLA laser
    pub fn new(
        n_reset: R,
        n_disable: D,
        n_ms: M,
        srq: I,
        serial: S,
    ) -> Result<Self, Error<S, I, R, M>> {
        Self::new_with_handler_between_retries(n_reset, n_disable, n_ms, srq, serial, None)
    }
}

impl<R, D, M, I, S, F> Laser<R, D, M, I, S, F>
where
    R: OutputPin,
    D: OutputPin,
    M: OutputPin,
    I: InputPin,
    S: Read<u8> + Write<u8>,
    F: FnMut(u32) -> AfterRequest,
{
    // Reset and establish communication with a new ITLA laser
    // `after_request` is called after each verification of the status and can be used
    //   for exponential backoff by adding delay between requests or for adding timeouts
    //   by returning AfterRequest::Exit
    pub fn new_with_handler_between_retries(
        n_reset: R,
        n_disable: D,
        n_ms: M,
        srq: I,
        serial: S,
        between_retries: Option<F>,
    ) -> Result<Self, Error<S, I, R, M>> {
        let mut this = Self {
            n_reset,
            n_disable,
            n_ms,
            srq,
            serial,
            tx_buf: Vec::new(),
            rx_buf: Vec::new(),
            state: LaserTransferState::Ready,
            last_ok: true,
            after_request: between_retries,
            retries: 0,
        };
        this.n_ms.set_high().map_err(Error::CommResetPin)?;
        this.reset().map_err(Error::ResetPin)?;
        Ok(this)
    }

    // Release all the components of the laser control
    pub fn release(self) -> (R, D, M, I, S) {
        (
            self.n_reset,
            self.n_disable,
            self.n_ms,
            self.srq,
            self.serial,
        )
    }

    // Get access to the serial link. This can be used for example
    //   to clear interrupts.
    // WARNING: Do not read or write on the link. This can
    //   interfere with the library and cause unexpected behaviour
    pub fn serial(&mut self) -> &mut S {
        &mut self.serial
    }

    // Prepare buffers for initiating communication
    fn start_send(&mut self) {
        // Sanity tests
        logger::assert!(self.tx_buf.is_empty(), "Tx buffer is not empty");
        logger::assert!(self.rx_buf.is_empty(), "Rx buffer is not empty");
        let req = self
            .current_command()
            .expect("Need a command to start sending it to the laser");

        self.tx_buf
            .extend(req.encode(self.last_ok).iter().rev().copied());
        #[cfg(test)]
        debug!("Send command {:X?} => {:?}", &self.tx_buf[..], req);
        #[cfg(not(test))]
        debug!("Send command {:X} => {}", &self.tx_buf[..], req);
    }

    // Returns the command that is currently sent to the laser
    pub fn current_command(&self) -> Option<Request> {
        match self.state {
            LaserTransferState::Ready => None,
            LaserTransferState::Command(req) => Some(req),
            LaserTransferState::ReadErrorStatus(_req) => Some(registers::Status::read()),
            LaserTransferState::WaitForClear(_req) => Some(registers::Status::read()),
        }
    }

    // Advance the communication process. This library is non-blocking and, as such,
    //   this function will stop when the UART peripheral is no longer ready to communicate
    //   with it. You can either call `nb::block!` on it to wait in a tight loop or wait
    //   for the UART interrupt to further advance the task.
    // When this function either returns `Ok(…)` or `Err(nb::Error::Other(…))`, the pending
    //   command is respectively completed or aborted. You should not call `step` again after
    //   this happens.
    pub fn step(&mut self) -> nb::Result<Response, Error<S, I, R, M>> {
        // Verify that there indeed is a command pending
        logger::assert_ne!(
            self.state,
            LaserTransferState::Ready,
            "Tried to poll after completion"
        );
        // Empty tx buffer
        while let Some(byte) = self.tx_buf.last().copied() {
            self.serial.write(byte).map_err(|e| e.map(Error::Tx))?;
            trace!("Printed {:X}", byte);
            // Do not pop before the peripheral has accepted the byte
            self.tx_buf.pop();
        }
        // Fill rx buffer
        while self.rx_buf.len() < 4 {
            let byte = self.serial.read().map_err(|e| e.map(Error::Rx))?;
            trace!("Received {:X}", byte);
            self.rx_buf.push(byte).expect("Length is less than 4");
        }
        // Convert to fixed-length buffer
        let buf: [u8; 4] = AsRef::<[u8]>::as_ref(&self.rx_buf)
            .try_into()
            .expect("Length is 4");
        let resp = Response::decode(buf);
        #[cfg(test)]
        debug!("Received response {:X?} => {:?}", self.rx_buf, resp);
        #[cfg(not(test))]
        debug!("Received response {:X} => {}", self.rx_buf, resp);
        self.rx_buf.clear();
        self.last_ok = resp.is_some();
        let resp = match resp {
            Some(r) if !r.ce => r,
            _ => {
                warn!("Communication error");
                self.start_send();
                return self.step();
            }
        };
        if resp.status == CommandStatus::ExecutionError {
            // If an execution error occurs, read the status field to retrieve the
            //  exact cause
            self.state = LaserTransferState::ReadErrorStatus(
                self.current_command().expect("Not in ready state"),
            );
            self.start_send();
            return self.step();
        }
        if resp.status != CommandStatus::Ok {
            #[cfg(test)]
            debug!("Status: {:?}", resp.status);
            #[cfg(not(test))]
            debug!("Status: {}", resp.status);
        }
        // Sanity check
        logger::assert_eq!(
            resp.register,
            self.current_command()
                .expect("Tested at top of function")
                .register(),
            "Request register is not output register?"
        );
        if self.srq().map_err(Error::SrqPin)? {
            return Err(Error::Srq(resp).into());
        }
        let out = match self.state {
            LaserTransferState::Ready => logger::unreachable!("Checked at start of function"),
            LaserTransferState::Command(_command) => {
                if resp.status == CommandStatus::CommandPending {
                    self.retries = 1;
                    // Wait for the command to complete on the laser side
                    self.state = LaserTransferState::WaitForClear(resp);
                    self.start_send();
                    self.step()
                } else {
                    self.state = LaserTransferState::Ready;
                    Ok(resp)
                }
            }
            LaserTransferState::ReadErrorStatus(command) => {
                self.state = LaserTransferState::Ready;
                error!("Error: {}", resp.val() & 0x00_0F);
                Err(Error::CommandError(command, resp).into())
            }
            LaserTransferState::WaitForClear(prev_resp) => {
                // See page 39 of RM for details of NOP command
                const PENDING_FLAGS: u16 = 0xFF_00;
                const MRDY_FLAG: u16 = 0x00_10;

                // Wait for no pending operations and for the module to be ready
                // TODO: Right now, the library requests from the laser in a busy loop. Provide
                //   option for delays between requests
                if resp.val() & PENDING_FLAGS == 0x00_00 && resp.val() & MRDY_FLAG == MRDY_FLAG {
                    self.state = LaserTransferState::Ready;
                    Ok(prev_resp)
                } else {
                    let retries = self.retries;
                    if self
                        .after_request
                        .as_mut()
                        .map_or(AfterRequest::Continue, |f| f(retries))
                        == AfterRequest::Continue
                    {
                        self.retries += 1;
                        self.start_send();
                        self.step()
                    } else {
                        Err(nb::Error::Other(Error::TooManyRetries(self.retries)))
                    }
                }
            }
        }?;
        Ok(out)
    }

    // Verify if the laser requested attention by the operator
    pub fn srq(&self) -> Result<bool, <I as InputPin>::Error> {
        self.srq.is_low()
    }

    // Disable the laser's output
    pub fn disable(&mut self) -> Result<(), <D as OutputPin>::Error> {
        self.n_disable.set_low()
    }

    // Enable the laser's output
    pub fn enable(&mut self) -> Result<(), <D as OutputPin>::Error> {
        self.n_disable.set_high()
    }

    // Reset the communication state to starting values
    fn reset_state(&mut self) {
        while self.serial.read().is_ok() {} // Clear input buffer
        self.tx_buf.clear();
        self.rx_buf.clear();
        self.last_ok = true;
        self.state = LaserTransferState::Ready;
    }

    // Reset the laser. This resets all unsaved state on the laser as
    //   well as all pending commands
    pub fn reset(&mut self) -> Result<(), <R as OutputPin>::Error> {
        self.n_reset.set_low()?;
        self.n_reset.set_high()?;
        self.reset_state();
        Ok(())
    }

    // Reset the communications with the laser. This resets all pending
    //   commands
    pub fn reset_comms(&mut self) -> Result<(), <M as OutputPin>::Error> {
        self.n_ms.set_low()?;
        self.n_ms.set_high()?;
        self.reset_state();
        Ok(())
    }

    // Start sending a command. To finish execution of the command, `laser.step()` must
    //   be called until Ok(…) or Err(Error::Other(…)) is returned.
    pub fn send(&mut self, command: Request) {
        logger::assert_eq!(
            self.state,
            LaserTransferState::Ready,
            "Can't send command while there is still one pending"
        );
        self.state = LaserTransferState::Command(command);
        self.start_send();
    }

    // Wait for the laser to be clear. i.e. to be ready to enable its output and to have
    //   finished pending commands.
    // This command is blocking.
    pub fn wait_for_clear(&mut self) -> nb::Result<(), Error<S, I, R, M>> {
        trace!("Waiting for laser clear");
        self.retries = 0;
        loop {
            self.send(registers::Status::read());
            let resp = nb::block!(self.step())?;
            let flags = resp.val();
            trace!("Laser flags: {:X}", flags);
            if flags & 0xFF_00 == 0 && flags & 0x00_10 == 0x00_10 {
                trace!("Laser clear");
                return Ok(());
            }
            let retries = self.retries;
            let early_exit = self
                .after_request
                .as_mut()
                .map_or(AfterRequest::Continue, |f| f(retries));
            if early_exit == AfterRequest::Exit {
                debug!("Requested early stop of wait_for_clear");
                return Err(nb::Error::Other(Error::TooManyRetries(self.retries)));
            }
            self.retries = self.retries.saturating_add(1);
        }
    }
}