src/movement/planner_queue.h

Go to the documentation of this file.

/*
 * planner_queue.h
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf 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 Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef PLANNER_QUEUE_H
#define PLANNER_QUEUE_H

#include "yarf.h"
#include "block.h"

#include <stdbool.h>
#include <stddef.h>

extern block_t q[PLANNER_QUEUE_SIZE];

extern volatile uint8_t q_head;

extern volatile uint8_t q_tail;




inline uint8_t 
plan_q_next_index(uint8_t block_index)
{
  block_index += 1;
  if (block_index == PLANNER_QUEUE_SIZE) {
    block_index = 0;
  }
  return block_index;
}


inline uint8_t
plan_q_prev_index(uint8_t block_index) {
  if (block_index == 0) {
    block_index = PLANNER_QUEUE_SIZE;
  }
  block_index -= 1;
  return block_index;
}

inline bool
plan_q_full(void)
{
  return (plan_q_next_index(q_head) == q_tail);
}

inline bool
plan_q_empty(void)
{
  return (q_head == q_tail);
}

inline uint8_t
plan_q_size(void)
{
  uint8_t size = q_head - q_tail;
  return size >= 0 ? size : (PLANNER_QUEUE_SIZE - size);
}

inline block_t *
plan_q_get(uint8_t index)
{
  return &q[index];
}


inline block_t *
plan_q_head(void)
{
  if (plan_q_full()) {
    return NULL;
  }
  return &q[q_head];
}

inline uint8_t
plan_q_head_index(void)
{
  return q_head;
}

inline void
plan_q_shift_head(void)
{
  if (!plan_q_full()) {
    q_head = plan_q_next_index(q_head);
  }
}

inline block_t *
plan_q_tail(void)
{
  if (plan_q_empty()) { 
    return NULL;
  }
  return &q[q_tail];
}


inline uint8_t
plan_q_tail_index(void)
{
  return q_tail;
}

inline void
plan_q_discard_tail(void)
{
  if (!plan_q_empty()) {
    q_tail = plan_q_next_index(q_tail);
  }
}

inline void
plan_q_clear(void)
{
  q_head = 0;
  q_tail = 0;
}

inline void
plan_q_init(void)
{
  plan_q_clear();
}

#endif //PLANNER_QUEUE_H