#include <SPI.h>
#include <SD.h>
#include <Servo.h>

/*
----------------- Physical Wiring---------------------

**Servo Motor to Arduino Nano**
Signal(orange or yellow): nano D9
Power (red): nano 5V
Ground (black) : nano GND

**SD Card Module to Arduino Nano**
MOSI(DI) : nano D11
MISO(DO) : Connect to D12
SCK : nano D13
CS: nano D10
VCC : nano 3.3V
GND: nano GND

**Joystick to Arduino Nano**
joystick1 HORZ : nano A0
joystick1 SEL : nano D2
VCC: Connect to 5V
GND: Connect to GND


~~~~~ General Instructions ~~~~~
°°° Setup °°°
After wiring, connect your Arduino Nano to your computer and open the Arduino IDE. Open the Serial Monitor and set the baud rate to 9600 bps.

°°° Direct Servo Control °°°
Enter a value between 0 and 180 in the Serial Monitor and press Enter to move the servo to that position.

°°° Playing Sequences °°°
Use the command 'play <filename.seq>' to play back a sequence. For example, play example.seq

°°° Listing Files °°°
Send list to display all .seq files stored on the SD card.

°°° For Help °°°
Send 'help' to see a list of available commands.

°°° Joystick Mode °°°
Press the joystick button once to toggle in and out of joystick control mode. Move the joystick left/right to control the servo in this mode. Press the button again to exit joystick mode.


°°° Creating Your Own Position Sequence File °°°
* File Format: Using a pc text editor prepare a text file with each line formatted as "position,delay", where "position" is a servo angle (0-180) and "delay" is the delay in milliseconds before moving to the next position.
* Saving: Save the file with a .seq extension on the SD card.
** you can Open the 'example.seq' file to see exactly how to format.

°°° Playback°°°
 Use the command play yourFileName.seq to play it back.


~~~~~ Additional Tips ~~~~~
SD Card Initialization:
 Upon startup, the system attempts to initialize the SD card and create an example sequence file (example.seq) if it does not already exist.

Joystick Mode: 
The joystick allows for intuitive control over the servo's position. The mode can be changed between "pan" (default) and "track" using Serial commands or the joystick button.

Sequence Files:
 .seq files are text files that can be edited with any text editor. They are used to store sequences of positions and delays for the servo motor.

~~~~~ Troubleshooting ~~~~~
SD Card Not Detected: 
Ensure the card is correctly inserted and formatted as FAT16 or FAT32. Try reinserting the card or testing with another card if issues persist.

Servo Not Responding: 
Check the wiring and ensure the servo is correctly powered. Verify the control wire is connected to the specified digital pin on the Arduino Nano.

Joystick Not Controlling Servo: 
Ensure the joystick's wiring is correct and it is calibrated if necessary. Pressing the joystick button toggles the control mode.
*/

Servo myservo;
int minPosition = 0; // Adjusted minimum servo position to prevent overshooting
int maxPosition = 180; // Adjusted maximum servo position to prevent overshooting
bool isInverted = true; // Servo diection, set to 'true' or 'false' to change rotation direction.
const int initialServoPosition = 90; // Example initial position
const int chipSelect = 10; // SD card module CS pin
const int servoPin = 9; // Servo control pin

// Variables for smoother pan mode operation
const long smoothUpdateInterval = 20; // Update interval for smooth movement (ms)
int targetPosition = initialServoPosition; // Target position initialized to the servo's initial position

// Variables for timing serial print information
unsigned long lastPrintTime = 0;  // Stores last print time
const long printInterval = 500;  // Interval at which to print the position (500ms)

// Define Variable for Timing Servo Movements
const long servoMoveInterval = 15;    // Interval at which to move the servo (15ms)

// Define a fixed pan speed for the servo *speed
const int servoPanSpeed = 2; // Adjust this value to set the pan speed (range 0-10)

// Joystick pins
const int joyXPin = A0; // Joystick X-axis
const int joyButtonPin = 2; // Joystick button pin

// Joystick modes
String joystickModeType = "pan"; // "track" or "pan"

// Tracks whether joystick mode is currently active
bool isJoystickModeActive = false; 

// Speed adjustment for pan mode
const int panSpeed = 1; 

// Time interval in milliseconds for pan mode updates
const int updateInterval = 100; 

// Define Variables for Button Debounce
static unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
const unsigned long debounceDelay = 50;    // the debounce time; increase if the output flickers

// Define a Variable for detecting mode changes
bool modeChanged = false; // Add this flag to detect mode changes

// Define a global variable to track the SD card initialization status
bool sdCardInitialized = false; // Tracks the SD card initialization status, defaulted to false upon startup


void setup() {
  Serial.begin(9600);
  while (!Serial); // Wait for serial port to connect
  
      for(int i = 0; i < 10; i++) {
    Serial.println();
  }
  //Serial.print(F("The system is booting up\n"));
  Serial.print(F("The system is booting up"));
  delay(250);
  Serial.print(F("."));
  delay(250);
  Serial.print(F("."));
  delay(250);
  Serial.print(F("."));
  delay(250);
  Serial.print(F("."));
  delay(250);
  Serial.println(F("."));
  Serial.println(F(""));

  // Initializing SD card
  Serial.print(F("-Initializing SD card"));
  delay(250);
  Serial.print(F("."));
  delay(250);
  Serial.print(F("."));
  delay(250);
  Serial.print(F("."));
  delay(250);
  Serial.print(F("."));
  delay(250);
  Serial.println(F("."));
  initializeSDCard(); // Use the new function for initializing the SD card
  
  if (!SD.begin(chipSelect)) {
    Serial.println(F("SD Card Initialization failed! *Please check SD card"));
    sdCardInitialized = false; // SD card failed to initialize
  
  } else {
    //Serial.println(F("-SD Card Initialization complete."));
    sdCardInitialized = true; // SD card successfully initialized
    createExampleFile(); // Ensure this is called here after confirming SD card initialization
    delay(1000); // Wait  before proceeding to the next step
  }

  // Now attach the servo and move it to the initial position after the SD card initialization
  myservo.attach(servoPin);
  Serial.println(F("\n-Initializing servo motor to position: 90"));
  delay(500); // 
  myservo.write(initialServoPosition); // Move servo to the initial position
  delay(650); // Wait to simulate the motor initialization time
  Serial.println(F("-motor initialization complete."));
  delay(750); // Wait for 2 seconds to simulate the motor initialization time

  // Set joystick mode to pan
  joystickModeType = "pan"; // Assuming you want to start in 'pan' mode
  Serial.print(F("\nJoystick mode set to: pan      "));
  delay(100);
  Serial.print(F("*"));
  delay(100);
  Serial.print(F("*"));
  delay(100);
  Serial.print(F("*"));
  delay(100);
  Serial.print(F(" Send "));
  delay(100);
  Serial.print(F("help "));
  delay(100);
  Serial.print(F("for "));
  delay(100);
  Serial.print(F("available "));
  delay(100);
  Serial.print(F("commands "));
  Serial.println();
  pinMode(joyButtonPin, INPUT_PULLUP); // Enable internal pull-up resistor for the button
  pinMode(joyXPin, INPUT); // X-axis as input
}

void createExampleFile() {
  // Ensure SD card is initialized
  if (!sdCardInitialized) {
    Serial.println(F("SD card not initialized. Cannot create example file."));
    return;
  }

  // Define the file name with .seq extension
  const char* exampleFileName = "example.seq";

  // Check if the example file already exists
  if (SD.exists(exampleFileName)) {
    return; // File already exists, no need to create it again
  }

  // Create the file on the SD card
  File file = SD.open(exampleFileName, FILE_WRITE);
  if (file) {
    Serial.println(F(" 'Creating example.seq' "));
    // Write the sequence data to the file
    const char* lines[] = {
      "180,350", "0,350", "170,350", "10,350",
      "160,350", "20,350", "150,350", "30,350",
      // Add more lines as needed
    };
    for (unsigned int i = 0; i < sizeof(lines) / sizeof(lines[0]); i++) {
      file.println(lines[i]);
    }
    file.close();
    Serial.println(F("example.seq created with sequence data."));
  } else {
    Serial.println(F("Failed to create example.seq."));
  }
}


void showHelp() {
  Serial.println();
  Serial.println(F("--- Help Menu ---"));
  Serial.println(F("* Send 'help': To display this list of commands."));
  Serial.println(F("* Send 'play <filename.seq>': To play a servo position sequence file. e.g. Send 'example.seq' ")); 
  Serial.println(F("* Send 'list': List all '.seq' files on the SD card."));
  Serial.println(F("* Send '<position>': Move servo to a specific position (0-180)."));
  Serial.println(F("* Send 'joy track': Set joystick mode to 'track'."));
  Serial.println(F("* Send 'joy pan': Set joystick mode to 'pan'."));
  Serial.println(F("* Click joystick button to toggle joystick control mode."));
  Serial.print(F("* Current joystick mode: "));
  Serial.println(joystickModeType); // Use joystickModeType here
}

// Implement initializeSDCard Function
void initializeSDCard() {
  if (SD.begin(chipSelect)) {
    Serial.println(F("SD Card initialized successfully."));
    sdCardInitialized = true;

    // Immediately check and create the example file if it doesn't exist
    createExampleFile();
  } else {
    sdCardInitialized = false;
  }
}

void listFiles() {
  Serial.println();
  delay(1000); // Delay for stability, might not be necessary
  if (!SD.begin(chipSelect)) {
    Serial.println(F("SD Card Initialization failed!"));
    // Reinitialize SD if needed
    initializeSDCard();
  }

  if (sdCardInitialized) {
    File root = SD.open("/");
    bool found = false; // Flag to indicate if any files were found
    Serial.println(F("SD card *sequence files*"));
    while (true) {
      File entry = root.openNextFile();
      if (!entry) {
        break; // No more files
      }
      String fileName  = entry.name();
      entry.close();

      // Check if the file name ends with .seq
      if (fileName.endsWith(".seq") || fileName.endsWith(".SEQ")) {
      //if (fileName .endsWith(".seq")) {
        found = true;
        Serial.println(fileName );
      }
    }
    root.close();

    if (!found) {
      Serial.println(F("No position sequence *.SEQ* files found on SD card!"));
      createExampleFile();  // Recreate the example file if no files are found
    } 
  }
 
    else {
    Serial.println(F("---> Verify SD card is installed then reboot the arduino."));
  }
}

void playbackPositionsFromFile(String filename) {
  if (!sdCardInitialized || !SD.begin(chipSelect)) {
    Serial.println(F("SD card is not initialized, please insert an SD card then reset the Arduino."));
    return;
  }

  if (!SD.exists(filename)) {
    // If the missing file is example.txt, attempt to recreate it
    if (filename.equals("example.txt")) {
      createExampleFile();
    } else {
      Serial.println();
      Serial.println(F("File not found. Use 'list' to see available files."));
      return;
    }
  }

  File file = SD.open(filename.c_str(), FILE_READ);
  if (!file) {
    Serial.println();
    Serial.println(F("Error opening file for playback - Check filename spelling."));
    Serial.println("* Send 'list' to view files on the SD card.  Send 'help' for a list of available commands.");
    return;
  }

  Serial.println();
  Serial.print(F("Starting sequence file: "));
  Serial.println(filename);
  while (file.available()) {
    String line = file.readStringUntil('\n');
    int commaIndex = line.indexOf(',');
    if (commaIndex != -1) {
      int position = line.substring(0, commaIndex).toInt();
      position = constrain(position, minPosition, maxPosition); // Apply constraint here
      int delayTime = line.substring(commaIndex + 1).toInt();
      myservo.write(position);
      Serial.print(F("Moved to position: "));
      Serial.println(position);
      delay(delayTime);
    }
  }
  
  Serial.print(F("Completed sequence file: "));    
  Serial.print(filename);
  Serial.println(F("      *Send 'help' for available commands."));
  file.close(); 
}

void loop() {
    static unsigned long lastUpdateTime = 0;
    static int stepSize = 1;
    static unsigned long buttonPressStartTime = 0;
    static bool buttonPressed = false;
    static bool longPressActionTaken = false;
    static bool lastButtonState = HIGH; // assuming button is unpressed to start

    unsigned long currentMillis = millis();

    if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n');
        input.trim();

        if (input.equalsIgnoreCase("help")) {
            showHelp();
        } else if (input.startsWith("play ")) {
            String filename = input.substring(5);
            playbackPositionsFromFile(filename);
        } else if (input.equalsIgnoreCase("list")) {
            listFiles();
        } else if (input.startsWith("joy ")) {
            if (input.endsWith("track")) {
                joystickModeType = "track";
                
                Serial.println(); // Ensure a newline for clear delineation
                Serial.println(F("Joystick mode set to 'track'."));
            } else if (input.endsWith("pan")) {
                joystickModeType = "pan";
                Serial.println(); // Ensure a newline for clear delineation
                Serial.println(F("Joystick mode set to 'pan'."));
                myservo.write(90); // Reset to center when switching to pan mode
            } else {
                Serial.println(F("Invalid joystick mode. Use 'joy track' or 'joy pan'."));
            }
        } else if (isDigit(input[0]) || (input[0] == '-' && isDigit(input[1]))) {
            int position = input.toInt();
            position = constrain(position, minPosition, maxPosition);
            myservo.write(position);
            Serial.print(F("Servo moved to position: "));
            Serial.println(position);
        } else {
            Serial.println();
            Serial.println(F("Unknown command."));
        }
    }

    // New button press handling logic
    bool readButtonState = digitalRead(joyButtonPin);

    // Check if button state has changed from the last reading
    if (readButtonState != lastButtonState) {
        // reset the debouncing timer
        lastDebounceTime = currentMillis;
    }

    if ((currentMillis - lastDebounceTime) > debounceDelay) {
        // whatever the readButtonState is at, it's been there for longer than the debounce delay, so take it as the actual current state

        // but we care about the button press event, so check if the current state is LOW (pressed)
        if (readButtonState == LOW && !buttonPressed) {
            buttonPressStartTime = currentMillis;
            buttonPressed = true;
            longPressActionTaken = false;
        } else if (readButtonState == HIGH && buttonPressed) {
            // Button was released, check if it was a short press
            if (!longPressActionTaken && (currentMillis - buttonPressStartTime < 1000)) { // Short press
                isJoystickModeActive = !isJoystickModeActive;
                Serial.println();
                Serial.println(isJoystickModeActive ? "Joystick mode ON." : "Joystick mode OFF -  Send 'help' to see a list of available commands.");
            }
            buttonPressed = false;
        }

        // Handle long press without waiting for button release
        if (buttonPressed && !longPressActionTaken && (currentMillis - buttonPressStartTime > 1000)) {
            // because the mode was changed, set a flag
            modeChanged = true;
            // Long press logic here
            joystickModeType = (joystickModeType == "pan") ? "track" : "pan";
            Serial.println();
            Serial.print("Switched to ");
            Serial.print(joystickModeType);
            Serial.println(" mode.");
            longPressActionTaken = true;
           // Optionally, here you could also set the targetPosition to the current servo position
           // to start the new mode from the current position
           targetPosition = myservo.read();
        }
    }

    lastButtonState = readButtonState; // save the reading. Next time through the loop, it'll be the lastButtonState
  

    if (isJoystickModeActive) {
        int joyXVal = analogRead(joyXPin); // Read the joystick X position
        if (joystickModeType == "track") {
        // Correctly handle inversion for both "track" and "pan" modes
        if (isInverted) {
            joyXVal = 1023 - joyXVal; // Invert joystick value
        }
            // Mapping directly the joystick value to servo position
            int servoPos = map(joyXVal, 0, 1023, minPosition, maxPosition);
            myservo.write(servoPos);
        }

// Inside the pan mode section of your loop()
if (isJoystickModeActive) {
    int joyXVal = analogRead(joyXPin); // Read the joystick X position

    if (joystickModeType == "track") {
        // Handle "track" mode
        if (isInverted) {
            joyXVal = 1023 - joyXVal; // Optionally invert joystick value for "track" mode
        }
        int servoPos = map(joyXVal, 0, 1023, minPosition, maxPosition);
        myservo.write(servoPos);
        // Optionally add logging or other handling specific to "track" mode
    } else if (joystickModeType == "pan") {
        // Handle "pan" mode
        unsigned long currentMillis = millis();
        if (currentMillis - lastUpdateTime >= smoothUpdateInterval) {
            int currentPosition = myservo.read();
            // Optionally invert joystick value for "pan" mode
            // Note: This inversion logic might be different or even omitted, depending on your specific needs
            if (isInverted) {
                joyXVal = 1023 - joyXVal;
            }
            int center = 512;
            int distance = abs(joyXVal - center);
            int deadZone = 10;
            if (distance > deadZone) {
                int directionMultiplier = (joyXVal > center) ? 1 : -1;
                // Adjust directionMultiplier if inversion is required differently from "track" mode
                targetPosition += directionMultiplier * stepSize;
                targetPosition = constrain(targetPosition, minPosition, maxPosition);
                myservo.write(targetPosition); // Move to the target position directly in "pan" mode
                lastUpdateTime = currentMillis;
            }
        }
        // Optionally add logging or other handling specific to "pan" mode
    }
    // Common code to execute after handling either mode, if any
}

// Print current position only when joystick mode is active
static unsigned long lastPrintTime = 0;
if (millis() - lastPrintTime >= printInterval) {
    Serial.print("Current position: ");
    Serial.print(myservo.read());
    // Updated to include joystick mode (pan/track) in the message
    Serial.print(" - joystick (");
    Serial.print(joystickModeType);
    Serial.println(") active, single click to EXIT, long press to CHANGE MODE");
    lastPrintTime = millis();
}

}
}