// Three NeoPixel rings
// seperately numerated rings
// One worm
// 12 clumsy descisions - getting better
/////////////////////////////////////
// (C) Hartmut Wöhlbier          ////
/////////////////////////////////////


#include <Adafruit_NeoPixel.h>

#define FIFO_LENGTH 18
#define NUM_PIXELS 35  // Number of pixels in each NeoPixel ring
#define NUM_RINGS 3    // Number of NeoPixel rings
#define PIXEL_PIN0 19   // Pin connected to the NeoPixel rings
#define PIXEL_PIN2 18   // Pin connected to the NeoPixel rings
#define PIXEL_PIN1 21   // Pin connected to the NeoPixel rings
#define CHANCE 2       // CHANCE 2: 50:50, 3: 1:3
#define DIMM 255
#define CALIBRATE false
#define SPEED 100  // Less is faster !!

#define DEBUG false

Adafruit_NeoPixel pixels[NUM_RINGS] = { Adafruit_NeoPixel(NUM_PIXELS, PIXEL_PIN0, NEO_GRB + NEO_KHZ800),
                                        Adafruit_NeoPixel(NUM_PIXELS, PIXEL_PIN1, NEO_GRB + NEO_KHZ800),
                                        Adafruit_NeoPixel(NUM_PIXELS, PIXEL_PIN2, NEO_GRB + NEO_KHZ800)
                                      };

// Color Arrays holds the info of each color to later add em up. 0: red, 1: green, 2: blue
uint32_t cA[3][NUM_RINGS][NUM_PIXELS];
uint32_t whiteC, blackC, redC, greenC, blueC;

uint32_t add2C(uint32_t color1, uint32_t color2) {
  // Get the RGB components of the current color
  uint8_t r1 = (color1 >> 16) & 0xff;
  uint8_t g1 = (color1 >> 8) & 0xff;
  uint8_t b1 = (color1 >> 0) & 0xff;

  // Get the RGB components of the new color
  uint8_t r2 = (color2 >> 16) & 0xff;
  uint8_t g2 = (color2 >> 8) & 0xff;
  uint8_t b2 = (color2 >> 0) & 0xff;

  // Mix the RGB components using the subtractive color model
  uint8_t r = r1 + r2 > 255 ? 255 : r1 + r2;
  uint8_t g = g1 + g2 > 255 ? 255 : g1 + g2;
  uint8_t b = b1 + b2 > 255 ? 255 : b1 + b2;

  return ((uint32_t)r << 16) | ((uint32_t)g << 8) | ((uint32_t)b);
}

uint32_t rgbTo32bit(uint8_t r, uint8_t g, uint8_t b) {
  uint32_t color32bit = ((uint32_t)r << 16) | ((uint32_t)g << 8) | ((uint32_t)b);
  return color32bit;
}

class Worm {
  public:
    int colorNum;  //
    int pos[FIFO_LENGTH];
    int ring[FIFO_LENGTH];
    uint32_t savedC[FIFO_LENGTH];
    uint32_t oldC[FIFO_LENGTH];
    int currentPixel = 0;  // The current pixel that is lit
    int currentRing = 0;   // The current ring the pixel is on
    int direction = 1;     // The direction the pixel is moving in
    uint32_t c = whiteC;

  public:
    Worm() {  //int currP, int currR, int dir) {
    }
    void moveOn() {

      cA[colorNum][ring[FIFO_LENGTH - 1]][pos[FIFO_LENGTH - 1]] = blackC;

      currentPixel += direction;
      if (currentPixel == -1) currentPixel = 34;
      currentPixel = currentPixel % NUM_PIXELS;
      if (currentPixel == 0 || currentPixel == 34 || currentPixel == 6 || currentPixel == 5) {
        if (random(CHANCE) == 0) {
          direction = -direction;
          if (currentRing == 0 && direction == 1 && currentPixel == 0) {
            currentRing = 1;
            currentPixel = 5;
          } else if (currentRing == 0 && direction == 1 && currentPixel == 6) {
            currentRing = 2;
            currentPixel = 34;
          } else if (currentRing == 0 && direction == -1 && currentPixel == 34) {
            currentRing = 1;
            currentPixel = 6;
          } else if (currentRing == 0 && direction == -1 && currentPixel == 5) {
            currentRing = 2;
            currentPixel = 0;
          } else if (currentRing == 1 && direction == 1 && currentPixel == 0) {
            currentRing = 2;
            currentPixel = 5;
          } else if (currentRing == 1 && direction == 1 && currentPixel == 6) {
            currentRing = 0;
            currentPixel = 34;
          } else if (currentRing == 1 && direction == -1 && currentPixel == 34) {
            currentRing = 2;
            currentPixel = 6;
          } else if (currentRing == 1 && direction == -1 && currentPixel == 5) {
            currentRing = 0;
            currentPixel = 0;
          } else if (currentRing == 2 && direction == 1 && currentPixel == 0) {
            currentRing = 0;
            currentPixel = 5;
          } else if (currentRing == 2 && direction == 1 && currentPixel == 6) {
            currentRing = 1;
            currentPixel = 34;
          } else if (currentRing == 2 && direction == -1 && currentPixel == 34) {
            currentRing = 0;
            currentPixel = 6;
          } else if (currentRing == 2 && direction == -1 && currentPixel == 5) {
            currentRing = 1;
            currentPixel = 0;
          } else {
            direction = -direction;
          }
        }
      }


      for (int i = FIFO_LENGTH - 1; i > 0; --i) {
        pos[i] = pos[i - 1];
        //Serial.print(pos[i]);Serial.print(" ");
      }
      pos[0] = currentPixel;
      for (int i = FIFO_LENGTH - 1; i > 0; --i) {
        ring[i] = ring[i - 1];
        //Serial.print(ring[i]);Serial.print(" ");
      }
      ring[0] = currentRing;

      for (int i = FIFO_LENGTH - 1; i > 0; --i) {
        oldC[i] = savedC[i];
        savedC[i] = savedC[i - 1];
        //Serial.print(ring[i]);Serial.print(" ");
      }
      savedC[colorNum] = cA[0][currentRing][currentPixel];

      // Turn on the new pixel
      cA[colorNum][currentRing][currentPixel] = c;
    }
};
Worm wormRed, wormGreen, wormBlue;

void setup() {
  if (DEBUG) {
    Serial.begin(115200);
    delay(100);
  }
  if (DEBUG) {
    Serial.println("Starting up...");
  }

  pinMode(PIXEL_PIN0, OUTPUT);
  pinMode(PIXEL_PIN1, OUTPUT);
  pinMode(PIXEL_PIN2, OUTPUT);

  randomSeed(1);  // randomSeed(analogRead(0));
  delay(100);

  whiteC = rgbTo32bit(255, 255, 255);
  blackC = rgbTo32bit(0, 0, 0);
  redC = rgbTo32bit(255, 0, 0);
  greenC = rgbTo32bit(0, 200, 0);
  blueC = rgbTo32bit(0, 0, 255);

  // for (int i = 0; i < NUM_RINGS; ++i) {
  //   pixels[i].begin();
  //   for (int j = 0; j < NUM_PIXELS; ++j) {
  //     pixels[i].setPixelColor(j, greenC);
  //   }
  //   pixels[i].show();
  // }
  // delay(500);
  for (int i = 0; i < NUM_RINGS; ++i) {
    pixels[i].begin();
    for (int j = 0; j < NUM_PIXELS; ++j) {
      // pixels[i].setPixelColor(j, blackC);  // Initialize all pixels to off
      cA[0][i][j] = blackC;
      cA[1][i][j] = blackC;
      cA[2][i][j] = blackC;
    }
    pixels[i].show();
  }

  wormRed.colorNum = 0;
  wormRed.currentPixel = 12;
  wormRed.currentRing = 0;
  wormRed.direction = 1;
  wormRed.c = redC;
  wormGreen.colorNum = 1;
  wormGreen.currentPixel = 12;
  wormGreen.currentRing = 1;
  wormGreen.direction = 1;
  wormGreen.c = greenC;
  wormBlue.colorNum = 2;
  wormBlue.currentPixel = 12;
  wormBlue.currentRing = 2;
  wormBlue.direction = 1;
  wormBlue.c = blueC;

  pixels[0].setBrightness(DIMM);
  pixels[1].setBrightness(DIMM);
  pixels[2].setBrightness(DIMM);

  // See where the start is. Can be romoved later
  if (CALIBRATE) {
    while (1) {
      pixels[0].setPixelColor(34, greenC);
      //pixels[0].setPixelColor(0, redC);
      pixels[0].setPixelColor(5, redC);
      pixels[0].setPixelColor(6, blueC);
      pixels[0].show();
      pixels[1].setPixelColor(34, rgbTo32bit(255, 255, 0));
      pixels[1].setPixelColor(0, redC);
      pixels[1].setPixelColor(5, redC);
      pixels[1].setPixelColor(6, greenC);
      pixels[1].show();
      pixels[2].setPixelColor(34, blueC);
      pixels[2].setPixelColor(0, redC);
      pixels[2].setPixelColor(5, redC);
      pixels[2].setPixelColor(6, rgbTo32bit(255, 255, 0));
      pixels[2].show();
    }
    delay(1000000);
  }

  if (DEBUG) {
    Serial.println("Finished Init.");
  }
}

void loop() {

  wormRed.moveOn();
  wormGreen.moveOn();
  wormBlue.moveOn();

  addEmUp();

  delay(SPEED);
}

void addEmUp() {
  for (int i = 0; i < NUM_RINGS; ++i) {
    for (int j = 0; j < NUM_PIXELS; ++j) {
      pixels[i].setPixelColor(j, add2C(cA[0][i][j], add2C(cA[1][i][j], cA[2][i][j])));
    }
    pixels[i].show();
  }
}