#include <Arduino.h>
#include <FastLED.h>
#include <Wire.h>
#include <avr/pgmspace.h>


//  Debug and Test options
#define _DEBUG_
//#define _TEST_

#ifdef _DEBUG_
  #define _PP(a) Serial.print(a);
  #define _PL(a) Serial.println(a);
#else
  #define _PP(a)
  #define _PL(a)
#endif

#define SWT_PIN  2
#define NUM_LEDS_STRIP 37
#define NUM_STRIPS 2
#define NUM_LEDS_SUM NUM_LEDS_STRIP * NUM_STRIPS

#define LED_PORT 3

typedef struct StripDefinition {
  int16_t first, last;
  uint8_t hue = 240;
  uint8_t sat = 240;
  uint8_t val = 255;
  uint8_t forward = 1;
} TStripDefinition;

typedef struct {
  CRGB leds[NUM_LEDS_SUM];
  // first and last led pos in strip
  uint8_t bpm = 80;
  uint8_t fade = 30;
  TStripDefinition stripDef[NUM_STRIPS];
} TWarpData;

TWarpData warpLEDS;

void Warp(uint8_t id);


uint16_t p,pos = 0;

void setup() {
  #if defined(_DEBUG_) || defined(_TEST_)
    Serial.begin(115200);
    delay(1000);
    _PL("FPV Warp tunnel by LunaX"); _PL();
  #endif
  FastLED.addLeds<WS2811, 3, GRB>(warpLEDS.leds, NUM_LEDS_SUM);
  uint16_t f, l;
  f = 0; l = NUM_LEDS_STRIP;
  _PL("----- INIT --------")

  warpLEDS.bpm = 80;
  warpLEDS.fade = 30;
  for (uint8_t i = 0; i < NUM_STRIPS; i++) {
    warpLEDS.stripDef[i].first = f;   
    warpLEDS.stripDef[i].last = f + NUM_LEDS_STRIP;
    warpLEDS.stripDef[i].hue = 200;
    warpLEDS.stripDef[i].sat = 200;
    warpLEDS.stripDef[i].val = 255;
    warpLEDS.stripDef[i].forward = 1;
    if (i > 0) {
      warpLEDS.stripDef[i].forward = (i % 2 == 0) ? 1 : 0;
    }
    f += (NUM_LEDS_STRIP+1);
    _PP("#");_PP(i);_PP(" F/L(");
    _PP(warpLEDS.stripDef[i].first); _PP(",");
    _PP(warpLEDS.stripDef[i].last);
    _PP(") Forward :"); _PP(warpLEDS.stripDef[i].forward) ;
    _PL();
  }
}

void loop() {
  Warp();
  FastLED.show();
}

void Warp() {
  _PL("Warp Tunnel");
  uint8_t bpm = random8(warpLEDS.bpm-20, warpLEDS.bpm+20);
  bpm=40;
  //uint8_t p = map(beat16(bpm, 0), 0, 65535, 0, NUM_LEDS_STRIP);
  p = pos;
  uint8_t pList[] = {
    ((warpLEDS.stripDef[0].forward==1)? warpLEDS.stripDef[0].first + p : warpLEDS.stripDef[0].last - p),
    ((warpLEDS.stripDef[1].forward==1)? warpLEDS.stripDef[1].first + p : warpLEDS.stripDef[1].last - p),
  //  ((warpLEDS.stripDef[2].forward==1)? warpLEDS.stripDef[2].first + p : warpLEDS.stripDef[2].last - p),
  //  ((warpLEDS.stripDef[3].forward==1)? warpLEDS.stripDef[3].first + p : warpLEDS.stripDef[3].last - p),
  //  ((warpLEDS.stripDef[4].forward==1)? warpLEDS.stripDef[4].first + p : warpLEDS.stripDef[4].last - p),
  //  ((warpLEDS.stripDef[5].forward==1)? warpLEDS.stripDef[5].first + p : warpLEDS.stripDef[5].last - p),
  };
  
  for (uint8_t n=0; n < NUM_STRIPS; n++) {
    warpLEDS.leds[pList[n]] = CHSV (
      warpLEDS.stripDef[n].hue,
      warpLEDS.stripDef[n].sat,
      warpLEDS.stripDef[n].val
    );
  }
  fadeToBlackBy(warpLEDS.leds, NUM_LEDS_SUM, warpLEDS.fade);

  if (++pos >= NUM_LEDS_STRIP) {
    pos = 0;
  }
  for (uint8_t n=0; n < NUM_STRIPS; n++){
    _PP("#"); _PP(n); _PP("\tF/L(");
    _PP(warpLEDS.stripDef[n].first); _PP("/");
    _PP(warpLEDS.stripDef[n].last);  _PP(")\tCurPos (");
    _PP(pList[n]);_PP(")\tDir:");
    _PP((warpLEDS.stripDef[n].forward==1)?"FWD":"BWD");
    _PP(" -> pos #");_PP(pos);
    _PL();
  }

  _PL();
}