/*
   TODO:  integrate safety switch
   funzies: add individual id tally/timestamp
   add daily locking schedule/routine
 

*/

#include <WiFi.h>
#include <ESPmDNS.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>
#include "time.h"

const char* ssid = "xxx";
const char* password = "xxx";
//begin door
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>
#include <analogWrite.h> //I'm not sure but I think this library simulates analogwrite for arduino IC because its not a command for ESP32
const int ENA_PIN = 21; // ESP32 pin GPIO21 (IO5) connected to the EN1 pin DRV8833
int SwitchPin = 23; //SPDT momentary cherry switch
int stopSwitch = 22;
const int IN1A_PIN = 17; // ESP32 pin GPIO17 connected to the IN1A pin DRV8833
const int IN2A_PIN = 16; // ESP32 pin GPIO16 connected to the IN2A pin DRV8833

const int ENB_PIN = 19; // ESP32 pin GPIO19 connected to the EN1 pin DRV8833
const int IN1B_PIN = 18; // ESP32 pin GPIO18 connected to the IN1B pin DRV8833
const int IN2B_PIN = 26; // ESP32 pin GPIO26 connected to the IN2B pin DRV8833
const char* ntpServer = "pool.ntp.org";
const long  gmtOffset_sec = -18000;
const int   daylightOffset_sec = 3600;
int Wyatt;
int Troub;

const unsigned long DOpenTimeout = 4000; //door open timeout
const unsigned long DCloseTimeout = 4000
                                    ; //door close timeout
unsigned long previousMillis1;
unsigned long previousMillis2;

int scanTime = 3; //In seconds
BLEScan* pBLEScan;

//String knownNames[] = { "Wyatt", "Trouble", "donkey", "cat", dd:33:16:00:02:08 is my beacon.};
String knownMACS[] = { "dd:33:16:00:02:08", "dd:33:16:00:02:47", "dd:33:0a:11:1a:a5", "dd:33:0a:11:1a:a6"};

class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    void onResult(BLEAdvertisedDevice advertisedDevice) {
      //Serial.printf("Advertised Device: %s \n", advertisedDevice.toString().c_str());
    }
};
//end door
void printLocalTime()
{
  struct tm timeinfo;
  if (!getLocalTime(&timeinfo)) {
    Serial.println("Failed to obtain time");
    return;
  }
  Serial.println(&timeinfo, "%A, %B %d %Y %H:%M:%S");
}
void setup() {
  {
    Serial.begin(115200);

    //connect to WiFi
    Serial.printf("Connecting to %s ", ssid);
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.print(".");
    }
    Serial.println(" CONNECTED");

    //init and get the time
    configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);
    printLocalTime();
  }
  //begin door setup
  pinMode(ENA_PIN, OUTPUT); //defines speed or on/off
  pinMode(IN1A_PIN, OUTPUT);
  pinMode(IN2A_PIN, OUTPUT);
  pinMode (LED_BUILTIN, OUTPUT); //initialize internal LED
  pinMode (SwitchPin, INPUT); //initialize switch limit
  pinMode(ENB_PIN, OUTPUT); //defines speed or on/off
  pinMode(IN1B_PIN, OUTPUT);
  pinMode(IN2B_PIN, OUTPUT);

  Serial.println("Scanning...");

  BLEDevice::init("");
  pBLEScan = BLEDevice::getScan(); //create new scan
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setActiveScan(true); //active scan uses more power, but get results faster
  pBLEScan->setInterval(100);
  pBLEScan->setWindow(99);  // less or equal setInterval value
  Serial.println("Scanning for these device MACs:");
  Serial.println(knownMACS[0].c_str());
  Serial.println(knownMACS[1].c_str());
  Serial.println(knownMACS[2].c_str());
  Serial.println(knownMACS[3].c_str());
  //end door setup

  Serial.println("Booting");
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  while (WiFi.waitForConnectResult() != WL_CONNECTED) {
    Serial.println("Connection Failed! Rebooting...");
    delay(5000);
    ESP.restart();
  }

  ArduinoOTA
  .onStart([]() {
    String type;
    if (ArduinoOTA.getCommand() == U_FLASH)
      type = "sketch";
    else // U_SPIFFS
      type = "filesystem";

    // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end()
    Serial.println("Start updating " + type);
  })
  .onEnd([]() {
    Serial.println("\nEnd");
  })
  .onProgress([](unsigned int progress, unsigned int total) {
    Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
  })
  .onError([](ota_error_t error) {
    Serial.printf("Error[%u]: ", error);
    if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed");
    else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed");
    else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed");
    else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed");
    else if (error == OTA_END_ERROR) Serial.println("End Failed");
  });

  ArduinoOTA.begin();

  Serial.println("Ready");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
}

void loop() {
  bool stopdoor = true;
  Serial.println ("Door switch status: ");
  Serial.print (digitalRead (stopSwitch)); //door is closed when value is 0
 while (stopdoor) {
   if (digitalRead (stopSwitch) == LOW) {Serial.println ("Waiting for Door to be closed"); Serial.println ("Switch condition:");Serial.print (digitalRead (stopSwitch));
      stopdoor = false;
    };  //meant to check if the main door is open, and pause program until its closed.
    bool detect = false;
    bool doormotor = true; //should these be TRUE?  Suspect this is associated with intial routine problem.
    bool doormotor2 = true;
    bool lock;
    bool lockSchedule;
    unsigned long currentMillis = millis();
    //begin door loop
    printLocalTime();
    digitalWrite (LED_BUILTIN, LOW); //default the internal LED to OFF
    Serial.println ("Switch Status:"); //show the switch status
    int SwitchState = digitalRead(SwitchPin);
    Serial.println (SwitchState);
    BLEScanResults foundDevices = pBLEScan->start(scanTime, false);
    Serial.print("Devices found: ");
    int deviceCount = foundDevices.getCount();
    Serial.println(deviceCount);

    //Serial.print("sizeof(knownMACS): ");
    //Serial.println(sizeof(knownMACS));
    //Serial.print("sizeof(knownMACS[0]: ");
    //Serial.println(sizeof(knownMACS[0]));

    for (uint32_t i = 0; i < deviceCount; i++)
  {
    BLEAdvertisedDevice device = foundDevices.getDevice(i);

      for (int j = 0; j < (sizeof(knownMACS) / sizeof(knownMACS[0])); j++)
      {
        //if (strcmp(device.getName().c_str(), knownMACS[j].c_str()) == 0)
        if (strcmp(device.getAddress().toString().c_str(), knownMACS[j].c_str()) == 0)
        {
          Serial.print("We found a device on the list of known MAC addresses; MAC is: ");

          Serial.print(device.getAddress().toString().c_str());
          Serial.print(". Device name is: ");
          digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on
          Serial.print(device.getName().c_str());
          int rssi = device.getRSSI();
          Serial.print(". RSSI: ");
          Serial.println(rssi);
          //here's where you put that motor code!
          Serial.println (" ");        Serial.println ("Dog Counter: ");
          Serial.print (++Wyatt);
          Serial.println (" ");
          detect = true;
          //unlock the door
   /*       
            Serial.println ("Retracting Deadbolt..");
            digitalWrite(IN1B_PIN, LOW); // Spin motor B clockwise & retract deadbolt
            digitalWrite(IN2B_PIN, HIGH);  // Spin motor B clockwise
            analogWrite(ENB_PIN, 255); // define the speed
            delay (4000); //wait X seconds while Motor B Spins
            digitalWrite(IN1B_PIN, LOW); // turn off motor
            digitalWrite(IN2B_PIN, LOW);  // turn off motor
            Serial.println ("Deadbolt retracted, opening door");
          
*/
          //Open Door
          digitalWrite(IN1A_PIN, HIGH); // Spin motor A clockwise
          digitalWrite(IN2A_PIN, LOW);  // Spin motor A clockwise
          analogWrite(ENA_PIN, 255); // define the speed
          delay (3000); //wait X seconds while Motor A Spins & ignores the switch
          Serial.println ("Turn on the Door motor for delay, and then for DOpenTimeout.");
          currentMillis = millis();
          previousMillis1 = currentMillis;
          while (doormotor)
          {
            currentMillis = millis();
            if (currentMillis - previousMillis1 >= DOpenTimeout) {
              doormotor = false;
            };
            if (digitalRead(SwitchPin) == HIGH) {
              doormotor = false;
            };
            Serial.println (currentMillis - previousMillis1);
            Serial.println ("inside the OPEN while loop!");

          }
          delay (1500); //add to switch response time to trim end posiion.
          Serial.println ("Next, turn off the door motor.");

          digitalWrite(IN1A_PIN, LOW); // turn off motor
          digitalWrite(IN2A_PIN, LOW);  // turn off motor


          // wait for dog, then initiate close
          delay(15000);  //dog transit period


          //CLOSE DOOR
          digitalWrite(IN1A_PIN, LOW); // Spin motor A clockwise
          digitalWrite(IN2A_PIN, HIGH);  // Spin motor A clockwise
          analogWrite(ENA_PIN, 255); // define the speed
          delay (3000
                ); //wait X seconds while Motor A Spins and ignores the switch
          Serial.println ("Turn on the Door motor for delay, and then for DCloseTimeout.");
          currentMillis = millis();
          previousMillis2 = currentMillis;
          while (doormotor2)
          {
            currentMillis = millis();
            if (currentMillis - previousMillis2 >= DCloseTimeout) {
              doormotor2 = false;
            };
            if (digitalRead(SwitchPin) == HIGH) {
              doormotor2 = false;
            };
            Serial.println (currentMillis - previousMillis2);
            Serial.println ("inside CLOSE while!");

          }
          delay (2000); //add to switch response time to trim end posiion.
          Serial.println ("Next, turn off the door motor.");

          digitalWrite(IN1A_PIN, LOW); // turn off motor
          digitalWrite(IN2A_PIN, LOW);  // turn off motor


     /*     
              //relock dog door
            Serial.println ("Extending Lock pin..");
            digitalWrite(IN1B_PIN, HIGH);   // Spin motor B anti-clockwise
            digitalWrite(IN2B_PIN, LOW);  // Spin motor B  anti-clockwise
            delay(4000); // wait X seconds while motor spins
            digitalWrite(IN1B_PIN, LOW); // Stop the motor and wait for a new tag sense.
            digitalWrite(IN2B_PIN, LOW);
            Serial.println ("Lock Extended.");
   */       
        }
      }
    }
    pBLEScan->clearResults();   // delete results fromBLEScan buffer to release memory
    delay(200);
    //end door loop
    ArduinoOTA.handle();
}
}//this needs another curly if we want to include that doorstop switch.