#include "sensor_commands.h"

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

  Callback callbacks[3];

// This callback expects an int.
  void print_int(void *data) {
    int parameter = *(int*) data;
    Serial.println(parameter);
  }

// This one expects a C string.
  void print_string(void *data) {
    char *parameter = (char*) data;
    Serial.println(parameter);
  }
  void initialize() {
    // Register callbacks.
    static int seventeen = 17;
    static int forty_two = 42;
    static char hello[] = "Hello, World!";
    callbacks[0] = Callback(print_int, &seventeen);
    callbacks[1] = Callback(print_int, &forty_two);
    callbacks[2] = Callback(print_string, hello);
  }

  void run() {
    // Test all the callbacks.
    for (size_t i = 0; i < 3; i++) {
      callbacks[i].function(callbacks[i].data);
    }
  }
}