#include <stdio.h>
#include <string.h>
#include <math.h>

int TILTPIN = D0;
int servoPin = A5;
int servoPos = 180;
Servo myServo;
int oldVal = 0;

int lastTimeOfOccurence = 0;
String deviceID = "";
int iStartedIt = 0;
int wobbled = 0;

void setup() {
  //  Servo
  myServo.attach( A5 );
  deviceID = System.deviceID();
  // Initialize the LED pin as an output
  pinMode(TILTPIN, INPUT);
  oldVal = analogRead(TILTPIN);
  lastTimeOfOccurence = Time.now();

  Particle.subscribe("iot-bobble-head", bobbleMe);

  Serial.begin(9600);
}

void loop() {
  int tiltData = analogRead(TILTPIN);

  if(abs(tiltData - oldVal) > 150 && wobbled == 0 && iStartedIt != 1) {
    Serial.println("Yes");
    Particle.publish("iot-bobble-head", deviceID);
    delay(5000);
    wobbled = 0;
    iStartedIt = 0;
  }
  oldVal = tiltData;
}

void bobbleMe(const char *event, const char *data) {
  String incomingDeviceId = (String)data;
  if( incomingDeviceId != deviceID && iStartedIt != 1) {
    wobbled = 1;
    iStartedIt = 0;
    bobble();
  } else {
    iStartedIt = 1;
    wobbled = 1;
    Serial.println("It's me");
  }
}
void bobble() {
  servoControl("10");
  delay(400);
  servoControl("90");
  delay(400);
  servoControl("10");
  delay(400);
  servoControl("90");
  delay(400);
  servoControl("10");
  delay(400);
  servoControl("90");
}
int servoControl(String command) {
  // Convert
  int newPos = command.toInt();
  // Make sure it is in the right range and set the position
  servoPos = constrain( newPos, 0 , 360);
  // Set the servo
  myServo.write( servoPos );
  return 1;
}