#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:
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();
  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);

}

//All LEDs blink red in sequence
void noGpsSignalAnimation(unsigned long currentMillis) {
  static unsigned long lastUpdate = 0;
  static bool state = false;
  const short noGpsBlinkSpeed = 1000;  // 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::Green);
FastLED.show();
}

void calibrationAnimation() { //Each LED cycles through red, green, and blue in sequence (like a wave).
  static unsigned long lastUpdate = 0;
  static uint8_t hue = 0;
  const long calibrationInterval = 50;  // Speed of calibration animation

  unsigned long currentMillis = millis();
  if (currentMillis - lastUpdate >= calibrationInterval) {
    lastUpdate = currentMillis;
    for (int i = 0; i < NUM_LEDS; i++) {
      leds[i] = CHSV(hue + (i * 85), 255, 255);
    }
    hue += 1;  // Adjust hue increment for wave speed
    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));
      }

      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
}