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

#include "Command.hpp"
#include "string.h"
#include "stm32wbxx_hal.h"
#include "commands.hpp"
#include "HeartbeatCommand.hpp"
#include "LedCommand.hpp"
#include "GPCommand.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("Execution of command with ID %n was not successful!", 1, command->id);
    }
    delete command;
    command_queue.pop();
  }
}

void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size) {

  switch (command_id)
  {
  case COMMAND_HEARTBEAT_REQUEST:
    push_command(new HeartbeatRequest(payload_ptr, size));
    break;
  case COMMAND_LED_REQUEST:
    push_command(new LedRequest(payload_ptr, size));
    break;
  case COMMAND_GP_REQUEST:
    push_command(new GPRequest(payload_ptr, size));
    break;

  default:
    break;
  }

}