/* twi.rs
 *
 * Developed by Tim Walls <tim.walls@snowgoons.com>
 * Copyright (c) All Rights Reserved, Tim Walls
 */
//! Generic Two Wire Interface (I2C & SPI) driver support

// Imports ===================================================================
use core::any::Any;
use ufmt::derive::uDebug;
use avr_oxide::concurrency::Isolated;
use avr_oxide::hal::generic::callback::IsrCallback;
use avr_oxide::io::IoError;
use avr_oxide::util::{OwnOrBorrow, OwnOrBorrowMut};
use oxide_macros::Persist;
use avr_oxide::OxideResult;
use avr_oxide::OxideResult::{Ok,Err};

// Declarations ==============================================================
/**
 * Identifies a particular source of TWI events, i.e. a particular
 * hardware TWI device, at runtime.
 */
#[derive(Clone,Copy,PartialEq,Eq,uDebug,Persist)]
pub enum TwiIdentity {
  /// TWI0 Master
  Twi0master,
  /// TWI0 Slave
  Twi0slave,
  /// TWI1 Master
  Twi1master,
  /// TWI1 Slave
  Twi1slave,
}

/// Two-Wire Interface Mode
#[derive(Clone,Copy,PartialEq,Eq,uDebug,Persist)]
pub enum InterfaceMode {
  /// I2C
  I2C,
  /// SDI
  SMBus
}

/// TWI port speeds
#[derive(Clone,Copy,PartialEq,Eq,uDebug,Persist)]
pub enum PortSpeed {
  /// Standard mode - 100KHz
  Standard,
  /// Fast mode - 400KHz
  Fast,
  /// Fast+ mode - 1MHz
  FastPlus
}

#[derive(Clone,Copy,PartialEq,Eq)]
#[cfg_attr(not(target_arch="avr"), derive(Debug))]
#[cfg_attr(target_arch="avr", derive(ufmt::derive::uDebug))]
pub enum TwiError {
  /// The device is busy and cannot accept the command
  DeviceBusy,

  /// A bus error prevented the command being sent
  BusError,

  /// No device acknowledged the command
  NotAcknowledged,

  /// Receive buffer overflowed
  BufferOverflow
}

/// A TWI device address
#[derive(Clone,Copy,PartialEq,Eq,uDebug,Persist)]
pub struct TwiAddr(u8);

/// Callback called when a TWI command is completed
pub type TwiCommandCompleteFunction = fn(Isolated, TwiIdentity, OxideResult<(Command<'static>, usize),TwiError>, Option<*const dyn Any>) -> ();
pub type TwiCommandCompleteCallback = IsrCallback<TwiCommandCompleteFunction,()>;

/// Trait for devices which can act as a TWI master
pub trait TwoWireMaster {
  /// Execute the command on the TWI bus.  If the bus is busy, block until
  /// it isn't.
  fn command(&mut self, command: Command<'static>);

  // Execute the given command on the TWI bus.  If the device is busy and
  // cannot execute the command, returns a DeviceBusy error and the
  // command (since we (deliberately) take ownership of the command, we
  // also need to return it if you want to be able to re-try.)
  fn try_command(&mut self, command: Command<'static>) -> OxideResult<(),(TwiError, Command<'static>)>;

  // Set the callback handler for completed commands
  fn set_command_complete_callback(&self, handler: TwiCommandCompleteCallback);
}

/// Trait for devices which can act as a TWI slave
pub trait TwoWireSlave {
  /// Set the address this device should respond to
  fn set_address(&mut self, address: TwiAddr);
}

pub type CommandBuffer = OwnOrBorrow<'static,[u8]>;
pub type ResponseBuffer = OwnOrBorrowMut<'static,[u8]>;
pub type CommandBufferStaticSet = &'static[&'static [u8]];

// Desired TWI command retry strategy
#[derive(Copy,Clone,Eq,PartialEq)]
pub enum RetryStrategy {
  // Commands will not be retried at all
  None,

  // Keep retrying until the entire command is completed
  UntilComplete,

  // Keep retrying until the slave device at least acknowledges our request.
  // Failures during the transaction will not cause it to be retried.
  UntilAcknowledged
}


pub enum NoneOneOrManyBuffer<'omb,T>
where
  T: Clone
{
  None,
  One(&'omb [T]),
  Many(&'omb [&'omb [T]])
}

pub enum NoneOneOrManyMutBuffer<'omb,T>
where
  T: Clone
{
  None,
  One(&'omb mut [T]),
  Many(&'omb [&'omb mut [T]])
}

/// A TWI command.  For simple writes and read, we can simply pass the driver a
/// buffer (or set of buffers), which will then be transmitted/received-into
/// with zero-copying.  For more complex cases, it is possible to instead
/// use a dynamic implementation of an object that implements the
/// TwiTransaction trait.
///
/// Because commands are processed asynchronously from the interrupt context,
/// ownership must pass to the driver.  The command will be given back
/// to the userland context when the `command_complete` callback is
/// executed.
pub struct Command<'c> {
  retry_strategy: RetryStrategy,
  address: TwiAddr,
  tx_buffer: NoneOneOrManyBuffer<'c,u8>,
  rx_buffer: NoneOneOrManyMutBuffer<'c,u8>
}

#[derive(Clone)]
pub struct CommandBuilder {
  retry_strategy: RetryStrategy,
  base_address: TwiAddr
}


pub trait TwiTransaction {
  /// Get the retry strategy to follow for this transaction
  fn get_retry_strategy(&self, isotoken: Isolated) -> RetryStrategy;

  /// Get the initial address to use for this transaction, as an 8-bit
  /// TWI address (i.e. including the read/write direction bit)
  fn get_initial_addr_bin(&self, isotoken: Isolated) -> u8;

  /// Get the read address for this transaction.  Returns None if there is
  /// no read address (i.e. this is a write-only transaction.)
  fn get_read_addr_bin(&self, isotoken: Isolated) -> Option<u8>;

  /// Get the byte to transmit at the given packet index, or return an error
  /// if there is no such byte.
  fn get_tx_byte(&self, isotoken: Isolated, index: usize) -> OxideResult<u8,IoError>;

  // Write the byte to the buffer at the given index.  Returns the number
  // of bytes space available after the written byte in the Ok() if
  // successful, otherwise an error.
  fn set_rx_byte(&mut self, isotoken: Isolated, index: usize, byte: u8) -> OxideResult<usize,IoError>;
}

// Code ======================================================================
/// Provides convenience functions for creating various common types of Command
/// implementation.
impl CommandBuilder {
  pub fn new(addr: TwiAddr) -> Self {
    CommandBuilder {
      retry_strategy: RetryStrategy::UntilComplete,
      base_address: addr
    }
  }

  pub fn address(self, addr: TwiAddr) -> Self {
    CommandBuilder {
      retry_strategy: self.retry_strategy,
      base_address: addr
    }
  }

  pub fn get_address(&self) -> TwiAddr {
    self.base_address
  }

  pub fn retry(&mut self, retry: RetryStrategy) -> &mut Self {
    self.retry_strategy = retry;
    self
  }

  pub fn write_cmd<'b>(&self, buffer: &'b[u8]) -> Command<'b> {
    Command {
      retry_strategy: self.retry_strategy.clone(),
      address: self.base_address.clone(),
      tx_buffer: NoneOneOrManyBuffer::One(buffer),
      rx_buffer: NoneOneOrManyMutBuffer::None
    }
  }

  pub fn write_multiple_cmd<'b>(&self, buffers: &'b[&'b[u8]]) -> Command<'b> {
    Command {
      retry_strategy: self.retry_strategy.clone(),
      address: self.base_address.clone(),
      tx_buffer: NoneOneOrManyBuffer::Many(buffers),
      rx_buffer: NoneOneOrManyMutBuffer::None
    }
  }

  pub fn read_cmd<'b>(&self, buffer: &'b mut [u8]) -> Command<'b> {
    Command {
      retry_strategy: self.retry_strategy.clone(),
      address: self.base_address.clone(),
      tx_buffer: NoneOneOrManyBuffer::None,
      rx_buffer: NoneOneOrManyMutBuffer::One(buffer)
    }
  }

  pub fn wtr_cmd<'b>(&self, write_buffer: &'b [u8], read_buffer: &'b mut [u8]) -> Command<'b> {
    Command {
      retry_strategy: self.retry_strategy.clone(),
      address: self.base_address.clone(),
      tx_buffer: NoneOneOrManyBuffer::One(write_buffer),
      rx_buffer: NoneOneOrManyMutBuffer::One(read_buffer)
    }
  }

  pub fn wmtr_cmd<'b>(&self, write_buffers: &'b[&'b[u8]], read_buffer: &'b mut [u8]) -> Command<'b> {
    Command {
      retry_strategy: self.retry_strategy.clone(),
      address: self.base_address.clone(),
      tx_buffer: NoneOneOrManyBuffer::Many(write_buffers),
      rx_buffer: NoneOneOrManyMutBuffer::One(read_buffer)
    }
  }
}

impl<'omb,T> NoneOneOrManyBuffer<'omb,T>
where
  T: Clone
{
  #[optimize(speed)]
  fn len(&self) -> usize {
    match self {
      Self::None => 0,
      Self::One(buf) => buf.len(),
      Self::Many(bufs) => {
        let mut len = 0usize;
        for slice in *bufs {
          len += slice.len();
        }
        len
      }
    }
  }
  #[optimize(speed)]
  fn read_at(&self, mut index: usize) -> OxideResult<T,IoError>
  where
    T: Default
  {
    if index >= self.len() {
      Err(IoError::EndOfFile)
    } else {
      Ok(match self {
        Self::None => panic!(), // Impossible
        Self::One(buf) => {
          buf[index].clone()
        },
        Self::Many(bufs) => {
          let mut val = T::default();
          for slice in *bufs {
            if index < slice.len() {
              val = slice[index].clone();
              break;
            } else {
              index -= slice.len()
            }
          }
          val
        }
      })
    }
  }
  #[optimize(speed)]
  fn is_none(&self) -> bool {
    match self {
      NoneOneOrManyBuffer::None => true,
      _ => false
    }
  }
}

impl<'omb,T> NoneOneOrManyMutBuffer<'omb,T>
where
  T: Clone
{
  #[optimize(speed)]
  fn len(&self) -> usize {
    match self {
      Self::None => 0,
      Self::One(buf) => buf.len(),
      Self::Many(bufs) => {
        let mut len = 0usize;
        for slice in *bufs {
          len += slice.len();
        }
        len
      }
    }
  }
  #[optimize(speed)]
  fn write_at(&mut self, index: usize, value: T) -> OxideResult<(),IoError> {
    if index >= self.len() {
      Err(IoError::NoFreeSpace)
    } else {
      match self {
        Self::None => {}, // Impossible
        Self::One(buf) => {
          buf[index] = value;
        },
        Self::Many(_bufs) => {
          unimplemented!()
        }
      };
      Ok(())
    }
  }
  #[optimize(speed)]
  fn is_none(&self) -> bool {
    match self {
      NoneOneOrManyMutBuffer::None => true,
      _ => false
    }
  }
}

impl<'c> TwiTransaction for Command<'c> {
  fn get_retry_strategy(&self, _isotoken: Isolated) -> RetryStrategy {
    self.retry_strategy.clone()
  }

  fn get_initial_addr_bin(&self, _isotoken: Isolated) -> u8 {
    if self.tx_buffer.is_none() {
      self.address.read_addr()
    } else {
      self.address.write_addr()
    }
  }

  fn get_read_addr_bin(&self, _isotoken: Isolated) -> Option<u8> {
    if self.rx_buffer.is_none() {
      None
    } else {
      Some(self.address.read_addr())
    }
  }

  fn get_tx_byte(&self, _isotoken: Isolated, index: usize) -> OxideResult<u8, IoError> {
    self.tx_buffer.read_at(index)
  }

  fn set_rx_byte(&mut self, _isotoken: Isolated, index: usize, byte: u8) -> OxideResult<usize, IoError> {
    self.rx_buffer.write_at(index, byte)?;
    Ok(self.rx_buffer.len() - index - 1)
  }
}

impl TwiAddr {
  pub const fn addr(addr: u8) -> Self {
    TwiAddr(addr & 0b11111110)
  }

  pub const fn addr_7bit(addr: u8) -> Self {
    TwiAddr(addr << 1)
  }

  pub fn write_addr(&self) -> u8 {
    self.0 & 0b11111110
  }

  pub fn read_addr(&self) -> u8 {
    self.0 | 0b00000001
  }
}

impl RetryStrategy {
  pub fn should_retry(&self, error: &TwiError) -> bool {
    match ( self, error ) {
      ( RetryStrategy::None, _ ) => false,
      ( _, TwiError::BufferOverflow ) => false, // Never retry buffer overflows
      ( RetryStrategy::UntilComplete, _ ) => true,
      ( RetryStrategy::UntilAcknowledged, TwiError::NotAcknowledged ) => true,
      _ => false
    }
  }
}


impl From<u8> for TwiAddr {
  fn from(val: u8) -> Self {
    TwiAddr(val & 0b11111110)
  }
}


pub mod base {
  use avr_oxide::hal::generic::twi::TwiError;

  pub(crate) enum TwiState {
    // We have no command to process
    Idle,

    // We need to start sending a command
    Ready,

    // We are waiting for the address ack
    WaitingAddrAck,

    // We are in the writing phase of a command
    Writing(usize),

    // We are in the reading phase of a command
    Reading(usize),

    // We have completed a transaction (read or write)
    TransactionComplete(usize),

    // A transaction error has occurred (under certain circumstances
    // we may transition to retry, or else we may abandon)
    TransactionError(TwiError),

    // Request a bus reset
    Reset,

    // Bus state is unknown
    Unknown
  }

  /// Implementation of TWI devices in megaAVR 0-Series devices like the
  /// ATmega4809
  #[allow(dead_code)]
  pub mod zeroseries {
    use core::cell::Cell;
    use core::marker::PhantomData;
    use avr_oxide::concurrency::Isolated;
    use avr_oxide::deviceconsts::clock::{MASTER_CLOCK_HZ, MASTER_CLOCK_PRESCALER};
    use avr_oxide::hal::generic::MuxControl;
    use avr_oxide::hal::generic::twi::{Command, InterfaceMode, PortSpeed, TwiCommandCompleteCallback, TwiError, TwiIdentity, TwiTransaction, TwoWireMaster};
    use avr_oxide::hal::generic::twi::base::TwiState;
    use avr_oxide::{isr_cb_invoke, OxideResult, panic_if_none, sleepctrl};
    use avr_oxide::hal::generic::cpu::private::PermitStandbyToken;
    use avr_oxide::hal::generic::cpu::SleepControl;
    use avr_oxide::util::datatypes::{BitFieldAccess, BitIndex, BitRange, Volatile, VolatileBitField};
    use avr_oxide::OxideResult::{Ok,Err};

    /// The AVR TWI control register block in 0-series devices
    #[repr(C)]
    pub struct TwoWireInterfaceControlBlock {
      pub(crate) ctrla: VolatileBitField,
      pub(crate) dualctrl: VolatileBitField,
      pub(crate) dbgctrl: VolatileBitField,
      pub(crate) mctrla: VolatileBitField,
      pub(crate) mctrlb: VolatileBitField,
      pub(crate) mstatus: VolatileBitField,
      pub(crate) mbaud: Volatile<u8>,
      pub(crate) maddr: Volatile<u8>,
      pub(crate) mdata: Volatile<u8>,
      pub(crate) sctrla: VolatileBitField,
      pub(crate) sctrlb: VolatileBitField,
      pub(crate) sstatus: VolatileBitField,
      pub(crate) saddr: Volatile<u8>,
      pub(crate) sdata: Volatile<u8>,
      pub(crate) saddrmask: VolatileBitField
    }

    pub(crate) const SDASETUP : BitIndex = BitIndex::bit_c(4);
    pub(crate) const FMPEN    : BitIndex = BitIndex::bit_c(1);
    pub(crate) const ENABLE   : BitIndex = BitIndex::bit_c(0);
    pub(crate) const ADDREN   : BitIndex = BitIndex::bit_c(0);
    pub(crate) const SDAHOLD  : BitRange = BitRange::range_c(2,3);
    pub(crate) const TIMEOUT  : BitRange = BitRange::range_c(2,3);
    pub(crate) const SMEN     : BitIndex = BitIndex::bit_c(1);
    pub(crate) const PMEN     : BitIndex = BitIndex::bit_c(2);
    pub(crate) const QCEN     : BitIndex = BitIndex::bit_c(4);
    pub(crate) const WIEN     : BitIndex = BitIndex::bit_c(6);
    pub(crate) const RIEN     : BitIndex = BitIndex::bit_c(7);
    pub(crate) const PIEN     : BitIndex = BitIndex::bit_c(5);
    pub(crate) const APIEN    : BitIndex = BitIndex::bit_c(6);
    pub(crate) const DIEN     : BitIndex = BitIndex::bit_c(7);
    pub(crate) const WIF      : BitIndex = BitIndex::bit_c(6);
    pub(crate) const RIF      : BitIndex = BitIndex::bit_c(7);
    pub(crate) const APIF     : BitIndex = BitIndex::bit_c(6);
    pub(crate) const DIF      : BitIndex = BitIndex::bit_c(7);
    pub(crate) const MCMD     : BitRange = BitRange::range_c(0,1);
    pub(crate) const ACKACT   : BitIndex = BitIndex::bit_c(2);
    pub(crate) const FLUSH    : BitIndex = BitIndex::bit_c(3);
    pub(crate) const BUSSTATE : BitRange = BitRange::range_c(0,1);
    pub(crate) const BUSERR   : BitIndex = BitIndex::bit_c(2);
    pub(crate) const ARBLOST  : BitIndex = BitIndex::bit_c(3);
    pub(crate) const RXACK    : BitIndex = BitIndex::bit_c(4);
    pub(crate) const CLKHOLD  : BitIndex = BitIndex::bit_c(5);

    pub(crate) const MCMD_NOACT     : u8 = 0x00;
    pub(crate) const MCMD_REPSTART  : u8 = 0x01;
    pub(crate) const MCMD_RECVTRANS : u8 = 0x02;
    pub(crate) const MCMD_SENDTRANS : u8 = 0x02;
    pub(crate) const MCMD_STOP      : u8 = 0x03;

    impl TwoWireInterfaceControlBlock {
      #[cfg(target_arch="avr")]
      pub(crate) unsafe fn instance_from_addr(addr: u16) -> &'static mut Self {
        core::mem::transmute(addr)
      }
      #[cfg(not(target_arch="avr"))]
      pub(crate) unsafe fn instance_from_addr(addr: u16) -> &'static mut Self {
        static mut INSTANCE: TwoWireInterfaceControlBlock = TwoWireInterfaceControlBlock {
          ctrla: VolatileBitField::all_clr(),
          dualctrl: VolatileBitField::all_clr(),
          dbgctrl: VolatileBitField::all_clr(),
          mctrla: VolatileBitField::all_clr(),
          mctrlb: VolatileBitField::all_clr(),
          mstatus: VolatileBitField::all_clr(),
          mbaud: Volatile::<u8>::zero(),
          maddr: Volatile::<u8>::zero(),
          mdata: Volatile::<u8>::zero(),
          sctrla: VolatileBitField::all_clr(),
          sctrlb: VolatileBitField::all_clr(),
          sstatus: VolatileBitField::all_clr(),
          saddr: Volatile::<u8>::zero(),
          sdata: Volatile::<u8>::zero(),
          saddrmask: VolatileBitField::all_clr(),
        };
        &mut INSTANCE
      }
    }

    impl PortSpeed {
      /// Convert a port speed into the MBAUD value for the TWI
      const fn to_master_baud(&self) -> u8 {
        // The basic formula is
        //  MBAUD = (fCLK_PER - 10*fSCL) / 2*fSCL.
        //
        // Where fCLK_PER is the peripheral clock speed, and
        // fSCL is the desired SCL clock frequency
        let f_clk_per = MASTER_CLOCK_HZ / MASTER_CLOCK_PRESCALER as u32;

        (match self {
          PortSpeed::Standard => {
            (f_clk_per - ( 100_000u32 * 10)) / (100_000u32 * 2)
          },
          PortSpeed::Fast => {
            (f_clk_per - ( 400_000u32 * 10)) / (400_000u32 * 2)
          },
          PortSpeed::FastPlus => {
            (f_clk_per - ( 1_000_000u32 * 10)) / (1_000_000u32 * 2)
          }
        }) as u8
      }
    }

    pub struct AtmelTwi<M>
    where
      M: MuxControl
    {
      pub(crate) control: &'static mut TwoWireInterfaceControlBlock,
      pub(crate) command_complete_handler: Cell<TwiCommandCompleteCallback>,
      pub(crate) phantom: PhantomData<M>,

      pub(crate) master_src_id: TwiIdentity,
      pub(crate) slave_src_id: TwiIdentity,

      pub(crate) command: Option<Command<'static>>,
      pub(crate) state: TwiState,

      pub(crate) sleep_inhibitor: Option<PermitStandbyToken>
    }

    impl<M> AtmelTwi<M>
    where
      M: MuxControl
    {
      pub fn mux(&mut self, mux: M) -> &mut Self {
        mux.route();
        self
      }

      pub fn mode(&mut self, mode: InterfaceMode, speed: PortSpeed) -> &mut Self {
        avr_oxide::concurrency::interrupt::isolated(|isotoken|{

          // Set interface-specific options
          match mode {
            InterfaceMode::I2C => {
              self.control.ctrla.set_to_isolated(isotoken, SDAHOLD, 0x01);
              self.control.mctrla.set_to_isolated(isotoken, TIMEOUT, 0x00);
            },
            InterfaceMode::SMBus => {
              self.control.ctrla.set_to_isolated(isotoken, SDAHOLD, 0x03);
              self.control.mctrla.set_to_isolated(isotoken, TIMEOUT, 0x01);
            }
          }

          // Set the baud rate register
          match speed {
            PortSpeed::FastPlus => {
              self.control.ctrla.set_isolated(isotoken, FMPEN);
              self.control.mbaud.write(speed.to_master_baud());
            },
            _ => {
              self.control.ctrla.clr_isolated(isotoken, FMPEN);
              self.control.mbaud.write(speed.to_master_baud());
            }
          }

          // Enable master
          self.control.mctrla.set_isolated(isotoken, ENABLE);
          // Disable 'smart mode' (hardware generated ACKs)
          self.control.mctrla.clr_isolated(isotoken, SMEN);

          // Request a reset and then let the state machine do its bit
          self.state = TwiState::Reset;
          self.master_process(isotoken);
        });

        self
      }
    }

    impl<M> TwoWireMaster for AtmelTwi<M>
    where
      M: MuxControl
    {

      fn command(&mut self, mut command: Command<'static>) {
        loop {
          match self.try_command(command) {
            Ok(()) => {
              return;
            },
            Err((TwiError::DeviceBusy, cmd)) => {
              command = cmd;
            },
            _ => {
              avr_oxide::oserror::halt(avr_oxide::oserror::OsError::InternalError);
            }
          }
        }
      }

      fn try_command(&mut self, command: Command<'static>) -> OxideResult<(), (TwiError, Command<'static>)> {
        avr_oxide::concurrency::interrupt::isolated(|isotoken|{
          match self.state {
            TwiState::Idle => {
              self.command = Some(command);
              self.state = TwiState::Ready;
              self.master_process(isotoken);
              Ok(())
            }
            _ => {
              Err((TwiError::DeviceBusy,command))
            }
          }
        })
      }

      fn set_command_complete_callback(&self, callback: TwiCommandCompleteCallback) {
        avr_oxide::concurrency::interrupt::isolated(|_isotoken|{
          self.command_complete_handler.replace(callback);
        });
      }
    }

    impl<M> AtmelTwi<M>
    where
      M: MuxControl
    {
      pub(crate) fn invoke_command_complete_callback(&mut self, isotoken: Isolated, result: OxideResult<(Command<'static>, usize),TwiError>) {
        isr_cb_invoke!(isotoken, self.command_complete_handler.get(), self.master_src_id, result)
      }

      pub(crate) fn master_process(&mut self, isotoken: Isolated) {
        const ERROR_FLAGS : u8 = ARBLOST.binary() | BUSERR.binary();
        const ERROR_AND_NOACK_FLAGS : u8 = ARBLOST.binary() | BUSERR.binary() | RXACK.binary();

        loop {
          let mut finished = false;
          let mut enable_ints = true;
          let mstatus = self.control.mstatus.snapshot();

          let next_state/*, finished, enableints) */ = match self.state {

            // Ready:  So, put the address on the bus and wait for ack
            TwiState::Ready => {
              if self.sleep_inhibitor.is_none() {
                self.sleep_inhibitor = Some(sleepctrl!(isotoken).inhibit_standby());
              }
              let addr = panic_if_none!(&self.command).get_initial_addr_bin(isotoken);

              self.control.maddr.write(addr);

              finished = true;

              TwiState::WaitingAddrAck
            },

            // WaitAddrAck: If successfully acknowledged, we can start
            // writing, if there is an error we can signal that
            TwiState::WaitingAddrAck => {
              if mstatus.is_set(WIF) {
                if (mstatus & ERROR_AND_NOACK_FLAGS) > 0 {
                  enable_ints = false;
                  TwiState::TransactionError(TwiError::NotAcknowledged)
                } else {
                  TwiState::Writing(0)
                }
              } else if mstatus.is_set(RIF) {
                if (mstatus & ERROR_FLAGS) > 0 {
                  enable_ints = false;
                  TwiState::TransactionError(TwiError::BusError)
                } else {
                  TwiState::Reading(0)
                }
              } else {
                // Anything else, keep on waiting
                finished = true;
                TwiState::WaitingAddrAck
              }
            },

            // Writing
            TwiState::Writing(index) => {
              if (mstatus & ERROR_FLAGS) > 0 {
                enable_ints = false;
                TwiState::TransactionError(TwiError::BusError)
              } else {
                if mstatus.is_set(WIF) && mstatus.is_clr(RXACK) {
                  // Slave is ready to receive more
                  match panic_if_none!(&self.command).get_tx_byte(isotoken, index) {
                    Ok(byte) => {
                      self.control.mctrlb.set_to(MCMD, MCMD_SENDTRANS);
                      self.control.mdata.write(byte);
                      finished = true;
                      TwiState::Writing(index+1)
                    },
                    Err(_) => {
                      // We may be finished, or we may switch to now read
                      // from the same address
                      match panic_if_none!(&self.command).get_read_addr_bin(isotoken) {
                        Some(addr) => {
                          self.control.maddr.write(addr);
                          self.control.mctrlb.set_to(MCMD, MCMD_REPSTART);
                          finished = true;
                          TwiState::WaitingAddrAck
                        },
                        None => {
                          self.control.mctrlb.set_to(MCMD, MCMD_STOP);
                          enable_ints = false;
                          TwiState::TransactionComplete(index+1)
                        }
                      }
                    }
                  }
                } else {
                  // Annything else, just carry on without change
                  finished = true;
                  TwiState::Writing(index)
                }
              }
            },

            // Reading
            TwiState::Reading(index) => {
              if (mstatus & ERROR_FLAGS) > 0 {
                enable_ints = false;
                TwiState::TransactionError(TwiError::BusError)
              } else {
                if mstatus.is_set(RIF) {
                  // There is a byte in the input
                  match panic_if_none!(&mut self.command).set_rx_byte(isotoken, index, self.control.mdata.read()) {
                    Ok(bytes_left) => {

                      // If we have space to receive more, we can send
                      // a RECVTRANS (read more), otherwise a STOP
                      if bytes_left > 0 {
                        self.control.mctrlb.clr(ACKACT);
                        self.control.mctrlb.set_to(MCMD, MCMD_RECVTRANS);
                        finished = true;
                        TwiState::Reading(index+1)
                      } else {
                        // Counter-intuitive, but we should NACK the last byte
                        // read (many peripherals will be stuck thinking they
                        // should keep sending data and will screw up,
                        // despite the STOP condition)
                        self.control.mctrlb.set(ACKACT);
                        self.control.mctrlb.set_to(MCMD, MCMD_STOP);
                        enable_ints = false;
                        TwiState::TransactionComplete(index+1)
                      }
                    },
                    Err(_) => {
                      self.control.mctrlb.set(ACKACT);
                      self.control.mctrlb.set_to(MCMD, MCMD_STOP);
                      enable_ints = false;
                      TwiState::TransactionError(TwiError::BufferOverflow)
                    }
                  }
                } else {
                  // Anything else, just carry on
                  finished = true;
                  TwiState::Reading(index)
                }
              }
            },

            // There has been an error.  Maybe we retry, maybe we don't...
            TwiState::TransactionError(err) => {
              if panic_if_none!(&self.command).get_retry_strategy(isotoken).should_retry(&err) {
                // Go back to the Ready state (which will trigger re-trying
                // the transaction)
                enable_ints = false;
                TwiState::Ready
              } else {
                // Discard the command, we won't need it any more
                self.command.take();
                self.invoke_command_complete_callback(isotoken, Err(err));

                // Reset the bus after an error
                // @todo change this so we only reset after 'hard' errors
                enable_ints = false;
                TwiState::Reset
              }
            }

            // Complete a transaction (yay)
            TwiState::TransactionComplete(bytes) => {
              let command = panic_if_none!(self.command.take());
              self.invoke_command_complete_callback(isotoken, Ok((command,bytes)));
              enable_ints = false;
              TwiState::Idle
            },

            TwiState::Reset => {
              // Force the bus state to idle
              self.control.mstatus.set_to(BUSSTATE, 0x01);
              // Clear the master's internal state
              self.control.mctrlb.set(FLUSH);
              enable_ints = false;
              TwiState::Idle
            }

            TwiState::Idle => {
              match self.sleep_inhibitor.take() {
                None => {},
                Some(token) => {
                  sleepctrl!(isotoken).permit_standby(token);
                }
              }
              finished = true;
              enable_ints = false;
              TwiState::Idle
            }

            // Anything else, reset the bus
            _ => {
              enable_ints = false;
              TwiState::Reset
            }
          };

          // Update our internal state record
          self.state = next_state;
          if finished {
            if enable_ints {
              self.control.mctrla.set_isolated(isotoken, WIEN);
              self.control.mctrla.set_isolated(isotoken, RIEN);
            } else {
              self.control.mctrla.clr_isolated(isotoken, WIEN);
              self.control.mctrla.clr_isolated(isotoken, RIEN);
            }
            break;
          }
        }
      }
    }

    #[doc(hidden)]
    #[macro_export]
    macro_rules! atmel_twi_tpl {
      ($twiref:expr, $mastersrcref:expr, $slavesrcref:expr, $implementation: ty, $isr_master:expr, $isr_slave:expr) => {
        use core::cell::Cell;
        use core::marker::PhantomData;
        use avr_oxide::hal::generic::twi::base::TwiState;
        use avr_oxide::hal::generic::twi::base::zeroseries::AtmelTwi;
        use avr_oxide::hal::generic::twi::TwiCommandCompleteCallback;
        use avr_oxide::hal::generic::twi::base::zeroseries::TwoWireInterfaceControlBlock;

        use avr_oxide::mut_singleton_explicit_init;

        pub type TwiImpl = $implementation;

        mut_singleton_explicit_init!(
          $implementation,
          __INSTANCE,
          initialise, instance, instance_isolated,
          AtmelTwi {
            control:         TwoWireInterfaceControlBlock::instance_from_addr($twiref),
            command_complete_handler: Cell::new(TwiCommandCompleteCallback::Nop(())),
            phantom:         PhantomData::default(),
            master_src_id:   $mastersrcref,
            slave_src_id:    $slavesrcref,
            command:         None,
            state:           TwiState::Unknown,
            sleep_inhibitor: None
          });

        #[oxide_macros::interrupt(isr=$isr_master)]
        fn twi_master_interrupt(isotoken: avr_oxide::concurrency::Isolated) {
          let atmeltwi = instance_isolated(isotoken);
          atmeltwi.master_process(isotoken);
        }

        #[oxide_macros::interrupt(isr=$isr_slave)]
        fn twi_slave_interrupt(isotoken: avr_oxide::concurrency::Isolated) {
        }
      }
    }
  }
}
// Tests =====================================================================
#[cfg(test)]
mod tests {
  use avr_oxide::hal::generic::twi::{NoneOneOrManyBuffer, NoneOneOrManyMutBuffer};

  #[test]
  fn test_none_one_or_many() {
    let test_data1 = [ 0x01u8, 0x02, 0x03, 0x04 ];
    let test_data2 = [ 0x05u8, 0x06, 0x07 ];
    let test_data3 = [ 0x08u8, 0x09, 0x0a, 0x0b, 0x0c ];
    let combined_test_data = [ &test_data1[..], &test_data2[..], &test_data3[..] ];


    // None
    let buffer = NoneOneOrManyBuffer::<u8>::None;
    assert_eq!(buffer.len(), 0);
    assert!(buffer.read_at(0).is_err());

    // One
    let buffer = NoneOneOrManyBuffer::One(&test_data1);
    assert_eq!(buffer.len(), test_data1.len());
    println!("Read at 0 == {:?}", buffer.read_at(0));
    assert_eq!(buffer.read_at(0).unwrap(), 0x01u8);
    println!("Read at 2 == {:?}", buffer.read_at(2));
    assert_eq!(buffer.read_at(2).unwrap(), 0x03u8);
    assert!(buffer.read_at(4).is_err());

    // Many
    let buffer = NoneOneOrManyBuffer::Many(&combined_test_data);
    assert_eq!(buffer.len(), test_data1.len() + test_data2.len() + test_data3.len());
    for i in 0..buffer.len() {
      assert_eq!(buffer.read_at(i).unwrap(), (i+1) as u8);
    }
    assert!(buffer.read_at(buffer.len()).is_err());
  }

  #[test]
  fn test_none_one_or_many_mut() {
    let mut test_data = [ 0x00u8, 0x00, 0x00, 0x00 ];

    // None
    let mut buffer = NoneOneOrManyMutBuffer::<u8>::None;
    assert_eq!(buffer.len(), 0);
    assert!(buffer.write_at(0,0x01).is_err());

    // One
    let mut buffer = NoneOneOrManyMutBuffer::One(&mut test_data);
    buffer.write_at(0, 0xde).unwrap();
    buffer.write_at(1, 0xad).unwrap();
    buffer.write_at(2, 0xbe).unwrap();
    buffer.write_at(3, 0xef).unwrap();
    assert!(buffer.write_at(4,0x01).is_err());

    assert_eq!(test_data, [ 0xde, 0xad, 0xbe, 0xef ]);
  }

}