/*
 * command_interpreter.c
 *
 *  Created on: Jul 10, 2021
 *      Author: Andreas Berthoud
 */

#include <stdio.h>
#include "usbd_cdc_if.h"
#include "commands.h"
#include "ble_cmd_mbx.h"

enum command_target_t {
  client,
  server,
};

// #define DEBUG_COMMADS 

void usb_receive(uint8_t *buf, uint32_t *len) {

  if (*len < 5) {
    log_error("usb_receive", "received command which cannot be interpreted", 0);
    return;
  }
  uint16_t length = buf[1] << 8 | buf[2];
  uint16_t command_total_length = length + 5;
  uint8_t command_target = buf[3];
  uint8_t stop_byte = 0x1;

  if (*len >= command_total_length - 1) {
    stop_byte = buf[command_total_length - 1];
  }

  if (stop_byte != 0xFF) {
    log_error("usb_receive", "received command has invalid stop byte: 0x%x", 1, stop_byte);
    return;
  }

  if (command_target == server) {
#ifdef DEBUG_COMMADS
    uint8_t buffer[512];
    buffer[0] = (uint8_t)('0');
    buffer[1] = (uint8_t)('x');
    uint8_t i;
    for (i = 0; i<command_total_length; i++) {
      snprintf((char*)(buffer + i + 2), 512-i-2, "%x", buf[i]);
    }
    buffer[i + 2] = (uint8_t)('\0');
    log_debug("usb_receive", "raw command: %s", 1, buffer);
#endif /* DEBUG_COMMADS */ 
    push_ble_command(buf, command_total_length);
    return;
  }

  uint8_t command_id = buf[0];
  uint8_t * payload_ptr = buf + 4;
  if (command_target == client) {
    handle_received_usb_command(command_id, payload_ptr, length);
  } else {
    log_error("usb_receive", "unknown command target: 0x%x", 1, command_target);
  }

}