/* Crawler Slave
 *  
 *  This code runs on the crawler i2c network
 *  and provides a cleaner, less CPU intensive control over PWM devices.
 */

#include <Wire.h>
#include <Servo.h>

// This servo is used to wipe and clean the camera lens
Servo windowWiperServo;

// This is a variable capable of storing up to 4 items
char instruction[32] = "";

// Values for the id, and raw value of an instruction
char * id;
char * raw_value;

void setup() {
  // Attach the wiper servo to pin 9
  windowWiperServo.attach(9);

  // This is only useful for debugging
  //Serial.begin(9600);

  // This is the address the pi will speak to us at
  Wire.begin(0x4);
  
  // Call receiveEvent when data received                
  Wire.onReceive(receiveEvent);

  //Serial.println("Finished Setup.");  
}

// Just loop to keep the running code alive, and wait for events to happen.
void loop() {
  delay(50);
}

// This method runs when we receive a message
void receiveEvent(int n) {

  for (int i = 0; i < n; i++) {
    instruction[i] = Wire.read();
    instruction[i + 1] = '\0'; //add null after ea. char
  }

  //RPi first byte is cmd byte so shift everything to the left 1 pos so temp contains our string
  for (int i = 0; i < n; ++i)
    instruction[i] = instruction[i + 1];

  id = strtok (instruction, "-");
  raw_value = strtok (NULL, "-");

  int value = atoi(raw_value);

  //Serial.println(id);
  //Serial.println(value);

  // Switch statements dont work on char *
  if (strcmp(id,"WIPE") == 0) // if id, and WIPE cancel out
  {
    windowWiperServo.write(value);
  }
}