// PESKY FLAW! Filename is not terryPin.ino, it is ServoCUBEvideosMS—4.ino
// 328 storing this sketch is off UNO, on independent veroboard so made some changes
// Yellow LED (blue in wokwi) is now lit from D13, not D7
// Kept the serial output for testing
// Need to be able to test with shorter KA interval, so added physical switch 
// D4 input from switch sets KA interval to 5s (for test) or 160s in normal use
// Takes 'keep alive' (KA) photo with one servo press every 2m40s (160s)
// The low-going leading EDGE of the 20s outputfrom motion sensor is D6 input.
// That's the major change from earlier LED emulation version
// It causes two servo presses on CUBE button, starting a video recording
// (and lighting red LED as an indicator)

#include <VarSpeedServo.h> // Uses the VarSpeedServo library
VarSpeedServo cubeservo;

// Constants
const byte triggerPin = 6; // Low input from button to D6
const byte yellowLEDPin = A0; // LED flashes at intervals for KA photo
const byte redLEDPin = A1; // Lit while video recording
const byte servoOutPos = 30; // Trial & error
const byte servoInPos = 10; // Trial & error
const byte switchPin = 4; // For easier testing when on Veroboard not UNO
const int waitBeforeRelease = 200;
const int waitAfterRelease = 200;

// Variables
int triggerCounter = 0; // Counts number of triggers (MS lows)
int KACounter = 0; // Counts number of KA photos
bool trigState = HIGH;
bool prevtrigState = HIGH;
unsigned long previousMillis = 0; // Effectively the program start time
unsigned long interval; // 5s (testing) or 160s (2m40) KA photos interval

void setup()
{
  pinMode(switchPin, INPUT_PULLUP);
  pinMode(triggerPin, INPUT_PULLUP); // D6
  pinMode(redLEDPin, OUTPUT); // D8
  pinMode(yellowLEDPin, OUTPUT); // D13
  Serial.begin(115200);
  Serial.println("ServoCUBEvideosMS-4");
  Serial.println("");
  cubeservo.attach(9); // CUBE servo (white wire)
  cubeservo.write(servoOutPos);
  delay(500);

  // Test D4 to select value of interval
  if (digitalRead(switchPin) == HIGH)
  {
    interval = 160000; // 2m40s
  }
  else
  {
    interval = 5000; // 5s
  }
//    while(1 == 1); // For testing. Stops program entering loop
}

void loop()
{
  unsigned long currentMillis = millis(); // Get current time
  // Input to D6 is from a motion sensor (MS), which is normally H
  // It goes L when motion detected, returning H about 18-20s later.
  // Get the current input state
  trigState = digitalRead(triggerPin);
  // Compare with its previous state
  if (trigState != prevtrigState)
  {
    if (trigState == LOW) // Implies this is the low-going edge we want
    {
      triggerCounter++;
      Serial.print("triggerCounter = ");
      Serial.println(triggerCounter);
      // NOW TAKE ALL THE ACTIONS NEEDED AFTER TRIGGERING
      Serial.print("Started video ");
      Serial.println(triggerCounter);

       digitalWrite(redLEDPin, HIGH); // Indicates recording in progress

     // Move servo arm in/out twice to press CUBE button twice
      singleCUBEServoPress(); //Single short sweep in and out
      singleCUBEServoPress(); //Single short sweep in and out

      //Stays high during video recording, i.e for 30s
      delay(8000); // 8s for testing
      // delay(30000); // 30s video
      digitalWrite(redLEDPin, LOW); // Stays low until next video record

      // Now end video recording with a single CUBE button press
      singleCUBEServoPress(); //Single short sweep in and out
      Serial.print("Ended video ");
      Serial.println(triggerCounter);

      previousMillis = millis(); // Reset, as KA not needed for 160s (5s test)

    }
    // Nothing needed otherwise, i.e no else() required here??
    // But will include for reporting at least for now
      // Save current state for next time through the loop
    prevtrigState = trigState;
  }
  //  else
  //  {
  //    // if the current state is HIGH then the button went from L to H:
  //    Serial.println("Input went from L to H");
  //  }

  // Brief delay (as seen in others' sketches), although could probably
  // be even shorter (1ms for stability?) as there is no bouncing involved
  delay(100);

  // End of trigger detection and actions

  /////////////////////////////////////////////////////////////////////

  // CUBE camera inactivity for about 3 mins powers it off. So must take
  // 'keep alive' (KA) photo before this
  // Check to see if it's time for a KA photo

// much time may have passed due to the video , so we need a new idea of what time it is
  currentMillis = millis();
  
  if (currentMillis - previousMillis >= interval)
  {
    digitalWrite(yellowLEDPin, HIGH); // (was blue)
    
  // Move servo arm in/out once to press CUBE button once
    singleCUBEServoPress(); //Single short sweep in and out - function below

//    delay(100); LED on during servo tap
    digitalWrite(yellowLEDPin, LOW);
    
    KACounter++; // Increment
    Serial.print("KA photo ");
    Serial.println(KACounter);
    Serial.println("");
    previousMillis = currentMillis; // Reset
  } // End of KA code

} // End of Void Loop

void singleCUBEServoPress()
{
  cubeservo.write(servoInPos); // Rotates to the 'In' position
  delay(waitBeforeRelease); // Duration of press; was 200
  cubeservo.write(servoOutPos); // Rotates to the 'Out' position
  delay(waitAfterRelease); // Duration of release/recovery; was 200
}