Kitchen SparkCore to MQTT bridge

Spark_core_mqtt.ino

// This #include statement was automatically added by the Spark IDE.
#include "MQTT/MQTT.h"

// Define the pins we're going to call pinMode on
const int led_red = A5;  
const int led_green = A4; // This one is the built-in tiny one to the right of the USB jack
const int led_blue = A6;

const int number_of_inputs = 4;
int inputs[] = { D1, D2, D3, D4};

bool switch_pressed = false;
int switch_value = 0;
unsigned long last_pressed_time = millis();

void callback(char* topic, byte* payload, unsigned int length);
byte server[] = { 192, 168, 1, 107 }; // MQTT server address
MQTT client(server, 1883, callback);

// recieve message
void callback(char* topic, byte* payload, unsigned int length) {
    char p[length + 1];
    memcpy(p, payload, length);
    p[length] = NULL;
    String message(p);
    
    

    if (message.equals("RED"))    
        RGB.color(255, 0, 0);
    else if (message.equals("GREEN"))    
        RGB.color(0, 255, 0);
    else if (message.equals("BLUE"))    
        RGB.color(0, 0, 255);
    else    
        RGB.color(255, 255, 255);
    delay(1000);
    
}

void read_switches() {
    int val;
    unsigned long this_pressed_time = millis();
    if ( this_pressed_time - last_pressed_time > 500 ) {
        switch_pressed = true;
        switch_value = 0;
        for ( int l = 0 ; l < 3 ; l++) {
            val = 0;
            // Have a couple of reads to avoid the bounce.
            for (int v = 0 ; v < 5 ; v ++) {
                val = val | digitalRead(inputs[l]);
            }
            switch_value = switch_value | ( val << l);
        }
    }
    last_pressed_time = this_pressed_time;
    
}

void setup() {
    
    pinMode(led_red, OUTPUT);
    pinMode(led_green, OUTPUT);
    pinMode(led_blue, OUTPUT);
    
    //Register our Spark function here
    Spark.function("rgb", set_rgb);

        // connect to the server
    client.connect("sparkclient");

    // publish/subscribe
    client.publish("/status/kitchen/core","hello world");
    client.subscribe("/open/kitchenlights");
    
    
    for ( int l = 0 ; l < 3 ; l++ ) {
        pinMode(inputs[l], INPUT);
        attachInterrupt(inputs[l], read_switches, RISING);
    }
}


void loop() {
    // If we started successfully, then wait for the client to connect
    // Run the mqtt loop code. It returns after doing it's stuff
    client.loop();
    if ( ! client.isConnected()) {
        client.connect("sparkclient");
        client.publish("/status/kitchen/core","hello world");
        client.subscribe("/open/kitchenlights");
    }
    if ( switch_pressed ) {
        char v[2];
        String str = String(switch_value);
        str.toCharArray(v, 2);
        client.publish("/status/kitchen/switch/value", v);
        client.publish("/status/kitchen/switch", "pressed");
        switch_pressed = false;
    }
}

int string_to_int(String input)
{
    //if (input.length() != 3) {
    //    return -1;
    //}
    int retval = 0;
    //for (int bit = 0; bit < 3; bit++ ) {
    //    retval = retval + atoi(input.getCharAt(bit)) * (( bit + 1 ) * 10);
    //}
    retval = input.toInt();
    return retval;
    
}
int set_rgb(String command)
{
  int redint;
  int blueint;
  int greenint;
  redint = string_to_int(command.substring(0,3));
  greenint = string_to_int(command.substring(3,6));
  blueint = string_to_int(command.substring(6));
  if (redint == -1 || blueint == -1 || greenint == -1) {
      return 1;
  }

  analogWrite(led_red, redint);
  analogWrite(led_green, greenint);
  analogWrite(led_blue, blueint);
  
  int len = command.length() + 1;
  char p[len];
  command.toCharArray(p, len);
  client.publish("/status/kitchen/colour", p);
  return 0;
}