/*
 * 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"
#include "usb_cmd_mbx.h"
#include "usbd_cdc_if.h"

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();
  }

  uint8_t result;
  while (get_number_of_usb_commands_in_mailbox()) {
#ifdef DEBUG_COMMADS
    log_debug("pop_and_execute_commands", "pop raw command", 0);
#endif /* DEBUG_COMMADS */ 
    raw_command_t raw_command = pop_usb_command();
    result = CDC_Transmit_FS(raw_command.payload, raw_command.size);

    if (!(result == USBD_OK || result == USBD_BUSY)) {
      log_error("pop_and_execute_commands", "Sending of raw command was not successful!", 0);
    }
  }
}