sensor_commands.cpp

#include "sensor_commands.h"

namespace sensor_commands {
  const uint8_t MAX_CALLBACKS = 20;
  const uint8_t MAX_COMMAND_SIZE = 30;

  uint8_t callbacks_count = 0;

  struct Callback {
    const char *name;
    union {
      void (*intFunction)(int32_t);
      void (*voidFunction)(void);
    };
    const char *doc;
    bool has_parameter;
  };

  Callback callbacks[MAX_CALLBACKS];

  //NOTE: Probably possible to DRY (with templates?)
  void defineCallback(const char *name, void (*function)(void), const char *doc) {
    if (callbacks_count < MAX_CALLBACKS) {
      callbacks[callbacks_count].name = name;
      callbacks[callbacks_count].voidFunction = function;
      callbacks[callbacks_count].doc = doc;
      callbacks[callbacks_count].has_parameter = false;
      callbacks_count++;
    } else {
      Serial.println(F("Too many callbacks have been defined."));
    }
  }

  void defineIntCallback(const char *name, void (*function)(int32_t), const char *doc) {
    if (callbacks_count < MAX_CALLBACKS) {
      callbacks[callbacks_count].name = name;
      callbacks[callbacks_count].intFunction = function;
      callbacks[callbacks_count].doc = doc;
      callbacks[callbacks_count].has_parameter = true;
      callbacks_count++;
    } else {
      Serial.println(F("Too many callbacks have been defined."));
    }
  }

  uint8_t parseCommand(const char *command, char *function_name, int32_t &argument) {
    char split_command[MAX_COMMAND_SIZE];
    strlcpy(split_command, command, MAX_COMMAND_SIZE);
    Serial.print(F("Received : '"));
    Serial.print(command);
    Serial.println("'");
    char *arg;
    char *part1;
    part1 = strtok(split_command, " ");
    if (!part1) {
      // Empty string
      return 1;
    }
    strlcpy(function_name, part1, MAX_COMMAND_SIZE);
    arg = strtok(NULL, " ");
    uint8_t code = 0;
    if (arg) {
      char *end;
      argument = strtol(arg, &end, 10);
      if (*end) {
        // Second argument isn't a number
        code = 2;
      }
    } else {
      // No argument
      code = 2;
    }
    return code;
  }

  void listAvailableCallbacks() {
    for (uint8_t i = 0; i < callbacks_count; i++) {
      Serial.print("  ");
      Serial.print(callbacks[i].name);
      Serial.print(callbacks[i].doc);
      Serial.println(".");
    }
    led_effects::showKITTWheel(color::red, 1);
  }

  void run(const char *command) {
    char function_name[MAX_COMMAND_SIZE];
    int32_t argument = 0;
    bool has_argument;
    has_argument = (parseCommand(command, function_name, argument) == 0);

    for (uint8_t i = 0; i < callbacks_count; i++) {
      if (!strcmp(function_name, callbacks[i].name) && has_argument == callbacks[i].has_parameter) {
        Serial.print("Calling : ");
        Serial.print(function_name);
        if (has_argument) {
          Serial.print("(");
          Serial.print(argument);
          Serial.println(")");
          callbacks[i].intFunction(argument);
        } else {
          Serial.println();
          callbacks[i].voidFunction();
        }
        return;
      }
    }
    Serial.println(F("Message not supported. Available commands :"));
    listAvailableCallbacks();
  }
}