#include <FastLED.h>

#define ENCA 2 //White motor cable
#define ENCB 3 //Yellow motor cable
#define PWMpin 11 //PWN pin control to motor controller
#define DIRpin 10 //direction control cable to motor controller
#define MANUAL A5 //Raises flag when signal is low
#define ARM A3 //Arms the system for GPS control when signal is low
#define buzzerPIN 4 //Buzzer control pin
#define LED_PIN A2 //LED control pin
#define NUM_LEDS 3  //exactly what it is
#define HOMEpin 7 //Home switch pin
#define RXPin 8 //GPS com pins *TDX on GPS board goes to pin 8*
#define TXPin 9 //GPS com pins *RDX on GPS goes to pin 9*


#define LED_TYPE    WS2812B
#define COLOR_ORDER GRB


CRGB leds[NUM_LEDS]; //sets the LED array

void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
pinMode(buzzerPIN,OUTPUT);
digitalWrite(buzzerPIN,LOW);
pinMode(LED_PIN,OUTPUT);
  FastLED.addLeds<LED_TYPE, LED_PIN, COLOR_ORDER>(leds, NUM_LEDS);
  fill_solid(leds, NUM_LEDS, CRGB::White);
  FastLED.show();
  delay(2000); // Wait for 2 seconds
  fill_solid(leds, NUM_LEDS, CRGB::Black);
  FastLED.show();
  GoodBuzzStartUp();
  delay(2000); // Wait for 2 seconds
}

void loop() {
  // put your main code here, to run repeatedly:
unsigned long currentMillis = millis();
//noGpsSignalAnimation(currentMillis);
//manualOnAnimation();
//calibrationAnimation();
//flagUpAnimation(currentMillis);
//StandbyAnimation(currentMillis);
//bounceAnimation(currentMillis);
//heartbeatAnimation(currentMillis);
//systemOffAnimation();
loadingBarAnimation();
//ErrorBuzz();
//GoodBuzz();
//delay(20);
//shortBeep();
//twoLongBuzzes();
//delay(2000);
}

//All LEDs blink red in sequence
void noGpsSignalAnimation(unsigned long currentMillis) {
  static unsigned long lastUpdate = 0;
  static bool state = false;
  const short noGpsBlinkSpeed = 500;  // Blink speed for No GPS Signal

  if (currentMillis - lastUpdate >= noGpsBlinkSpeed) {
    lastUpdate = currentMillis;
    state = !state;
    for (int i = 0; i < NUM_LEDS; i++) {
      leds[i] = state ? CRGB::Red : CRGB::Black;
    }
    FastLED.show();
  }
}

void manualOnAnimation() { //All LEDs solid green
  fill_solid(leds, NUM_LEDS, CRGB::Lime);
  FastLED.show();
}

void calibrationAnimation() { //Each LED cycles through red, green, and blue in sequence (like a wave).
  fill_solid(leds, NUM_LEDS, CRGB::White);
  FastLED.show();
  }


void loadingBarAnimation() {
  static unsigned long previousMillis = 0;
  static int loadingIndex = 0;
  const unsigned long interval = 333; // Interval between LED transitions

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    // Clear all LEDs
    fill_solid(leds, NUM_LEDS, CRGB::Black);

    // Light up the current loadingIndex LED in white
    for (int i = 0; i <= loadingIndex; i++) {
      leds[i] = CRGB::White;
    }

    // Update the LED strip
    FastLED.show();

    // Move to the next LED in the sequence
    loadingIndex++;
    if (loadingIndex >= NUM_LEDS+1) {
      loadingIndex = 0; // Reset to the first LED
      fill_solid(leds, NUM_LEDS, CRGB::Black);
      FastLED.show();
    }
  }
}

//Alternates LEDs flashing orange
void flagUpAnimation(unsigned long currentMillis) {
  static unsigned long lastUpdate = 0;
  static bool state = false;
  const short flagUpBlinkSpeed = 1000;  // Blink speed for Flag Up

  if (currentMillis - lastUpdate >= flagUpBlinkSpeed) {
    lastUpdate = currentMillis;
    state = !state;
    for (int i = 0; i < NUM_LEDS; i++) {
      if (state) {
        leds[i] = (i % 2 == 0) ? CRGB::Black : CRGB::Orange;  // Odd LEDs orange, even LEDs black
      } else {
        leds[i] = (i % 2 == 0) ? CRGB::Orange : CRGB::Black;  // Odd LEDs black, even LEDs orange
      }
    }
    FastLED.show();
  }
}

//LEDs display a slow chasing yellow light
void StandbyAnimation(unsigned long currentMillis) { 
  static unsigned long lastUpdate = 0;
  static uint8_t pos = 0;
  const short gpsStandbyInterval = 800;  // Speed of GPS Standby animation
  

  if (currentMillis - lastUpdate >= gpsStandbyInterval) {
    lastUpdate = currentMillis;
    for (int i = 0; i < NUM_LEDS; i++) {
      leds[i] = (i == pos) ? CRGB::Yellow : CRGB::Black;
    }
    pos = (pos + 1) % NUM_LEDS;
    FastLED.show();
  }
}

void bounceAnimation(unsigned long currentMillis) {
  static unsigned long lastUpdate = 0;
  static int position = 0;
  static int direction = 1;
  const long bounceInterval = 800;  // Speed of bounce animation

  if (currentMillis - lastUpdate >= bounceInterval) {
    lastUpdate = currentMillis;

    // Clear all LEDs
    for (int i = 0; i < NUM_LEDS; i++) {
      leds[i] = CRGB::Black;
    }

    // Set the current position to blue
    leds[position] = CRGB::Blue;

    // Update position
    position += direction;

    // Reverse direction if we hit the ends
    if (position == NUM_LEDS - 1 || position == 0) {
      direction = -direction;
    }
    FastLED.show();
  }
}

void heartbeatAnimation(unsigned long currentMillis) {
  static unsigned long lastUpdate = 0;
  static bool flashing = false;
  static uint8_t flashCount = 0;  // Counts the number of flashes
  const unsigned long flashInterval = 100;   // Interval for each flash (milliseconds)
  const unsigned long pauseInterval = 4000;  // Long pause after flashes (milliseconds)
  const uint8_t flashBrightness = 255;       // Full brightness for flashes
  const uint8_t ledBrightness = 0;            // LED off brightness

  // Handle the flashing and pause
  if (flashing) {
    if (currentMillis - lastUpdate >= flashInterval) {
      lastUpdate = currentMillis;
      flashCount++;
      
      // Toggle LED state between on and off
      if (flashCount % 2 == 0) {
        // Turn LEDs off
        fill_solid(leds, NUM_LEDS, CRGB::Black);
      } else {
        // Turn LEDs on with full brightness
        fill_solid(leds, NUM_LEDS, CRGB(0, 255, 0));
        Serial.println("THUMP");
      }

      FastLED.show();  // Update the LED strip

      // Stop flashing after two flashes and start the pause
      if (flashCount >= 4) {  // 2 flashes * 2 states (on/off) = 4 transitions
        flashing = false;
        flashCount = 0;
        lastUpdate = currentMillis;  // Reset lastUpdate to start the pause
      }
    }
  } else {
    // Handle the long pause
    if (currentMillis - lastUpdate >= pauseInterval) {
      lastUpdate = currentMillis;
      flashing = true;  // Start flashing again
    }
  }

  FastLED.show();  // Ensure the LEDs are updated
}

void GoodBuzzStartUp(){
  Serial.println("Good Buzz");
  // Timing and state variables
  static unsigned long beepStartTime = 0;
  static bool beepPhase = false;        // True for beep on, false for beep off
  static bool beepSequenceActive = false;
  static bool firstBeepDone = false;   // Indicates if the first beep is done
  static bool TwoBeepComplete = false;

  const unsigned long beepDuration = 30;   // Duration of each beep (milliseconds)
  const unsigned long beepInterval = 100;   // Duration between beeps (milliseconds)
  int frequency = 2000; //Hz frequency of buzzer

  while(TwoBeepComplete == false){
  unsigned long currentMillis = millis();
  unsigned long elapsedTime = currentMillis - beepStartTime;

  if (!beepSequenceActive) {
    // Start the beep sequence
    beepStartTime = currentMillis;
    beepSequenceActive = true;
    beepPhase = true;
    firstBeepDone = false;
    tone(buzzerPIN, frequency, beepDuration);  // Start the first beep
  } else {
    if (beepPhase) {
      // Handle the beep duration
      if (elapsedTime >= beepDuration) {
        noTone(buzzerPIN);  // Turn off the buzzer
        beepStartTime = currentMillis;  // Reset the timer
        beepPhase = false;  // Move to the pause phase
      }
    } 
    // Handle the pause between beeps
    if (!firstBeepDone && elapsedTime >= beepInterval) {
      tone(buzzerPIN, frequency, beepDuration);  // Start the second beep
      beepStartTime = currentMillis;  // Reset the timer for the second beep
      beepPhase = true;  // Move to the beep phase
      firstBeepDone = true;  // Mark the first beep as done
      TwoBeepComplete = true;
    }
    // Complete the sequence after the second beep
    if (firstBeepDone && elapsedTime >= beepDuration + beepInterval) {
      noTone(buzzerPIN);  // Turn off the buzzer
      beepSequenceActive = false;  // Reset the sequence
    }
  }
}
}

void GoodBuzz(){
  Serial.println("Good Buzz");
  // Timing and state variables
  static unsigned long beepStartTime = 0;
  static bool beepPhase = false;        // True for beep on, false for beep off
  static bool beepSequenceActive = false;
  static bool firstBeepDone = false;   // Indicates if the first beep is done
  static bool TwoBeepComplete = false;

  const unsigned long beepDuration = 30;   // Duration of each beep (milliseconds)
  const unsigned long beepInterval = 100;   // Duration between beeps (milliseconds)
  int frequency = 2000; //Hz frequency of buzzer


  unsigned long currentMillis = millis();
  unsigned long elapsedTime = currentMillis - beepStartTime;

  if (!beepSequenceActive) {
    // Start the beep sequence
    beepStartTime = currentMillis;
    beepSequenceActive = true;
    beepPhase = true;
    firstBeepDone = false;
    tone(buzzerPIN, frequency, beepDuration);  // Start the first beep
  } else {
    if (beepPhase) {
      // Handle the beep duration
      if (elapsedTime >= beepDuration) {
        noTone(buzzerPIN);  // Turn off the buzzer
        beepStartTime = currentMillis;  // Reset the timer
        beepPhase = false;  // Move to the pause phase
      }
    } 
    // Handle the pause between beeps
    if (!firstBeepDone && elapsedTime >= beepInterval) {
      tone(buzzerPIN, frequency, beepDuration);  // Start the second beep
      beepStartTime = currentMillis;  // Reset the timer for the second beep
      beepPhase = true;  // Move to the beep phase
      firstBeepDone = true;  // Mark the first beep as done
      TwoBeepComplete = true;
    }
    // Complete the sequence after the second beep
    if (firstBeepDone && elapsedTime >= beepDuration + beepInterval) {
      noTone(buzzerPIN);  // Turn off the buzzer
      beepSequenceActive = false;  // Reset the sequence
    }
  }
}

void ErrorBuzz() {
  static unsigned long beepStartTime = 0;
  static bool beepPhase = true;        // True for beep on, false for beep off
  static bool beepSequenceActive = false;
  static int beepCount = 0;           // Count of beeps completed
  int frequency = 2000;         //Hz frequency of buzzer

  const unsigned long beepDuration = 100;   // Duration of each beep (milliseconds)
  const unsigned long beepPauseDuration = 300; // Duration between beeps (milliseconds)

  unsigned long currentMillis = millis();
  unsigned long elapsedTime = currentMillis - beepStartTime;

  if (!beepSequenceActive) {
    // Initialize the beep sequence
    beepStartTime = currentMillis;
    beepCount = 0;
    beepSequenceActive = true;
    beepPhase = true;
    tone(buzzerPIN, frequency, beepDuration);  // Start the first beep
  } else {
    if (beepPhase) {
      // Handle the beep duration
      if (elapsedTime >= beepDuration) {
        noTone(buzzerPIN);  // Turn off the buzzer
        beepStartTime = currentMillis;  // Reset the timer
        beepPhase = false;  // Move to the pause phase
      }
    } else {
      // Handle the pause between beeps
      if (elapsedTime >= beepPauseDuration) {
        beepCount++;
        if (beepCount < 5) {
          // Start the next beep
          tone(buzzerPIN, frequency, beepDuration);
          beepStartTime = currentMillis;  // Reset the timer for the next beep
          beepPhase = true;  // Move to the beep phase
        } else {
          // End the sequence after 5 beeps
          noTone(buzzerPIN);  // Ensure the buzzer is off
          beepSequenceActive = false;  // Reset the sequence
        }
      }
    }
  }

}

void shortBeep() {
  short shortBeepDuration = 30;  // Duration of the short beep in milliseconds
  short buzzFrequency = 2000;  // Frequency of the buzz in Hz
  tone(buzzerPIN, buzzFrequency,shortBeepDuration);
}

void twoLongBuzzes() {
  Serial.println("Long Buzz");
  // Timing and state variables
  static unsigned long beepStartTime = 0;
  static bool beepPhase = false;        // True for beep on, false for beep off
  static bool beepSequenceActive = false;
  static bool firstBeepDone = false;   // Indicates if the first beep is done
  static bool TwoBeepComplete = false;

  const unsigned long beepDuration = 1000;   // Duration of each beep (milliseconds)
  const unsigned long beepInterval = 500;   // Duration between beeps (milliseconds)
  int frequency = 2000; //Hz frequency of buzzer


  unsigned long currentMillis = millis();
  unsigned long elapsedTime = currentMillis - beepStartTime;

  if (!beepSequenceActive) {
    // Start the beep sequence
    beepStartTime = currentMillis;
    beepSequenceActive = true;
    beepPhase = true;
    firstBeepDone = false;
    tone(buzzerPIN, frequency, beepDuration);  // Start the first beep
  } else {
    if (beepPhase) {
      // Handle the beep duration
      if (elapsedTime >= beepDuration) {
        noTone(buzzerPIN);  // Turn off the buzzer
        beepStartTime = currentMillis;  // Reset the timer
        beepPhase = false;  // Move to the pause phase
      }
    } 
    // Handle the pause between beeps
    if (!firstBeepDone && elapsedTime >= beepInterval) {
      tone(buzzerPIN, frequency, beepDuration);  // Start the second beep
      beepStartTime = currentMillis;  // Reset the timer for the second beep
      beepPhase = true;  // Move to the beep phase
      firstBeepDone = true;  // Mark the first beep as done
      TwoBeepComplete = true;
    }
    // Complete the sequence after the second beep
    if (firstBeepDone && elapsedTime >= beepDuration + beepInterval) {
      noTone(buzzerPIN);  // Turn off the buzzer
      beepSequenceActive = false;  // Reset the sequence
    }
  }
}

void systemOffAnimation() { //All LEDs off
  static uint8_t brightness = 0;
  static int direction = 1; // 1 for increasing brightness, -1 for decreasing
  const uint8_t minBrightness = 0; // Minimum brightness
  const uint8_t maxBrightness = 255; // Maximum brightness
  const short fadeSpeed = 10; // Speed of fading, lower values mean faster fading
  static unsigned long previousMillis = 0; 
  unsigned long currentMillis = millis();

 // Check if it's time to update the brightness
  if (currentMillis - previousMillis >= fadeSpeed) {
    previousMillis = currentMillis;
    // Update brightness
    brightness += direction;

  // Change direction if limits are reached
  if (brightness <= minBrightness || brightness >= maxBrightness) {
    direction *= -1;
  }

  // Clear all LEDs
  fill_solid(leds, NUM_LEDS, CRGB::Black);
  
  // Set the brightness of the third LED to white with the updated brightness
    leds[2] = CRGB(0, 0, 255);
    leds[2].fadeLightBy(255 - brightness); // Apply the pulsing effect

  // Update the LED strip
  FastLED.show();
  }
}