#include <BlockNot.h>
#include <NewPing.h>
const byte diagnosticMode = HIGH;

// high/far and low/near threshold distances, in cm
const int highThreshold = 80;
const int lowThreshold = 10;

// Set pins
const int echoPin = 2;
const int triggerPin = 3;
// Switch must be a PWM-enabled pin
const int switchPin = 5;

NewPing sonar(triggerPin, echoPin, 200);

int blinkCounter = 1;

// Set event timings
// Duration, in milliseconds, between fade-in/out steps
const int fadeSpeed = 20;
// Duration, in seconds, of lights-on
BlockNot lightsOnTimer(diagnosticMode ? 20 : 60, SECONDS);
// Duration, in milliseconds, between motion sensor checks
BlockNot motionCheckTimer(500);
// Duration, in milliseconds, of onboard LED blink
BlockNot blinkTimer(50);

byte blinkState = LOW;
byte lightSwitchState = LOW;
byte motionDetected = LOW;

/*
 * Detecting moving objects with HC-SR04 ultrasonic sensor
 * 
 * From https://www.industrialshields.com/blog/arduino-industrial-1/detecting-moving-objects-with-hc-sr04-ultrasonic-sensor-471
 * 
 * Simulation here: https://wokwi.com/projects/386238353288923137
 * 
 * To do:
 * - Replace motion check and light-on delay() with millis for nonblocking code [ DONE ]
 * - Add PWM soft on/off [ DONE ]
 * - Move motion state to function [ DONE ]
 * - Blink builtin LED for motion check status [ DONE ]
 * - Look at https://github.com/EasyG0ing1/BlockNot library or
 *   https://arduino.stackexchange.com/questions/85003/avoid-using-delay-with-the-hr-s04-ultrasonic-sensor for nonblocking SR04 reading [ DONE ]
 * - Make fade in and fade out nonblocking
 * 
 */

void setup() {
    if (diagnosticMode) Serial.begin(9600);
    // initialize digital pin LED_BUILTIN as an output.
    pinMode(LED_BUILTIN, OUTPUT);
    digitalWrite(LED_BUILTIN, LOW);
    // Fade in, hold 3s, fade out, on powerup
    pinMode(switchPin, OUTPUT);
    for (int fadeValue = 0 ; fadeValue >= 255; fadeValue += 5) 
    {
      analogWrite(switchPin, fadeValue);
      delay(fadeSpeed);
    }
    delay(3000);
    for (int fadeValue = 255 ; fadeValue >= 0; fadeValue -= 3) 
    {
      analogWrite(switchPin, fadeValue);
      delay(fadeSpeed * 2);
    }
  }

void loop() {

  if (blinkTimer.TRIGGERED && blinkState) 
  {
    blinkState = LOW;
    if (diagnosticMode) Serial.print("Blink stop, turning LED "); Serial.println(digitalRead(LED_BUILTIN) ? "off" : "on");
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }
  // is it time to do a motion check?
  if (motionCheckTimer.TRIGGERED)
  {
    blinkCounter++;
    if (blinkCounter > 10) 
    {
      if (diagnosticMode) Serial.print("Blink start, turning LED "); Serial.println(digitalRead(LED_BUILTIN) ? "off" : "on");
      digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
      blinkTimer.RESET;
      blinkState = HIGH;
      blinkCounter = 1;
    }

    sonar.ping_timer(echoCheck);
    // Motion detected?
    if (motionDetected) 
    {
      if (lightSwitchState == LOW) 
      {
        if (diagnosticMode) Serial.print("Turning on light for "); Serial.print(diagnosticMode ? 20 : 60); Serial.println(" seconds");
        lightSwitchState = HIGH;
        digitalWrite(LED_BUILTIN, HIGH);
        fadeIn(switchPin, 0);    
      }
    }
  }
    
  if (lightSwitchState)
  {
    if (lightsOnTimer.TRIGGERED && !motionDetected)
    {
      if (diagnosticMode) Serial.println("Light is on, and time is up, turning lights off.");
      lightSwitchState = LOW;
      digitalWrite(LED_BUILTIN, LOW);
      fadeOut(switchPin);          
    }
  }
}

void fadeIn(int Pin, int startPoint) {
  if (startPoint > 255) startPoint = 255;
  if (startPoint < 0) startPoint = 0;
  for (int fadeValue = startPoint ; fadeValue <= 255; fadeValue += 5) 
  {
    analogWrite(Pin, fadeValue);
    // wait for fadeSpeed (20-30) milliseconds to see the dimming effect
    delay(fadeSpeed);
  }
}

void fadeOut(int Pin) {
  for (int fadeValue = 255 ; fadeValue >= 0; fadeValue -= 3) 
  {
    analogWrite(Pin, fadeValue);
    // wait for fadeSpeed (20-30) * 2 milliseconds (fade slowly) to see the dimming effect
    delay(fadeSpeed * 2);
    // check for motion during fadeout
    // if motion, turn on light and break out of loop
    sonar.ping_timer(echoCheck);
    if (motionDetected)
    {
        lightSwitchState = HIGH;
        digitalWrite(LED_BUILTIN, HIGH);
        fadeIn(switchPin, fadeValue);
        // reset the lights-on timer, break out of the fadeOut loop
        lightsOnTimer.RESET;
        break;     
    }
  }
}

void echoCheck() { // Timer2 interrupt calls this function every 24uS where you can check the ping status.
  // Don't do anything here!
  if (sonar.check_timer()) { // This is how you check to see if the ping was received.
    // Ping returned, uS result in ping_result, convert to cm with US_ROUNDTRIP_CM.
    int distance = sonar.ping_result / US_ROUNDTRIP_CM;
    if (diagnosticMode) Serial.print("Ping: "); Serial.print(distance); Serial.println("cm");
    if (distance >= lowThreshold and distance <= highThreshold) 
    {
      motionDetected = HIGH;
    } else {
      motionDetected = LOW;
    }
  }
  // Don't do anything here!
}