/* mod.rs
 *
 * Developed by Tim Walls <tim.walls@snowgoons.com>
 * Copyright (c) All Rights Reserved, Tim Walls
 */
//! SNaFuS - Simple Numbered File System
//!
//! A trivial filesystem-like implementation for AVR storage like EEPROMs.
//! Uses numbers rather than names to identify files (mostly to minimise
//! memory overhead in the client rather than on the storage - strings are
//! expensive!)

// Imports ===================================================================
use core::ops::DerefMut;
use avr_oxide::io::IoError;
use avr_oxide::OxideResult;

#[cfg(target_arch="avr")]
use avr_oxide::sync::{Mutex,MutexGuard};
#[cfg(not(target_arch="avr"))]
use std::sync::{Mutex,MutexGuard};

use avrox_storage::{RandomWrite, RandomRead, SAddr, SSize, FileAddr};
use avrox_storage::fs::filesystem::{BlockNumber, BLOCKSIZE, FileSystemRead, FileSystemResult, FileSystemWrite};
use avrox_storage::fs::{File, OpenOptions};
use avrox_storage::random::{AccessHint, Storage};
use avr_oxide::OxideResult::{Err,Ok};


// Declarations ==============================================================
pub const MAXFILES  : usize = 32;
pub(crate) const BLOCKSIZE_USABLE : usize = BLOCKSIZE - BlockHeader::SIZE;

const MAGIC    : [u8; 2] = [ 0x05, 0xf5 ];
const VERSION  : u8      = 1;
const CANARY   : u16     = 0xBE;

/// A number identifying a particular file
#[cfg_attr(not(target_arch="avr"), derive(Debug))]
#[cfg_attr(target_arch="avr", derive(ufmt::derive::uDebug))]
#[derive(Copy,Clone,PartialEq,Eq)]
pub struct FileUid(u8);

pub struct SnafusFileSystem<SD>
where
  SD: Storage
{
  storage_driver: Mutex<SD>,
}

unsafe impl<SD> Sync for SnafusFileSystem<SD>
where
  SD: Storage
{}

#[cfg_attr(not(target_arch="avr"), derive(Debug))]
#[cfg_attr(target_arch="avr", derive(ufmt::derive::uDebug))]
#[repr(C)]
struct DirectoryEntry {
  first_block: BlockNumber,
  last_block:  BlockNumber,
  block_count: BlockNumber
}

#[cfg_attr(not(target_arch="avr"), derive(Debug))]
#[cfg_attr(target_arch="avr", derive(ufmt::derive::uDebug))]
#[repr(C)]
struct FsHeader {
  magic:              [u8; 2],
  endian_canary:      u16,
  version:            u8,
  usable_blocks:      BlockNumber,
  first_block:        BlockNumber
}

#[cfg_attr(not(target_arch="avr"), derive(Debug))]
#[cfg_attr(target_arch="avr", derive(ufmt::derive::uDebug))]
#[repr(C)]
struct BlockHeader {
  prev_block: BlockNumber,
  next_block: BlockNumber,
  checksum:   u8,
  bytes_used: u8
}


// Code ======================================================================
impl From<u8> for FileUid {
  fn from(val: u8) -> Self {
    if val >= (MAXFILES as u8) {
      avr_oxide::oserror::halt(avr_oxide::oserror::OsError::BadParams);
    }

    FileUid(val)
  }
}

impl FileUid {
  fn offset_addr(&self) -> SAddr {
    DirectoryEntry::SIZE as SAddr * self.0 as SAddr
  }
  /// Create a new File UID with the given ID
  pub fn with_id(val: u8) -> Self {
    FileUid::from(val)
  }
  /// Create a new File UID with the given ID
  pub const fn with_id_c(val: u8) -> Self {
    assert!((val as usize) < MAXFILES);
    FileUid(val)
  }

  const fn first() -> Self {
    FileUid(0)
  }

  fn next_id(&self) -> Option<Self> {
    if self.0+1 < MAXFILES as u8 {
      Some(FileUid(self.0+1))
    } else {
      None
    }
  }
}



impl DirectoryEntry {
  const SIZE: usize = 6;

  fn from_bytes(buf: &[u8]) -> DirectoryEntry {
    DirectoryEntry {
      first_block: BlockNumber::from_bytes(&buf[0..=1]),
      last_block:  BlockNumber::from_bytes(&buf[2..=3]),
      block_count: BlockNumber::from_bytes(&buf[4..=5]),
    }
  }

  fn as_bytes(&self) -> [u8; Self::SIZE] {
    [
      self.first_block.as_bytes()[0],
      self.first_block.as_bytes()[1],
      self.last_block.as_bytes()[0],
      self.last_block.as_bytes()[1],
      self.block_count.as_bytes()[0],
      self.block_count.as_bytes()[1],
    ]
  }

  fn empty() -> DirectoryEntry {
    DirectoryEntry {
      first_block: BlockNumber::INVALID,
      last_block: BlockNumber::INVALID,
      block_count: BlockNumber::ZERO,
    }
  }

  fn first_block(&self) -> OxideResult<BlockNumber,IoError> {
    if self.first_block.is_valid() {
      Ok(self.first_block)
    } else {
      Err(IoError::OutOfRange)
    }
  }
}



impl FsHeader {
  const SIZE: usize = 9;

  fn as_bytes(&self) -> [u8; Self::SIZE] {
    [
      MAGIC[0],
      MAGIC[1],
      self.version,
      (self.endian_canary >> 8) as u8,
      self.endian_canary as u8,
      self.usable_blocks.as_bytes()[0],
      self.usable_blocks.as_bytes()[1],
      self.first_block.as_bytes()[0],
      self.first_block.as_bytes()[1],
    ]
  }

  fn from_bytes(buf: &[u8]) -> FsHeader {
    FsHeader {
      magic: [ buf[0], buf[1] ],
      version: buf[2],
      endian_canary: (((buf[3] as u16) << 8) | buf[4] as u16),
      usable_blocks: BlockNumber::from_bytes(&buf[5..=6]),
      first_block: BlockNumber::from_bytes(&buf[7..=8])
    }
  }
}

impl BlockHeader {
  const SIZE: usize = 6;

  fn as_bytes(&self) -> [u8; Self::SIZE] {
    [
      self.prev_block.as_bytes()[0],
      self.prev_block.as_bytes()[1],
      self.next_block.as_bytes()[0],
      self.next_block.as_bytes()[1],
      self.checksum,
      self.bytes_used
    ]
  }

  fn from_bytes(buf: &[u8]) -> BlockHeader {
    BlockHeader {
      prev_block: BlockNumber::from_bytes(&buf[0..=1]),
      next_block: BlockNumber::from_bytes(&buf[2..=3]),
      checksum: buf[4],
      bytes_used: buf[5]
    }
  }

  fn next_block(&self) -> OxideResult<BlockNumber,IoError> {
    if self.next_block.is_valid() {
      Ok(self.next_block)
    } else {
      Err(IoError::OutOfRange)
    }
  }

  fn bytes_used(&self) -> usize {
    self.bytes_used as usize
  }
}

impl<SD> SnafusFileSystem<SD>
where
  SD: Storage
{
  const ADDRESSABLE_BYTES  : SSize = SD::ADDRESSABLE_BYTES;
  const TOTAL_BLOCKS       : SSize = Self::ADDRESSABLE_BYTES / BLOCKSIZE as SSize;
  const HEADER_SIZE        : SSize = FsHeader::SIZE as SSize;
  const BITMAP_SIZE        : SSize = ((Self::TOTAL_BLOCKS / 8) + 1) as SSize;
  const DIRECTORY_SIZE     : SSize = DirectoryEntry::SIZE as SSize * MAXFILES as SSize;
  const FIRST_USABLE_BLOCK : u32   = ((Self::HEADER_SIZE + Self::BITMAP_SIZE + Self::DIRECTORY_SIZE) / (BLOCKSIZE as SSize)) + 1;
  const USABLE_BLOCKS      : u32   = (Self::TOTAL_BLOCKS as u32) - Self::FIRST_USABLE_BLOCK;

  const HEADER_START       : SAddr = 0x0000 as SAddr;
  const DIRECTORY_START    : SAddr = Self::HEADER_SIZE as SAddr;
  const BITMAP_START       : SAddr = (Self::HEADER_SIZE + Self::DIRECTORY_SIZE) as SAddr;

  pub fn with_driver(driver: SD) -> Self {
    SnafusFileSystem {
      storage_driver: Mutex::new(driver)
    }
  }

  #[cfg(target_arch="avr")]
  fn lock_driver(&self) -> MutexGuard<SD> {
    self.storage_driver.lock()
  }

  #[cfg(not(target_arch="avr"))]
  fn lock_driver(&self) -> MutexGuard<SD> {
    self.storage_driver.lock().unwrap()
  }



  fn get_directory_entry_using(&self, driver: &SD, file: FileUid) -> OxideResult<DirectoryEntry,IoError>
    where
      SD: RandomRead
  {
    if (file.0 as usize) < MAXFILES {
      let mut buffer = [0x00; DirectoryEntry::SIZE];

      driver.read_full_at_hinted(Self::DIRECTORY_START + file.offset_addr(), &mut buffer, AccessHint::NONSEQUENTIAL)?;

      let entry = DirectoryEntry::from_bytes(&buffer);

      if entry.first_block.is_valid() {
        Ok(entry)
      } else {
        Err(IoError::NotFound)
      }
    } else {
      Err(IoError::NotFound)
    }
  }

  fn get_directory_entry(&self, file: FileUid) -> OxideResult<DirectoryEntry,IoError>
  where
    SD: RandomRead
  {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    self.get_directory_entry_using(driver, file)
  }


  /// Extend a file by allocating a new block at the end.  The directory
  /// entry IS updated, the old last block's next pointer IS NOT updated,
  /// but the new block number is returned to allow the caller to do it.
  fn extend_file(&self, driver: &mut SD, file: FileUid) -> OxideResult<BlockNumber,IoError>
    where
      SD: RandomRead + RandomWrite
  {
    if (file.0 as usize) < MAXFILES {
      let mut buffer = [0x00; DirectoryEntry::SIZE];

      driver.read_full_at_hinted(Self::DIRECTORY_START + file.offset_addr(), &mut buffer, AccessHint::NONSEQUENTIAL)?;

      let mut directory_entry = DirectoryEntry::from_bytes(&buffer);

      let new_block = self.first_free_block(&driver)?;

      let new_block_header = BlockHeader {
        prev_block: directory_entry.last_block,
        next_block: BlockNumber::INVALID,
        checksum: 0,
        bytes_used: 0
      };

      // Special case if this is the first block in the file
      if directory_entry.first_block.is_invalid() {
        directory_entry.first_block = new_block;
      }
      directory_entry.last_block = new_block;
      directory_entry.block_count += 1;

      self.mark_block_used(driver, new_block)?;
      driver.write_all_at(new_block.offset_addr(), &new_block_header.as_bytes())?;
      driver.write_all_at_hinted(Self::DIRECTORY_START + file.offset_addr(), &directory_entry.as_bytes(),
                                 AccessHint::NONSEQUENTIAL)?;
      Ok(new_block)
    } else {
      Err(IoError::OutOfRange)
    }
  }


  /// Load the header for the given block
  fn get_block_header(&self, block: BlockNumber) -> OxideResult<BlockHeader,IoError>
  where
    SD: RandomRead
  {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    self.get_block_header_using(driver, block)
  }

  fn get_block_header_using(&self, driver: &mut SD, block: BlockNumber) -> OxideResult<BlockHeader,IoError>
    where
      SD: RandomRead
  {
    let mut buffer = [0x00; BlockHeader::SIZE];
    driver.read_full_at_hinted(block.offset_addr(), &mut buffer, AccessHint::NONSEQUENTIAL)?;

    Ok(BlockHeader::from_bytes(&buffer))
  }


  /// Find the first free block in the filesystem
  fn first_free_block(&self, driver: &SD) -> OxideResult<BlockNumber,IoError>
  where
    SD: RandomRead
  {
    let mut buffer = [0x00u8];

    for i in 0..Self::BITMAP_SIZE {
      driver.read_at(Self::BITMAP_START + i, &mut buffer)?;

      if buffer[0] != 0xff { // Yay, one of these is free
        let mut mask = 0b0000_0001u8;
        let mut block = i * 8;

        while (buffer[0] & mask) != 0x00 {
          mask <<= 1;
          block += 1;
        }

        if block < Self::TOTAL_BLOCKS {
          return Ok(block.into());
        } else {
          return Err(IoError::NoFreeSpace);
        }
      }
    }

    Err(IoError::NoFreeSpace)
  }

  /// Mark a block as used (or clear the mark) in the bitmap
  fn mark_block_used_or_free(&self, driver: &mut SD, block: BlockNumber, used: bool) -> OxideResult<(),IoError>
  where
    SD: RandomRead + RandomWrite
  {
    let bitmap_byte_addr = (block.value() / 8) as SAddr;
    let mut bitmap_byte = [0x00u8];


    driver.read_at_hinted(Self::BITMAP_START + bitmap_byte_addr,
                          &mut bitmap_byte,
                          AccessHint::NONSEQUENTIAL)?;

    if used {
      bitmap_byte[0] |= 1<<(block.value() % 8);
    } else {
      bitmap_byte[0] &= !(1<<(block.value() % 8));
    }

    driver.write_at_hinted(Self::BITMAP_START + bitmap_byte_addr,
                           &bitmap_byte,
                           AccessHint::NONSEQUENTIAL)?;

    Ok(())
  }


  /// Mark a block as used in the bitmap
  fn mark_block_used(&self, driver: &mut SD, block: BlockNumber) -> OxideResult<(),IoError>
  where
    SD: RandomRead + RandomWrite
  {
    self.mark_block_used_or_free(driver, block, true)
  }

  /// Mark a block as free in the bitmap
  fn mark_block_free(&self, driver: &mut SD, block: BlockNumber) -> OxideResult<(),IoError>
  where
    SD: RandomRead + RandomWrite
  {
    self.mark_block_used_or_free(driver, block, false)
  }

  /// True iff the filesystem is formatted and ready for use.
  pub fn is_formatted(&self) -> FileSystemResult<bool>
  where
    SD: RandomRead
  {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    let mut buffer = [0x00u8; FsHeader::SIZE];
    driver.read_full_at_hinted(Self::HEADER_START, &mut buffer, AccessHint::NONSEQUENTIAL)?;

    let header = FsHeader::from_bytes(&buffer);
    if header.magic == MAGIC && header.version == VERSION {
      if header.endian_canary != CANARY {
        Err(IoError::EndianMismatch)
      } else {
        Ok(true)
      }
    } else {
      Ok(false)
    }
  }

  /// Format the filesystem.  All data will be lost.
  pub fn format(&self) -> OxideResult<(),IoError>
  where
    SD: RandomWrite + RandomRead
  {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    #[cfg(test)]
    println!("Formatting SNaFuS: {} bytes avail, {} usable blocks starting at {}",
      Self::ADDRESSABLE_BYTES, Self::USABLE_BLOCKS, Self::FIRST_USABLE_BLOCK);

    // Prepare the header
    let header = FsHeader {
      magic: MAGIC,
      version: VERSION,
      endian_canary: CANARY,
      usable_blocks: Self::USABLE_BLOCKS.into(),
      first_block:   Self::FIRST_USABLE_BLOCK.into()
    };

    // First off we're going to erase the magic number, so an incompletely
    // formatted driver won't be readable
    driver.write_all_at_hinted(Self::HEADER_START, &[0xde,0xad], AccessHint::WRITEONLY)?;

    // And the directory table
    for i in 0..MAXFILES {
      driver.write_all_at_hinted(Self::DIRECTORY_START + ((i * DirectoryEntry::SIZE) as SAddr),
                          &DirectoryEntry::empty().as_bytes(),
                                 AccessHint::WRITEONLY)?;
    }

    // Now write the block bitmap
    for i in 0..Self::BITMAP_SIZE {
      driver.write_at_hinted(Self::BITMAP_START + i, &[0x00],
                             AccessHint::WRITEONLY)?;
    }

    // Mark the unusable blocks as used in the bitmap (means we don't need
    // any special-cases in the free-block-search method)
    for i  in 0..Self::FIRST_USABLE_BLOCK {
      self.mark_block_used(driver, i.into())?;
    }

    // Now write the header for every usable block
    let block_header = BlockHeader {
      prev_block: 0.into(),
      next_block: 0.into(),
      checksum: 0,
      bytes_used: 0
    };
    for block in 0..Self::TOTAL_BLOCKS {
      let block_addr = (block * BLOCKSIZE as SSize) as SAddr;

      driver.write_all_at_hinted(block_addr, &block_header.as_bytes(),
                          AccessHint::WRITEONLY)?;
    }

    // OK, finally write out the header
    driver.write_all_at_hinted(Self::HEADER_START, &header.as_bytes(),
                               AccessHint::WRITEONLY)?;

    driver.flush()?;

    Ok(())
  }

  fn truncate_at_using(&self, driver: &mut SD, file: FileUid, new_last_block: BlockNumber, offset: usize) -> FileSystemResult<()>
  where
    SD: RandomRead + RandomWrite
  {
    if new_last_block.is_valid() {
      let mut new_last_block_hdr = self.get_block_header_using(driver, new_last_block)?;
      let mut dir_entry = self.get_directory_entry_using(driver, file)?;

      if offset <= new_last_block_hdr.bytes_used as usize {
        let mut block_to_delete = new_last_block_hdr.next_block;
        let mut blocks_deleted = 0u16;

        // Walk up the block chain marking free
        while block_to_delete.is_valid() {
          let deleted_block_hdr = self.get_block_header_using(driver,block_to_delete)?;
          self.mark_block_free(driver, block_to_delete)?;
          block_to_delete = deleted_block_hdr.next_block;
          blocks_deleted += 1;
        }

        // Update the header of our new final block
        new_last_block_hdr.bytes_used = offset as u8;
        new_last_block_hdr.next_block = BlockNumber::INVALID;
        driver.write_all_at_hinted(new_last_block.offset_addr(), &new_last_block_hdr.as_bytes(),
                                   AccessHint::NONSEQUENTIAL)?;

        // Update the directory entry
        dir_entry.last_block = new_last_block;
        dir_entry.block_count -= blocks_deleted;
        driver.write_all_at_hinted(Self::DIRECTORY_START + file.offset_addr(), &dir_entry.as_bytes(),
                                   AccessHint::NONSEQUENTIAL)?;

        Ok(())
      } else {
        Err(IoError::OutOfRange)
      }
    } else {
      Err(IoError::OutOfRange)
    }
  }


  #[cfg(test)]
  fn get_16b_at(&mut self, addr: SAddr) -> [u8; 16]
  where SD: RandomRead
  {
    let mut buffer = [0x00u8; 16];

    let mut driver_lock = self.storage_driver.lock().unwrap();
    let driver = driver_lock.deref_mut();

    driver.read_full_at(addr, &mut buffer);

    buffer
  }

  #[cfg(test)]
  fn get_block_map_16b(&mut self) -> [u8; 16]
  where SD: RandomRead {
    self.get_16b_at(Self::BITMAP_START)
  }

  #[cfg(test)]
  fn get_header_16b(&mut self) -> [u8; 16]
    where SD: RandomRead {
    self.get_16b_at(Self::HEADER_START)
  }

  #[cfg(test)]
  fn get_directory_16b(&mut self) -> [u8; 16]
    where SD: RandomRead {
    self.get_16b_at(Self::DIRECTORY_START)
  }

  #[cfg(test)]
  fn get_block_16b(&mut self, block: BlockNumber) -> [u8; 16]
    where SD: RandomRead {
    self.get_16b_at((BLOCKSIZE * block.value() as usize) as SAddr)
  }

  #[cfg(test)]
  fn get_first_free_block(&mut self) -> OxideResult<BlockNumber,IoError>
  where
    SD: RandomRead
  {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    self.first_free_block(&driver)
  }
}

impl<SD> FileSystemRead for SnafusFileSystem<SD>
  where
    SD: Storage + RandomRead
{
  type FID = FileUid;
  const BLOCK_DATA_SIZE: usize = BLOCKSIZE_USABLE;

  fn read_from_location(&self, _file: Self::FID, block: BlockNumber, offset: usize, buf: &mut [u8]) -> FileSystemResult<(usize, (BlockNumber, usize))> {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    let block_header = self.get_block_header_using(driver, block)?;

    if offset > block_header.bytes_used() {
      return Err(IoError::OutOfRange);
    }

    let max_readable  = block_header.bytes_used() - offset;
    let ( bytes_to_read, end_of_block ) = if max_readable > buf.len() {
      (buf.len(),false)
    } else {
      (max_readable,true)
    };

    driver.read_full_at(block.offset_addr() + BlockHeader::SIZE as SAddr + offset as SAddr,
                        &mut buf[0..bytes_to_read])?;

    // Work out the new read position
    if end_of_block {
      Ok((bytes_to_read, (block_header.next_block, 0)))
    } else {
      Ok((bytes_to_read, (block, offset + bytes_to_read)))
    }
  }

  fn check_exists(&self, file: Self::FID) -> bool {
    self.get_directory_entry(file).is_ok()
  }

  fn get_file_size(&self, file: Self::FID) -> FileSystemResult<FileAddr> {
    let dir_entry = self.get_directory_entry(file)?;
    let last_block = self.get_block_header(dir_entry.last_block)?;

    if dir_entry.block_count.value() > 0 {
      let size = ((dir_entry.block_count.value() - 1) as FileAddr * BLOCKSIZE_USABLE as FileAddr) + last_block.bytes_used as FileAddr;

      Ok(size)
    } else {
      Err(IoError::OutOfRange)
    }
  }

  fn get_free_space(&self) -> FileSystemResult<u64> {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    let mut blocks_used : u64 = 0;

    for i in 0..Self::BITMAP_SIZE {
      let mut buf = [ 0x00u8 ];

      driver.read_at(Self::BITMAP_START + i, &mut buf)?;

      for bit in 0..=7 {
        if (buf[0] & (0x01<<bit)) > 0 {
          blocks_used += 1;
        }
      }
    }

    Ok((Self::TOTAL_BLOCKS as u64 - blocks_used) * BLOCKSIZE_USABLE as u64)
  }

  fn block_and_offset_for_file_location(&self, file: Self::FID, addr: FileAddr) -> FileSystemResult<(BlockNumber, usize)> {
    let dir_entry = self.get_directory_entry(file)?;

    let mut block_number = dir_entry.first_block()?;
    let mut bytes_remaining = addr;

    loop {
      let block_header = self.get_block_header(block_number)?;

      if bytes_remaining <= (block_header.bytes_used() as FileAddr) {
        break;
      }

      bytes_remaining -= block_header.bytes_used() as FileAddr;
      block_number = block_header.next_block()?;
    }

    Ok((block_number,bytes_remaining as usize))
  }

  fn get_next_file_in_directory(&self, _parent: Option<Self::FID>, previous: Option<Self::FID>) -> Option<Self::FID> {
    let mut candidate = match previous {
      None => Some(FileUid::first()),
      Some(previous) => previous.next_id()
    };

    while candidate.is_some() {
      if self.get_directory_entry(candidate.unwrap()).is_ok() {
        break;
      } else {
        candidate = candidate.unwrap().next_id();
      }
    }

    candidate
  }
}

impl<SD> FileSystemWrite for SnafusFileSystem<SD>
  where
    SD: Storage + RandomRead + RandomWrite
{
  fn open<P: Into<Self::FID>>(&self, fid: P, options: &OpenOptions) -> OxideResult<File<Self>, IoError> where Self: Sized {
    File::open_or_create(self, fid, options.clone())
  }

  fn create_file(&self, file: Self::FID) -> OxideResult<(), IoError> {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    if (file.0 as usize) < MAXFILES {
      let mut buffer = [0x00; DirectoryEntry::SIZE];

      driver.read_full_at_hinted(Self::DIRECTORY_START + file.offset_addr(), &mut buffer, AccessHint::NONSEQUENTIAL)?;

      let entry = DirectoryEntry::from_bytes(&buffer);

      if entry.first_block.is_valid() {
        Err(IoError::Exists)
      } else {
        let new_directory_entry = DirectoryEntry {
          first_block: BlockNumber::INVALID,
          last_block: BlockNumber::INVALID,
          block_count: BlockNumber::ZERO,
        };
        driver.write_all_at_hinted(Self::DIRECTORY_START + file.offset_addr(), &new_directory_entry.as_bytes(),
                                   AccessHint::NONSEQUENTIAL)?;

        // Now extend the file with the first block
        self.extend_file(driver, file)?;

        Ok(())
      }
    } else {
      Err(IoError::OutOfRange)
    }
  }

  fn write_at_location(&self, file: Self::FID, block: BlockNumber, offset: usize, buf: &[u8]) -> FileSystemResult<(usize, (BlockNumber, usize))> {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    if offset >= BLOCKSIZE_USABLE {
      return Err(IoError::OutOfRange);
    }

    let max_writeable = BLOCKSIZE_USABLE - offset;
    let bytes_to_write = if max_writeable < buf.len() { max_writeable } else { buf.len() };

    let mut block_header = self.get_block_header_using(driver, block)?;

    // If our write would extend the used space in this block, we extend it
    if (offset + bytes_to_write) > block_header.bytes_used as usize {
      block_header.bytes_used = (offset + bytes_to_write) as u8;
    }

    // Now write the actual data
    driver.write_all_at(block.offset_addr() + BlockHeader::SIZE as SAddr + offset as SAddr, &buf[0..bytes_to_write])?;

    // Did we write to the end of the block?  If so the next address is
    // in the next block
    if (offset + bytes_to_write) < BLOCKSIZE_USABLE {
      // Not at end of block
      driver.write_all_at_hinted(block.offset_addr(), &block_header.as_bytes(), AccessHint::WRITEONLY)?;
      Ok((bytes_to_write,(block,offset+bytes_to_write)))
    } else {
      // At end of block; new head location is beginning of the next block
      if block_header.next_block.is_invalid() {
        // There is no next block yet, so we need to extend
        block_header.next_block = self.extend_file(driver, file)?;
      }
      // Update the block header
      driver.write_all_at_hinted(block.offset_addr(), &block_header.as_bytes(), AccessHint::WRITEONLY)?;
      Ok((bytes_to_write,(block_header.next_block,0)))
    }
  }

  fn truncate_at(&self, file: Self::FID, new_last_block: BlockNumber, offset: usize) -> FileSystemResult<()> {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    self.truncate_at_using(driver, file, new_last_block, offset)
  }

  fn remove_file(&self, file: Self::FID) -> FileSystemResult<()> {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    let mut dir_entry = self.get_directory_entry_using(driver, file)?;
    let first_block = dir_entry.first_block;

    if first_block.is_valid() {
      self.truncate_at_using(driver, file, first_block, 0)?;
    } // Don't fail if it isn't, we will delete the directory entry anyway

    dir_entry.block_count = BlockNumber::ZERO;
    dir_entry.first_block = BlockNumber::INVALID;
    dir_entry.last_block = BlockNumber::INVALID;
    driver.write_all_at_hinted(Self::DIRECTORY_START + file.offset_addr(), &dir_entry.as_bytes(),
                               AccessHint::NONSEQUENTIAL)?;
    self.mark_block_free(driver, first_block)?;

    Ok(())
  }

  fn sync(&self) -> FileSystemResult<()> {
    let mut driver_lock = self.lock_driver();
    let driver = driver_lock.deref_mut();

    driver.flush()?;

    Ok(())
  }
}

// Tests =====================================================================
#[cfg(test)]
mod tests {
  use avrox_storage::buffered::PageBuffer;
  use avrox_storage::serprom::generic::dummy::DummyPromBusClient;
  use avrox_storage::serprom::composite::tests::TestCompositeProm;
  use avr_oxide::devices::serialbus::UsesSerialBusClient;
  use avrox_storage::{FileAddr, RandomRead, RandomWrite};
  use avrox_storage::fs::filesystem::{FileSystemRead, FileSystemWrite};
  use avrox_storage::fs::snafus::{BlockNumber, BLOCKSIZE_USABLE, FileUid, SnafusFileSystem};

  type TestBuffer = PageBuffer<32,TestCompositeProm>;


  #[test]
  pub fn test_format_snafus() {
    let mut test_fs = SnafusFileSystem::with_driver(TestBuffer::with_driver(TestCompositeProm::using_client(DummyPromBusClient::new())));

    assert!(!test_fs.is_formatted().unwrap());

    test_fs.format();

    assert!(test_fs.is_formatted().unwrap());

    // Check the block map looks right (only the first two blocks should be
    // marked used)
    println!("Block map: {:x?}", test_fs.get_block_map_16b());
    assert_eq!(test_fs.get_block_map_16b(),
               [ 0x03u8, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00]);

    println!("Directory: {:x?}", test_fs.get_directory_16b());
    assert_eq!(test_fs.get_directory_16b(), [0x00; 16]);

    println!("Header:    {:x?}", test_fs.get_header_16b());
    assert_eq!(test_fs.get_header_16b(),
               [0x05u8, 0xf5, // Magic number
                 0x01, // Version
                 0x00, 0xbe, // Endianness
                 0x01, 0xfe, // Usable blocks
                 0x00, 0x02, // First block
                 0, 0, 0, 0, 0, 0, 0 ]); // Directory entries

    println!("Block 2:   {:x?}", test_fs.get_block_16b(2.into()));
    assert_eq!(test_fs.get_block_16b(2.into()),
               [0x00u8, 0x00, // Previous block
                 0x00, 0x00, // Next block
                 0x00, // Checksum
                 0x00, // Bytes Used
                 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff ]);

    println!("First free block: {:?}", test_fs.get_first_free_block());
    assert_eq!(test_fs.get_first_free_block().unwrap().value(), 2);
    println!("Free space: {}", test_fs.get_free_space().unwrap());
  }

  #[test]
  pub fn test_create_file() {
    let mut test_fs = SnafusFileSystem::with_driver(TestBuffer::with_driver(TestCompositeProm::using_client(DummyPromBusClient::new())));
    test_fs.format().unwrap();

    let dir_entry = test_fs.create_file(FileUid(1)).unwrap();
    println!("New dir entry: {:x?}", &dir_entry);

    // Check the block map looks right (three blocks used)
    println!("Block map: {:x?}", test_fs.get_block_map_16b());
    assert_eq!(test_fs.get_block_map_16b(),
               [ 0x07u8, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00]);

    println!("Directory: {:x?}", test_fs.get_directory_16b());
    assert_eq!(test_fs.get_directory_16b(),
               [0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x02,
                 0x00, 0x02, 0x00, 0x01,
                 0x00, 0x00, 0x00, 0x00]);

    println!("Block 2:   {:x?}", test_fs.get_block_16b(2.into()));
    assert_eq!(test_fs.get_block_16b(2.into()),
               [0x00u8, 0x00, // Previous block
                 0x00, 0x00, // Next block
                 0x00, // Checksum
                 0x00, // Bytes Used
                 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff ]);

    println!("First free block: {:?}", test_fs.get_first_free_block());
    assert_eq!(test_fs.get_first_free_block().unwrap().value(), 3);

    println!("Location of first byte:  {:?}", test_fs.block_and_offset_for_file_location(FileUid(1),0));
    println!("Location of second byte: {:?}", test_fs.block_and_offset_for_file_location(FileUid(1),1));

    assert!(test_fs.block_and_offset_for_file_location(FileUid(1),1).is_err());
    println!("Free space: {}", test_fs.get_free_space().unwrap());
  }

  #[test]
  pub fn test_internal_write_methods() {
    let mut test_fs = SnafusFileSystem::with_driver(TestBuffer::with_driver(TestCompositeProm::using_client(DummyPromBusClient::new())));
    test_fs.format().unwrap();

    let dir_entry = test_fs.create_file(FileUid(0)).unwrap();
    println!("New dir entry: {:x?}", &dir_entry);

    let test_data1 = b"Some test data";
    let test_data2 = b"Some more test data";
    let test_data3 = &[ 0xabu8; 255 ];

    let (block,offset) = test_fs.block_and_offset_for_file_location(FileUid(0),0).unwrap();

    // Let's write at the 'end' of the file
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), block, offset, test_data1).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);

    assert_eq!(bytes, test_data1.len());
    assert_eq!(block, new_block);
    assert_eq!(new_offset, test_data1.len());

    // Let's do another write over the top
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), block, offset, test_data2).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);

    assert_eq!(bytes, test_data2.len());
    assert_eq!(block, new_block);
    assert_eq!(new_offset, test_data2.len());

    // Now let's append
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), new_block, new_offset, test_data2).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);

    assert_eq!(bytes, test_data2.len());
    assert_eq!(block, new_block);
    assert_eq!(new_offset, test_data2.len()*2);

    // OK!  Now let's append something that will consume to the end of the block
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), new_block, new_offset, test_data3).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);

    assert_eq!(bytes, BLOCKSIZE_USABLE - (test_data2.len()*2));
    assert_ne!(block, new_block);
    assert_eq!(new_offset, 0);


    // Woohoo.  Check the metadata looks about right
    // Check the block map looks right (now four blocks used)
    println!("Block map: {:x?}", test_fs.get_block_map_16b());
    assert_eq!(test_fs.get_block_map_16b(),
               [ 0x0fu8, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00]);

    println!("Directory: {:x?}", test_fs.get_directory_16b());
    assert_eq!(test_fs.get_directory_16b(),
               [0x00, 0x02, // First block
                 0x00, 0x03, // Last block
                 0x00, 0x02, // Block count
                 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00]);

    println!("Block 2:   {:x?}", test_fs.get_block_16b(2.into()));
    assert_eq!(test_fs.get_block_16b(2.into()),
               [0x00u8, 0x00, // Previous block
                 0x00, 0x03, // Next block
                 0x00, // Checksum
                 BLOCKSIZE_USABLE as u8, // Bytes Used
                 b'S', b'o', b'm', b'e', b' ', b'm', b'o', b'r', b'e', b' ' ]);

    println!("Block 3:   {:x?}", test_fs.get_block_16b(3.into()));
    assert_eq!(test_fs.get_block_16b(3.into()),
               [0x00u8, 0x02, // Previous block
                 0x00, 0x00, // Next block
                 0x00, // Checksum
                 0 as u8, // Bytes Used
                 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff ]);

    println!("Free space: {}", test_fs.get_free_space().unwrap());
  }

  #[test]
  pub fn test_truncate_and_delete() {
    let mut test_fs = SnafusFileSystem::with_driver(TestBuffer::with_driver(TestCompositeProm::using_client(DummyPromBusClient::new())));
    test_fs.format().unwrap();

    let original_free_space = test_fs.get_free_space().unwrap();
    println!("Original free space = {}", original_free_space);

    let dir_entry = test_fs.create_file(FileUid(0)).unwrap();
    println!("New dir entry: {:x?}", &dir_entry);

    let test_data = [0xde; 256];

    let (block,offset) = test_fs.block_and_offset_for_file_location(FileUid(0),0).unwrap();

    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), block, offset, &test_data).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), new_block, new_offset, &test_data).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), new_block, new_offset, &test_data).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), new_block, new_offset, &test_data).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);


    println!("File length {}, free space {}", test_fs.get_file_size(FileUid(0)).unwrap(), test_fs.get_free_space().unwrap());
    assert_eq!(test_fs.get_file_size(FileUid(0)).unwrap(), 1000);

    // Now let's truncate halfway through the first block
    test_fs.truncate_at(FileUid(0),block,125).unwrap();
    println!("File length {}, free space {}", test_fs.get_file_size(FileUid(0)).unwrap(), test_fs.get_free_space().unwrap());

    assert_eq!(test_fs.get_file_size(FileUid(0)).unwrap(), 125);
    assert_eq!(test_fs.get_free_space().unwrap(), original_free_space - BLOCKSIZE_USABLE as u64); // We're only using 1 block now

    // Now we should delete entirely
    test_fs.remove_file(FileUid(0)).unwrap();
    assert!(!test_fs.check_exists(FileUid(0)));
    assert_eq!(test_fs.get_free_space().unwrap(), original_free_space);
    println!("Free space = {}", original_free_space);
  }

  #[test]
  pub fn test_internal_read_methods() {
    let mut test_fs = SnafusFileSystem::with_driver(TestBuffer::with_driver(TestCompositeProm::using_client(DummyPromBusClient::new())));
    test_fs.format().unwrap();

    let dir_entry = test_fs.create_file(FileUid(0)).unwrap();
    println!("New dir entry: {:x?}", &dir_entry);

    let test_data1 = b"Some test data";
    let test_data2 = b"Some more test data";
    let test_data3 = &[ 0xabu8; 255 ];

    let (first_block,offset) = test_fs.block_and_offset_for_file_location(FileUid(0), 0).unwrap();
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), first_block, offset, test_data1).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);

    println!("Block 2:   {:x?}", test_fs.get_block_16b(2.into()));

    let mut bigbuffer = [0x00u8; 512];
    let mut smallbuffer = [0x00u8; 6];
    let (read_bytes,(read_block, read_offset)) = test_fs.read_from_location(FileUid(0), first_block, offset, &mut bigbuffer).unwrap();

    println!("Read {} bytes, new_position ({:?},{})", read_bytes, read_block, read_offset);
    assert_eq!(read_bytes, 14);
    assert_eq!(read_block, BlockNumber::INVALID); // We read til EOF

    let (read_bytes,(read_block, read_offset)) = test_fs.read_from_location(FileUid(0), first_block, offset, &mut smallbuffer).unwrap();
    println!("Read {} bytes, new_position ({:?},{})", read_bytes, read_block, read_offset);
    assert_eq!(read_bytes, 6);
    assert_eq!(read_block.value(), 2); // This time we didn't read all
    assert_eq!(read_offset, 6);

    // Now let's read the rest from there
    let (read_bytes,(read_block, read_offset)) = test_fs.read_from_location(FileUid(0), read_block, read_offset, &mut bigbuffer).unwrap();
    println!("Read {} bytes, new_position ({:?},{})", read_bytes, read_block, read_offset);
    assert_eq!(read_bytes, 14-6);
    assert_eq!(read_block, BlockNumber::INVALID); // Now we should be EOF again

    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), first_block, offset, test_data2).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);
    // Now let's append
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), new_block, new_offset, test_data2).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);
    // OK!  Now let's append something that will consume to the end of the block
    let (bytes,(new_block,new_offset)) = test_fs.write_at_location(FileUid(0), new_block, new_offset, test_data3).unwrap();
    println!("Wrote {} bytes, new position ({:?},{})", bytes, new_block, new_offset);

    // Now let's try and read everything
    let (read_bytes,(read_block, read_offset)) = test_fs.read_from_location(FileUid(0), first_block, offset, &mut bigbuffer).unwrap();
    println!("Read {} bytes, new_position ({:?},{})", read_bytes, read_block, read_offset);
    assert_eq!(read_bytes, BLOCKSIZE_USABLE);
    assert_eq!(read_block.value(), 3);
    assert_eq!(read_offset, 0);

    println!("Free space: {}", test_fs.get_free_space().unwrap());
  }
}