/*
 ******************************************************************************
 *  Copyright (c) 2015 Particle Industries, Inc.  All rights reserved.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation, either
 * version 3 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, see <http://www.gnu.org/licenses/>.
 ******************************************************************************
 */
/* HC-SR04 Ping / Range finder wiring:
 * -----------------------------------
 * Particle - HC-SR04
 *      GND - GND
 *      VIN - VCC
 *       D2 - TRIG
 *       D6 - ECHO
 */
#include "application.h"
int candyset = 0;
Servo myservo;// create servo object using the built-in Particle Servo Library
int servoPin = D0;
int handler(const char *event, const char *data)
{
  myservo.attach(servoPin);
  myservo.write(90);
  delay(1000);
  myservo.detach();
  return 1;
  myservo.detach();
}
void setup() {
    Serial.begin(9600);
    Particle.subscribe("candy2", handler);
    myservo.attach(D0);      //Initialize the servo attached to pin D0
    myservo.write(0);        //set servo to initial position
    delay(500);
    myservo.detach();
}
void loop() {
    // Trigger pin, Echo pin, delay (ms), visual=true|info=false
    ping(D2, D6, 20, true);
}
void ping(pin_t trig_pin, pin_t echo_pin, uint32_t wait, bool info)
{
    uint32_t duration, inches, cm;
    static bool init = false;
    if (!init) {
        pinMode(trig_pin, OUTPUT);
        digitalWriteFast(trig_pin, LOW);
        pinMode(echo_pin, INPUT);
        delay(50);
        init = true;
    }
    /* Trigger the sensor by sending a HIGH pulse of 10 or more microseconds */
    digitalWriteFast(trig_pin, HIGH);
    delayMicroseconds(10);
    digitalWriteFast(trig_pin, LOW);
    duration = pulseIn(echo_pin, HIGH);
    /* Convert the time into a distance */
    // Sound travels at 1130 ft/s (73.746 us/inch)
    // or 340 m/s (29 us/cm), out and back so divide by 2
    // Ref: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
    inches = duration / 74 / 2;
    cm = duration / 29 / 2;
    if (inches <= 5)
    {
      if (candyset==0)
      {
        Particle.publish("candy1","yeah!");
        candyset = 1;
      }
    }
    else if (inches>5 && inches <100)
    {
      candyset = 0;
    }
    if (info) { /* Visual Output */
        Serial.printf("%2d", inches);
        Serial.println();
    } else { /* Informational Output */
        Serial.printlnf("%6d in / %6d cm / %6d us", inches, cm, duration);
    }
    delay(wait); // slow down the output
}