/* delayq.rs
 *
 * Developed by Tim Walls <tim.walls@snowgoons.com>
 * Copyright (c) All Rights Reserved, Tim Walls
 */
//! Simple delay queue implementation


// Imports ===================================================================
use core::ops::{Sub, SubAssign};
use avr_oxide::alloc::vec::Vec;
use avr_oxide::alloc::boxed::Box;

// Declarations ==============================================================

/**
 * Trait implemented for types that have a minimum bound
 */
pub trait Floored {

  /// Return the minimum possible value for this type
  ///
  /// # Invariant
  /// The critical invariant for any type implementing this trait is this:
  ///   `(X) - (X) == floor()` for all `X`
  fn floor() -> Self;
}

#[derive(Copy,Clone,PartialEq,Eq)]
pub struct DelayQueueHandle(usize);

pub trait DelayQueue<C,T>
where
  C: Sub + SubAssign + Eq + Ord + Copy + Floored
{
  fn insert_at(&mut self, position: C, item: T) -> DelayQueueHandle;

  /// Remove an element from the delay queue
  ///
  /// Returns `true` if the element was found and removed, `false` if there
  /// was no matching element.
  fn remove(&mut self, cancel_handle: DelayQueueHandle) -> bool;

  fn decrement(&mut self, by: C);

  fn consume_next_ready(&mut self) -> Option<T>;
}

pub struct SimpleDelayQueue<C,T>
where
  C: Sub + SubAssign + Eq + Ord + Copy + Floored
{
  first: Option<Box<SimpleDelayQueueElement<C,T>>>,
  ready: Vec<T>
}

pub struct SimpleDelayQueueElement<C,T> {
  counter: C,
  contains: T,

  next: Option<Box<Self>>
}

// Code ======================================================================
impl Floored for u8 {
  fn floor() -> Self {
    0u8
  }
}
impl Floored for u16 {
  fn floor() -> Self {
    0u16
  }
}
impl Floored for u32 {
  fn floor() -> Self {
    0u32
  }
}


impl<C,T> SimpleDelayQueue<C,T>
where
  C: Sub + SubAssign + Eq + Ord + Copy + Floored
{
  /// Create a new, empty delay queue
  pub fn new() -> Self {
    SimpleDelayQueue {
      first: Option::None,
      ready: Vec::new()
    }
  }

  /// Pop all the ready elements in the queue off the front and into the
  /// ready list.
  fn move_ready(&mut self) {
    loop {
      match &self.first {
        None => break,
        Some(element) => {
          if element.counter != C::floor() {
            break;
          }
        }
      };

      let content = match self.first.take() {
        None => avr_oxide::oserror::halt(avr_oxide::oserror::OsError::InternalError),
        Some(node) => {
          self.first = node.next;
          node.contains
        }
      };
      self.ready.push(content);
    }
  }


}

impl<C,T> DelayQueue<C,T> for SimpleDelayQueue<C,T>
where
  C: Sub + SubAssign + Eq + Ord + Copy + Floored
{
  fn insert_at(&mut self, mut position: C, item: T) -> DelayQueueHandle {

    let mut insert_at = &mut self.first as *mut Option<Box<SimpleDelayQueueElement<C,T>>>;
    loop {
      unsafe {
        match &mut *insert_at  {
          None => {
            // End of the list - we'll stick our element here
            break;
          },
          Some(some_element) => {
            if position < some_element.counter {
              // I fits, so I sits
              //
              // We modify the counter of the element we'll sit in front of
              some_element.counter -= position;
              break;
            } else {
              // I don't fit, decrement the delay and move on to the next one
              position -= some_element.counter;
            }

            insert_at = &mut some_element.next as *mut Option<Box<SimpleDelayQueueElement<C,T>>>;
          }
        }
      };
    }

    // insert_at now points at the Option<> that will point at our element
    unsafe {
      let new_element = Box::new(SimpleDelayQueueElement {
        counter: position,
        contains: item,
        next: core::mem::replace(&mut *insert_at, Option::None)
      });
      let dqhandle = DelayQueueHandle(&*new_element as *const SimpleDelayQueueElement<C,T> as usize);
      (&mut *insert_at).replace(new_element);

      dqhandle
    }
  }

  /// Remove an element from the delay queue
  ///
  /// Returns `true` if the element was found and removed, `false` if there
  /// was no matching element.
  fn remove(&mut self, cancel_handle: DelayQueueHandle) -> bool {
    let mut candidate = &mut self.first as *mut Option<Box<SimpleDelayQueueElement<C,T>>>;
    loop {
      unsafe {
        match &mut *candidate {
          None => {
            // No more candidates to search for - return false
            return false;
          },
          Some(some_element) => {
            let candidate_handle = DelayQueueHandle(&**some_element as *const SimpleDelayQueueElement<C,T> as usize);

            if candidate_handle == cancel_handle {
              break;
            } else {
              candidate = &mut some_element.next as *mut Option<Box<SimpleDelayQueueElement<C,T>>>;
            }
          }
        }
      }
    }
    // Candidate now points at the Option<> that we are going to remove
    unsafe {
      // Extract the contents of the Option, replacing it with None
      match (&mut *candidate).take() {
        None => {
          avr_oxide::oserror::halt(avr_oxide::oserror::OsError::InternalError);
        },
        Some(instance) => {
          // Now replace it with what was in the 'next'
          let _none = core::mem::replace(&mut *candidate, instance.next);
        }
      }

    }
    true
  }

  #[optimize(speed)]
  fn decrement(&mut self, mut by: C) {
    // Decrement the head element by our count.  It is possible multiple
    // elements will be released, so keep going round until either there are
    // none left, or the last one remaining still has a while to wait.
    while by > C::floor() {
      match &mut self.first {
        None => break,
        Some(element) => {
          if element.counter > by {
            element.counter -= by;
            by = C::floor(); // I've consumed all the decrement
          } else {
            // I've released (one or more) elements; subtract from the
            // remaining decrement
            by -= element.counter;
            element.counter = C::floor();
          }
        }
      }

      self.move_ready();
    }
  }

  #[optimize(speed)]
  fn consume_next_ready(&mut self) -> Option<T> {
    self.ready.pop()
  }
}


// Tests =====================================================================
#[cfg(test)]
mod tests {
  use std::string::String;
  #[allow(unused_imports)]
  use super::*;

  #[test]
  fn delay_queue_ordered() {
    let mut queue = SimpleDelayQueue::<u16,&str>::new();

    queue.insert_at(1, "First");
    queue.insert_at(16, "Second");
    queue.insert_at(17, "Third");
    queue.insert_at(24, "Fourth");
    queue.insert_at(39, "Fifth");
    queue.insert_at(42, "Sixth");

    assert_eq!(queue.consume_next_ready(), Option::None);
    assert_eq!(queue.consume_next_ready(), Option::None);

    queue.decrement(1);
    assert_eq!(queue.consume_next_ready(), Option::Some("First"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    for _i in 0..14 {
      queue.decrement(1);
      assert_eq!(queue.consume_next_ready(), Option::None);
    }

    queue.decrement(1);
    assert_eq!(queue.consume_next_ready(), Option::Some("Second"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    queue.decrement(1);
    assert_eq!(queue.consume_next_ready(), Option::Some("Third"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    for _i in 0..50 {
      queue.decrement(1);
    }
    // Implementation detail; unconsumed but ready entries will pile up in
    // reverse order
    assert_eq!(queue.consume_next_ready(), Option::Some("Sixth"));
    assert_eq!(queue.consume_next_ready(), Option::Some("Fifth"));
    assert_eq!(queue.consume_next_ready(), Option::Some("Fourth"));
    assert_eq!(queue.consume_next_ready(), Option::None);
  }

  #[test]
  fn delay_queue_out_of_order() {
    let mut queue = SimpleDelayQueue::<u16,&str>::new();

    queue.insert_at(24, "Fourth");
    queue.insert_at(42, "Sixth");
    queue.insert_at(17, "Third");
    queue.insert_at(1, "First");
    queue.insert_at(16, "Second");
    queue.insert_at(39, "Fifth");


    assert_eq!(queue.consume_next_ready(), Option::None);
    assert_eq!(queue.consume_next_ready(), Option::None);

    queue.decrement(1);
    assert_eq!(queue.consume_next_ready(), Option::Some("First"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    for _i in 0..14 {
      queue.decrement(1);
      assert_eq!(queue.consume_next_ready(), Option::None);
    }

    queue.decrement(1);
    assert_eq!(queue.consume_next_ready(), Option::Some("Second"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    queue.decrement(1);
    assert_eq!(queue.consume_next_ready(), Option::Some("Third"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    for _i in 0..50 {
      queue.decrement(1);
    }
    // Implementation detail; unconsumed but ready entries will pile up in
    // reverse order
    assert_eq!(queue.consume_next_ready(), Option::Some("Sixth"));
    assert_eq!(queue.consume_next_ready(), Option::Some("Fifth"));
    assert_eq!(queue.consume_next_ready(), Option::Some("Fourth"));
    assert_eq!(queue.consume_next_ready(), Option::None);
  }

  #[test]
  fn delay_queue_large_decrement() {
    let mut queue = SimpleDelayQueue::<u16,&str>::new();

    queue.insert_at(24, "Fourth");
    queue.insert_at(42, "Sixth");
    queue.insert_at(17, "Third");
    queue.insert_at(1, "First");
    queue.insert_at(16, "Second");
    queue.insert_at(39, "Fifth");

    queue.decrement(17);
    assert_eq!(queue.consume_next_ready(), Option::Some("Third"));
    assert_eq!(queue.consume_next_ready(), Option::Some("Second"));
    assert_eq!(queue.consume_next_ready(), Option::Some("First"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    queue.decrement(17);
    assert_eq!(queue.consume_next_ready(), Option::Some("Fourth"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    queue.decrement(17);
    assert_eq!(queue.consume_next_ready(), Option::Some("Sixth"));
    assert_eq!(queue.consume_next_ready(), Option::Some("Fifth"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    queue.decrement(17);
    assert_eq!(queue.consume_next_ready(), Option::None);
  }

  #[test]
  fn delay_queue_remove() {
    let mut queue = SimpleDelayQueue::<u16,&str>::new();

    queue.insert_at(24, "Fourth");
    let sixth = queue.insert_at(42, "Sixth");
    let third = queue.insert_at(17, "Third");
    let first = queue.insert_at(1, "First");
    queue.insert_at(16, "Second");
    queue.insert_at(39, "Fifth");

    // Removing first time should succeed:
    assert_eq!(queue.remove(first), true);
    assert_eq!(queue.remove(sixth), true);
    // Attempting to remove a second time should fail:
    assert_eq!(queue.remove(sixth), false);


    queue.decrement(17);
    assert_eq!(queue.consume_next_ready(), Option::Some("Third"));
    assert_eq!(queue.consume_next_ready(), Option::Some("Second"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    // This should fail to remove, because we already dropped the thing naturally
    assert_eq!(queue.remove(third), false);

    queue.decrement(17);
    assert_eq!(queue.consume_next_ready(), Option::Some("Fourth"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    queue.decrement(17);
    assert_eq!(queue.consume_next_ready(), Option::Some("Fifth"));
    assert_eq!(queue.consume_next_ready(), Option::None);

    queue.decrement(17);
    assert_eq!(queue.consume_next_ready(), Option::None);
  }
}