#include <FastLED.h>

#define TRIG_PIN A3
#define ECHO_PIN A2

#define LED_PIN     8
#define NUM_LEDS    16
#define BRIGHTNESS  255
#define LED_TYPE    WS2812
#define COLOR_ORDER GRB
CRGB leds[NUM_LEDS];

#define UPDATES_FREQ    100 // per second
#define MEASURE_FREQ      2 // per second

#define DISTANCE_MAX  400 // cm
#define DISTANCE_MIN  0 // cm


CRGBPalette16 currentPalette;
TBlendType    currentBlending;

CRGB rgb;

extern const TProgmemPalette16 mySemaphorePalette_p PROGMEM;

/***
*
 ***
    *
 ***/
void setup() {
  delay(2000);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);


  FastLED.addLeds<LED_TYPE, LED_PIN, COLOR_ORDER>(leds, NUM_LEDS); 
  FastLED.setBrightness( BRIGHTNESS );

  currentPalette = mySemaphorePalette_p;
  currentBlending = LINEARBLEND;


  FillColorDelay( CRGB::Yellow, 1000);
  

  
  FillColorDelay( CRGB::Black, 1000);
}

/*
 *
 *
 *
 ****/
void loop() {
  static unsigned int distance = 10;
  static uint8_t colorIndex;
  static byte offset = 0;
  static unsigned int offset_delay = 100;         

  static unsigned long lastOffsetTime = 0;        
  static unsigned long lastMeasureTime = 0;       
  static unsigned long lastPrintTime = 0;         

  
  offset_delay = map( distance, 20, 400, 20, 100);
  if ( millis() - lastOffsetTime >= offset_delay ) {
    offset = (offset < NUM_LEDS - 1 ? offset + 1 : 0); 
    lastOffsetTime = millis();
  }

 
  if (millis() - lastMeasureTime >= 1000 / MEASURE_FREQ) {
    distance = get_distance();
    colorIndex = map(distance, DISTANCE_MIN, DISTANCE_MAX, 0, 240);
    // update palette
    newGradientPaletteByDistance(distance);
    lastMeasureTime = millis();
  }

  if ( distance > 30 ) {
    FillLEDsFromPalette( offset );
  } else if ( offset % 2 == 0 ) {
    FillColorDelay(CRGB::Blue,200);
    // fill_solid( leds, NUM_LEDS, CRGB::Blue);
  } else {
    FillColorDelay(CRGB::Red,200);
    // fill_solid( leds, NUM_LEDS, CRGB::Red);
  }


  FastLED.show();
  FastLED.delay(1000 / UPDATES_FREQ);
}



void newGradientPaletteByDistance(int distance) {
  uint8_t xyz[12];  
  CRGB rgb;

  rgb = ColorFromPalette( mySemaphorePalette_p, map(distance, DISTANCE_MIN, DISTANCE_MAX, 0, 240), BRIGHTNESS, currentBlending);

  
  xyz[0] = 0;     xyz[1] = rgb.r;   xyz[2] = rgb.g;   xyz[3] = rgb.b; 
  xyz[4] = 40;    xyz[5] = rgb.r;   xyz[6] = rgb.g;   xyz[7] = rgb.b;  
  xyz[8] = 255;   xyz[9] = 0;       xyz[10] = 0;      xyz[11] = 0;      

  currentPalette.loadDynamicGradientPalette(xyz);
}




void FillLEDsFromPalette( byte offset) {
  

  
  for ( int i = 0; i < NUM_LEDS; i++) {
    leds[ i ] = ColorFromPalette( currentPalette, (i+offset) * 255 / (NUM_LEDS - 1), BRIGHTNESS, currentBlending);
  }
 
}


void FillColorDelay ( CRGB color, unsigned long duration ) {
  fill_solid( leds, NUM_LEDS, color);
  FastLED.show();
  FastLED.delay(duration);
}



const TProgmemPalette16 mySemaphorePalette_p PROGMEM = {
  CRGB::Magenta, CRGB::Red,     CRGB::Red,     CRGB::Red,
  CRGB::Orange,  CRGB::Orange,  CRGB::Orange,  CRGB::Orange,
  CRGB::Yellow,  CRGB::Yellow,  CRGB::Yellow,  CRGB::Green,
  CRGB::Green,   CRGB::Green,   CRGB::Green,   CRGB::White
};




int get_distance() {
  static int distance;
  uint16_t duration = 0;
  uint32_t interval = 0;

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = (duration / 2) / 29;

  return distance; 
}