/*
 * execute.cpp
 *
 *  Created on: 18 Jul 2021
 *      Author: Andreas Berthoud
 */

#include <queue>
#include <cstring>

#include "Command.hpp"
#include "execute.hpp"
#include "stm32wbxx_hal.h"
#include "LogCommand.hpp"

std::queue<Command*> command_queue;

// TODO: Add limit and return false if queue is full. Otherwise, we will get a full heap and crash :-(
void push_command(Command * command) {
  command_queue.push(command);
}

void pop_and_execute_commands() {
  while (!command_queue.empty()) {
    Command * command = command_queue.front();
    bool was_successful = command->execute();
    HAL_Delay(5);  // this delay is required. Otherwise, the command were not sent somehow...
    if (!was_successful) {
      log_error("pop_and_execute_commands", "Execution of command with ID %n was not successful!", 1, command->id);
    }
    delete command;
    command_queue.pop();
  }
}