/*
 * ble_cmd_mbx.cpp
 *
 *  Created on: Jul 23, 2021
 *      Author: Andreas Berthoud
 */

#include <queue>
#include "string.h"
#include "ble_cmd_mbx.h"
#include "stm32_seq.h"
#include "app_conf.h"


std::queue<raw_command_t> raw_ble_command_queue;

void push_ble_command(uint8_t * payload_ptr, uint16_t length) {
  raw_command_t command = {
    .size = length,
  };

  memcpy(command.payload, payload_ptr, length);
  raw_ble_command_queue.push(command);
  UTIL_SEQ_SetTask(1<<CFG_TASK_SEND_COMMAND_ID, CFG_SCH_PRIO_0);
  return;
}

uint8_t get_number_of_ble_commands_in_mailbox() {
  return raw_ble_command_queue.size();
}

raw_command_t pop_ble_command() {
  raw_command_t command = raw_ble_command_queue.front();
  raw_ble_command_queue.pop();
  return command;
}