/*
 * Command.cpp
 *
 *  Created on: Jul 8, 2021
 *      Author: Andreas Berthoud
 */
#include <queue>
#include <cstring>

#include "Command.hpp"
#include "string.h"
#include "commands.hpp"

Command::Command(CommandId id) : id(id) {
  this->data[0] = id;
  this->data[3] = 0x00; // reserved
  this->payload_ptr = this->data + 4;
 }

void Command::set_payload_length(uint16_t payload_length) {
  this->payload_length = payload_length;
  data[1] = (payload_length & 0xFF00) >> 8;
  data[2] = payload_length & 0x00FF;
}

uint16_t Command::get_payload_length() {
  return this->payload_length;
}

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();
    if (!was_successful) {
      log_error("Execution of command with ID %n was not successful!", 1, command->id);
    }
    delete command;
    command_queue.pop();
  }
}