sensor_commands.cpp

#include "sensor_commands.h"

namespace sensor_commands {
  const uint8_t MAX_CALLBACKS = 20;

  uint8_t callbacks_count = 0;
// A callback contains both a function and a pointer to arbitrary data
// that will be passed as argument to the function.
  struct Callback {
    Callback(const char *s = 0, void (*f)(void*) = 0, void *d = 0) :
        name(s), function(f), data(d) {
    }
    const char *name;
    void (*function)(void*);
    void *data;
  };

  Callback callbacks[MAX_CALLBACKS];

  void defineCallback(const char *n, void (*f)(void*), void *d) {
    if (callbacks_count < MAX_CALLBACKS) {
      callbacks[callbacks_count] = Callback(n, f, d);
      callbacks_count++;
    } else {
      Serial.println(F("Too many callbacks have been defined."));
    }
  }

  bool startsWith(const char *a, const char *b) {
    return !strncmp(a, b, strlen(b));
  }

  bool parseCommand(const char *c, char *fname, long *param) {
    return 0;
  }

  void run(const char *c) {
    char command[30];
    char *function_name;
    long parameter;
    parseCommand(c, function_name, &parameter);
    strlcpy(command, c, 30);
    Serial.print(F("Received : '"));
    Serial.print(command);
    Serial.println("'");
    char *arg;
    function_name = strtok(command, " ");
    if (!function_name) {
      // Empty string
      return;
    }
    arg = strtok(NULL, " ");
    if (arg) {
      char *end;
      parameter = strtol(arg, &end, 10);
      if (*end) {
        // Second argument isn't a number
        parameter = -9999;
      }
    } else {
      parameter = -9999;
    }
    Serial.print("Command : '");
    Serial.print(function_name);
    Serial.print("'. Parameter : ");
    Serial.println(parameter);

    // Test all the callbacks.
    for (uint8_t i = 0; i < callbacks_count; i++) {
      if (startsWith(command, callbacks[i].name)) {
        Serial.print("OHHHH YES!!!");
      }
      Serial.print("Trying '");
      Serial.print(callbacks[i].name);
      Serial.println("'");
      callbacks[i].function(callbacks[i].data);
    }
    Serial.println("Done.");
  }
}