#include <NewPing.h>
#include <FastLED.h>

// Setup the variables for the HC-SR04
const int trigPin1 = 8; // Sensor 1
const int echoPin1 = 7; // Sensor 1

const int trigPin2 = 6; // Sensor 2
const int echoPin2 = 5; // Sensor 2

#define LED_PIN1 2
#define NUM_LEDS1 16
#define BRIGHTNESS1 255
#define LED_TYPE WS2812
#define COLOR_ORDER GRB
CRGB leds1[NUM_LEDS1];

#define LED_PIN2 3
#define NUM_LEDS2 16
#define BRIGHTNESS2 255
CRGB leds2[NUM_LEDS2];

#define UPDATES_PER_SECOND 1

CRGBPalette16 currentPalette;
TBlendType currentBlending;

extern CRGBPalette16 myRedWhiteBluePalette;
extern const TProgmemPalette16 myRedWhiteBluePalette_p PROGMEM;

void setup() {
    Serial.begin(9600);
    delay(10); // power-up safety delay
    FastLED.addLeds<LED_TYPE, LED_PIN1, COLOR_ORDER>(leds1, NUM_LEDS1).setCorrection(TypicalLEDStrip);
    FastLED.setBrightness(BRIGHTNESS1);
    FastLED.addLeds<LED_TYPE, LED_PIN2, COLOR_ORDER>(leds2, NUM_LEDS2).setCorrection(TypicalLEDStrip);
    FastLED.setBrightness(BRIGHTNESS2);
    currentPalette = RainbowColors_p;
    currentBlending = LINEARBLEND;
}

void loop() {
    // establish variables for duration of the ping, 
    // and the distance result in inches and centimeters:
    long duration1, duration2, inches1, inches2, cm1, cm2;

    // Read Sensor 1
    pinMode(trigPin1, OUTPUT);
    digitalWrite(trigPin1, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin1, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin1, LOW);

    pinMode(echoPin1, INPUT);
    duration1 = pulseIn(echoPin1, HIGH);

    // Read Sensor 2
    pinMode(trigPin2, OUTPUT);
    digitalWrite(trigPin2, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin2, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin2, LOW);

    pinMode(echoPin2, INPUT);
    duration2 = pulseIn(echoPin2, HIGH);

    // convert the time into a distance
    inches1 = microsecondsToInches(duration1);
    cm1 = microsecondsToCentimeters(duration1);

    inches2 = microsecondsToInches(duration2);
    cm2 = microsecondsToCentimeters(duration2);

    Serial.print("Sensor 1: ");
    Serial.print(inches1);
    Serial.print("in, ");
    Serial.print(cm1);
    Serial.print("cm");

    Serial.print(" | Sensor 2: ");
    Serial.print(inches2);
    Serial.print("in, ");
    Serial.print(cm2);
    Serial.print("cm");
    Serial.println();

    if (cm1 <= 75) {
        static uint8_t startIndex1 = 0;
        startIndex1 = startIndex1 + 2; /* motion speed */
        FillLEDsFromPaletteColors(leds1, NUM_LEDS1, startIndex1);
        FastLED.show();
        FastLED.delay(100 / UPDATES_PER_SECOND);
    }

    if (cm2 <= 100) {
        static uint8_t startIndex2 = 0;
        startIndex2 = startIndex2 + 2; /* motion speed */
        FillLEDsFromPaletteColors(leds2, NUM_LEDS2, startIndex2);
        FastLED.show();
        FastLED.delay(100 / UPDATES_PER_SECOND);
    }

    ChangePalettePeriodically();
}

long microsecondsToInches(long microseconds) {
    return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds) {
    return microseconds / 29 / 2;
}

void FillLEDsFromPaletteColors(CRGB* leds, int numLeds, uint8_t colorIndex) {
    uint8_t brightness = 255;
    for (int i = 0; i < numLeds; ++i) {
        leds[i] = ColorFromPalette(currentPalette, colorIndex, brightness, currentBlending);
        colorIndex += 3;
    }
}

void ChangePalettePeriodically() {
    uint8_t secondHand = (millis() / 1000) % 60;
    static uint8_t lastSecond = 99;
    if (lastSecond != secondHand) {
        lastSecond = secondHand;
        if (secondHand == 0) { currentPalette = RainbowColors_p; currentBlending = LINEARBLEND; }
        if (secondHand == 10) { currentPalette = RainbowStripeColors_p; currentBlending = NOBLEND; }
        if (secondHand == 15) { currentPalette = RainbowStripeColors_p; currentBlending = LINEARBLEND; }
        if (secondHand == 20) { SetupPurpleAndGreenPalette(); currentBlending = LINEARBLEND; }
        if (secondHand == 25) { SetupTotallyRandomPalette(); currentBlending = LINEARBLEND; }
        if (secondHand == 30) { SetupBlackAndWhiteStripedPalette(); currentBlending = NOBLEND; }
        if (secondHand == 35) { SetupBlackAndWhiteStripedPalette(); currentBlending = LINEARBLEND; }
        if (secondHand == 40) { currentPalette = CloudColors_p; currentBlending = LINEARBLEND; }
        if (secondHand == 45) { currentPalette = PartyColors_p; currentBlending = LINEARBLEND; }
        if (secondHand == 50) { currentPalette = myRedWhiteBluePalette_p; currentBlending = NOBLEND; }
        if (secondHand == 55) { currentPalette = myRedWhiteBluePalette_p; currentBlending = LINEARBLEND; }
    }
}

void SetupTotallyRandomPalette() {
    for (int i = 0; i < 16; ++i) {
        currentPalette[i] = CHSV(random8(), 255, random8());
    }
}

void SetupBlackAndWhiteStripedPalette() {
    // 'black out' all 16 palette entries...
    fill_solid(currentPalette, 16, CRGB::Black);
    // and set every fourth one to white.
    currentPalette[0] = CRGB(88, 190, 95);
    currentPalette[4] = CRGB(222, 228, 30);
    currentPalette[8] = CRGB(15, 212, 11);
    currentPalette[12] = CRGB(226, 200, 6);
}

void SetupPurpleAndGreenPalette() {
    CRGB purple = CHSV(HUE_PURPLE, 255, 255);
    CRGB green = CHSV(HUE_GREEN, 255, 255);
    CRGB black = CRGB::Black;
    currentPalette = CRGBPalette16(
        green, green, black, black,
        purple, purple, black, black,
        green, green, black, black,
        purple, purple, black, black
    );
}

const TProgmemPalette16 myRedWhiteBluePalette_p PROGMEM = {
    CRGB::Red,
    CRGB::Gray, // 'white' is too bright compared to red and blue
    CRGB::Blue,
    CRGB::Black,
    CRGB::Red,
    CRGB::Gray,
    CRGB::Blue,
    CRGB::Black,
    CRGB::Red,
    CRGB::Red,
    CRGB::Gray,
    CRGB::Gray,
    CRGB::Blue,
    CRGB::Blue,
    CRGB::Black,
    CRGB::Black
};