#include	"dda_queue.h"

/** \file
	\brief DDA Queue - manage the move queue
*/

#include	<string.h>

#include	"config_wrapper.h"
#include	"timer.h"
#include	"serial.h"
#include	"temp.h"
#include	"delay.h"
#include	"sersendf.h"
#include	"clock.h"
#include "cpu.h"
#include	"memory_barrier.h"

/**
  Movebuffer head pointer. Points to the last move in the queue. This variable
  is used both in and out of interrupts, but is only written outside of
  interrupts.
*/
static uint8_t mb_head = 0;

/// movebuffer tail pointer. Points to the currently executing move
/// this variable is read/written both in and out of interrupts.
uint8_t	mb_tail = 0;

/// move buffer.
/// holds move queue
/// contents are read/written both in and out of interrupts, but
/// once writing starts in interrupts on a specific slot, the
/// slot will only be modified in interrupts until the slot is
/// is no longer live.
/// The size does not need to be a power of 2 anymore!
DDA BSS movebuffer[MOVEBUFFER_SIZE];

/// Find the next DDA index after 'x', where 0 <= x < MOVEBUFFER_SIZE
#define MB_NEXT(x) ((x) < MOVEBUFFER_SIZE - 1 ? (x) + 1 : 0)

/// check if the queue is completely full
uint8_t queue_full() {
	MEMORY_BARRIER();
  return MB_NEXT(mb_head) == mb_tail;
}

/// check if the queue is completely empty
uint8_t queue_empty() {
  uint8_t result;

  ATOMIC_START
    result = (mb_tail == mb_head && movebuffer[mb_tail].live == 0);
  ATOMIC_END

	return result;
}

/// Return the current movement, or NULL, if there's no movement going on.
DDA *queue_current_movement() {
  DDA* current;

  ATOMIC_START
    current = &movebuffer[mb_tail];

    if ( ! current->live)
      current = NULL;
  ATOMIC_END

  return current;
}

// -------------------------------------------------------
// This is the one function called by the timer interrupt.
// It calls a few other functions, though.
// -------------------------------------------------------
/// Take a step or go to the next move.
void queue_step() {

  if (movebuffer[mb_tail].live) {
    dda_step(&movebuffer[mb_tail]);
	}

  // Start the next move if this one is done.
  if ( ! movebuffer[mb_tail].live) {
    /**
      This is a simplified version of next_move() (which we'd use it it wasn't
      so performance critical here).

      queue_empty() used in next_move() needs no atomic protection, because
      we're in an interrupt already.
    */
    if (mb_tail != mb_head) {
      mb_tail = MB_NEXT(mb_tail);
      dda_start(&movebuffer[mb_tail]);
    }
  }
}

/// add a move to the movebuffer
/// \note this function waits for space to be available if necessary, check queue_full() first if waiting is a problem
/// This is the only function that modifies mb_head and it always called from outside an interrupt.
void enqueue_home(TARGET *t, uint8_t endstop_check, uint8_t endstop_stop_cond) {
	// don't call this function when the queue is full, but just in case, wait for a move to complete and free up the space for the passed target
	while (queue_full())
		delay_us(100);

  uint8_t h = MB_NEXT(mb_head);

	DDA* new_movebuffer = &(movebuffer[h]);

  // Initialise queue entry to a known state. This also clears flags like
  // dda->live, dda->done and dda->wait_for_temp.
  new_movebuffer->allflags = 0;

  new_movebuffer->endstop_check = endstop_check;
  new_movebuffer->endstop_stop_cond = endstop_stop_cond;
  dda_create(new_movebuffer, t);

  /**
    It's pointless to queue up movements which don't actually move the stepper,
    e.g. pure velocity changes or movements shorter than a single motor step.

    That said, accept movements which do move the steppers by forwarding
    mb_head. Also kick off movements if it's the first movement after a pause.
  */
  if ( ! new_movebuffer->nullmove) {
    // make certain all writes to global memory
    // are flushed before modifying mb_head.
    MEMORY_BARRIER();

    mb_head = h;

    uint8_t isdead;

    ATOMIC_START
      isdead = (movebuffer[mb_tail].live == 0);
    ATOMIC_END

    if (isdead) {
      timer_reset();
      next_move();
      // Compensate for the cli() in timer_set().
      sei();
    }
	}
}

/// go to the next move.
/// be aware that this is sometimes called from interrupt context, sometimes not.
/// Note that if it is called from outside an interrupt it must not/can not
/// be interrupted such that it can be re-entered from within an interrupt.
/// The timer interrupt MUST be disabled on entry. This is ensured because
/// the timer was disabled at the start of the ISR or else because the current
/// move buffer was dead in the non-interrupt case (which indicates that the
/// timer interrupt is disabled).
void next_move() {

  if (queue_empty() == 0) {
    // Tail must be set before calling timer_set(), as timer_set() reenables
    // the timer interrupt, potentially exposing mb_tail to the timer
    // interrupt routine.
    mb_tail = MB_NEXT(mb_tail);

      dda_start(&movebuffer[mb_tail]);
		}
	}

/// DEBUG - print queue.
/// Qt/hs format, t is tail, h is head, s is F/full, E/empty or neither
void print_queue() {
  sersendf_P(PSTR("Queue: %d/%d%c\n"), mb_tail, mb_head, (queue_full()?'F':(queue_empty()?'E':' ')));
}

/// dump queue for emergency stop.
/// Make sure to have all timers stopped with timer_stop() or
/// unexpected things might happen.
/// \todo effect on startpoint is undefined!
void queue_flush() {

  // if the timer were running, this would require
  // wrapping in ATOMIC_START ... ATOMIC_END.
  mb_tail = mb_head;
  movebuffer[mb_head].live = 0;
}

/// wait for queue to empty
void queue_wait() {
	while (queue_empty() == 0)
		clock();
}