#include <neopixel.h>

#define echoPin D4 // attach pin D2 Arduino to pin Echo of HC-SR04
#define trigPin D3 //attach pin D3 Arduino to pin Trig of HC-SR04
#define PIXEL_PIN D5
#define PIXEL_COUNT 8
#define PIXEL_TYPE WS2812
Adafruit_NeoPixel strip = Adafruit_NeoPixel(PIXEL_COUNT, PIXEL_PIN, PIXEL_TYPE);
int servoPin = A1;
int buttonPin = D2;
Servo myServo;

// variables about ultrasonic sensor distance
long duration; 
int distance; 

long focusTime;
bool isFocusing;
int buttonValue;
// for interval detection
unsigned long startTime;
unsigned long deltaTime;

long keepFocusThres = 10000; // one minute, 10s for testing
long notFocusThres = 10000; // one minute, 10s for testing
long interruptFocesThres = 10000; // 30 seconds, 10s for testing
long loopDelayTime = 2500; 
int openDegree = 20; // open/close degree every time
int servoDegree;
int focusCounter;
int notFocusCounter;
unsigned long lastDetect = 0;
unsigned long now;

void setup() {
    pinMode(trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
    pinMode(echoPin, INPUT); // Sets the echoPin as an INPUT
    myServo.attach( A1 );
    pinMode( buttonPin , INPUT_PULLUP);
    strip.begin();
    strip.show(); // Initialize all pixels to 'off'
    Serial.begin(9600); // Serial Communication is starting with 9600 of baudrate speed
    isFocusing = false;
    focusTime = 0;
    focusCounter = 0;
    notFocusCounter = 0;
    servoDegree = 125;
    myServo.write( 90 );
}

void loop() {
    buttonValue = digitalRead( buttonPin );
    
    // button pushed, trigger LED
    if( buttonValue == LOW ){
        Particle.publish("remoteCom");
    }
    // detect every "loopDelayTime" ms
    now = millis();
    if (now-lastDetect<loopDelayTime){
        return;
    }
    lastDetect = now;
    // Clears the trigPin condition
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration = pulseIn(echoPin, HIGH);
    // Calculating the distance
    distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
    // Displays the distance on the Serial Monitor
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");
    if (distance<=60) {
        // is focsing
        if(!isFocusing) {
            isFocusing = true;
            focusCounter = 1;
            Serial.print("focus Counter: ");
            Serial.print(focusCounter);
        }
        else {
            focusCounter++;
            Serial.print("focus Counter: ");
            Serial.print(focusCounter);
            if (focusCounter>=keepFocusThres/loopDelayTime) {
                Serial.print("focus open five degree ");
                // open the flower for five degree
                servoDegree -= openDegree;
                servoDegree = constrain( servoDegree, 0 , 125);
                Serial.print(servoDegree);
                Serial.print("");
                myServo.write( servoDegree );
                Particle.publish("openFlower", String(servoDegree));
                // clear counter
                focusCounter = 0;
                isFocusing = false;
            }
        }
    }
    else {
        if (notFocusCounter>0) {
            notFocusCounter++;
            if (notFocusCounter >= interruptFocesThres/loopDelayTime) {
                // interrupt the counting time for focusing time
                isFocusing = false;
            }
            if (notFocusCounter >= notFocusThres/loopDelayTime) {
                Serial.print("not focus: close five degree ");
                // close the flower for five degree
                servoDegree += openDegree;
                servoDegree = constrain( servoDegree, 0 , 125);
                Serial.print(servoDegree);
                Serial.print("");
                myServo.write( servoDegree );
                Particle.publish("openFlower", String(servoDegree));
                // clear counter
                notFocusCounter = 0;
                isFocusing = false;
            }
        }
        else {
            notFocusCounter = 1;
        }
    }
}