// Three NeoPixel rings
// seperately numerated rings
// One worm
// 12 clumsy descisions

#include <Adafruit_NeoPixel.h>

#define FIFO_LENGTH 8
#define NUM_PIXELS 24   // Number of pixels in each NeoPixel ring
#define NUM_RINGS 3     // Number of NeoPixel rings
#define PIXEL_PIN0 9     // Pin connected to the NeoPixel rings
#define PIXEL_PIN1 10     // Pin connected to the NeoPixel rings
#define PIXEL_PIN2 11     // Pin connected to the NeoPixel rings
#define CHANCE 8          // CHANCE 2: 50:50, 3: 1:3

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)
};

uint32_t addC( int ring, int pixel, uint32_t color)
{
  uint32_t currentColor = pixels[ring].getPixelColor(pixel);
  // Get the RGB components of the current color
  uint8_t r1 = (currentColor >> 16) & 0xff;
  uint8_t g1 = (currentColor >>  8) & 0xff;
  uint8_t b1 = (currentColor >>  0) & 0xff;

  // Get the RGB components of the new color
  uint8_t r2 = (color >> 16) & 0xff;
  uint8_t g2 = (color >>  8) & 0xff;
  uint8_t b2 = (color >>  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 the mixed color of the NeoPixel
  return pixels[ring].Color(r, g, b);
}


class Worm {
  public:
    int pos[FIFO_LENGTH];
    int ring[FIFO_LENGTH];
    uint32_t savedC[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
    unsigned long lastUpdate = 0;   // The time of the last update
    uint32_t c = pixels[0].Color(255, 0, 0);

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

    }
    void moveOn() {

      pixels[ring[FIFO_LENGTH - 1]].setPixelColor(pos[FIFO_LENGTH - 1], 0, 0, 0); //savedC[FIFO_LENGTH - 1]); // Turn off the current pixel, i.e. return to the color it had before
      //pixels[ring[FIFO_LENGTH - 1]].setPixelColor(pos[FIFO_LENGTH - 1], savedC[FIFO_LENGTH - 1]); // Turn off the current pixel, i.e. return to the color it had before
      currentPixel += direction;
      currentPixel = currentPixel % NUM_PIXELS;
      if (currentPixel == 0 || currentPixel == -1 || currentPixel == 4 || currentPixel == 3) {
        if (currentRing == 0 && direction == 1 && currentPixel == 0) {
          //Serial.println("currentRing == 0 && direction == 1 && currentPixel == 0");
          if (random(CHANCE) == 0) {
            currentRing = 1; currentPixel = 3; direction = -1;
          } else {
            currentRing = 0; currentPixel = 0; direction = 1;
          }
        }
        else if (currentRing == 1 && direction == -1 && currentPixel == -1) {
          //Serial.println("currentRing == 1 && direction == -1 && currentPixel == -1");
          if (random(CHANCE) == 0) {
            currentRing = 2; currentPixel = 4; direction = 1;
          } else {
            currentRing = 1; currentPixel = 23; direction = -1;
          }
        }
        else if (currentRing == 2 && direction == 1 && currentPixel == 0) {
          //Serial.println("currentRing == 2 && direction == 1 && currentPixel == 0");
          if (random(CHANCE) == 0) {
            currentRing = 0; currentPixel = 3; direction = -1;
          } else {
            currentRing = 2; currentPixel = 0;  direction = 1;
          }
        }
        else if (currentRing == 0 && direction == -1 && currentPixel == -1) {
          //Serial.println("currentRing == 0 && direction == -1 && currentPixel == -1 /");
          if (random(CHANCE) == 0) {
            currentRing = 1; currentPixel = 4; direction = 1;
          } else {
            currentRing = 0; currentPixel = 23; direction = -1;
          }
        }
        else if (currentRing == 1 && direction == 1 && currentPixel == 0) {
          //Serial.println("currentRing == 1 && direction == 1 && currentPixel == 0");
          if (random(CHANCE) == 0) {
            currentRing = 2; currentPixel = 3; direction = -1;
          } else {
            currentRing = 1; currentPixel = 0;  direction = 1;
          }
        }
        else if (currentRing == 2 && direction == -1 && currentPixel == -1) {
          //Serial.println("currentRing == 2 && direction == -1 && currentPixel == -1");
          if (random(CHANCE) == 0) {
            currentRing = 0; currentPixel = 4; direction = 1;
          } else {
            currentRing = 2; currentPixel = 23; direction = -1;
          }
        }
        else if (currentRing == 0 && direction == 1 && currentPixel == 4) {
          //Serial.println("currentRing == 0 && direction == 1 && currentPixel == 4");
          if (random(CHANCE) == 0) {
            currentRing = 2; currentPixel = 23; direction = -1;
          } else {
            currentRing = 0; currentPixel = 4; direction = 1;
          }
        }
        else if (currentRing == 2 && direction == -1 && currentPixel == 3) {
          //Serial.println("currentRing == 2 && direction == -1 && currentPixel == 3");
          if (random(CHANCE) == 0) {
            currentRing = 1; currentPixel = 0;  direction = 1;
          } else {
            currentRing = 2; currentPixel = 3; direction = -1;
          }
        }
        else if (currentRing == 1 && direction == 1 && currentPixel == 4) {
          //Serial.println("currentRing == 1 && direction == 1 && currentPixel == 4 #");
          if (random(CHANCE) == 0) {
            currentRing = 0; currentPixel = 23; direction = -1;
          } else {
            currentRing = 1; currentPixel = 4; direction = 1;
          }
        }
        else if (currentRing == 0 && direction == -1 && currentPixel == 3) {
          //Serial.println("currentRing == 0 && direction == -1 && currentPixel == 3");
          if (random(CHANCE) == 0) {
            currentRing = 2; currentPixel = 0; direction = 1;
          } else {
            currentRing = 0; currentPixel = 3; direction = -1;
          }
        }
        else if (currentRing == 2 && direction == 1 && currentPixel == 4) {
          //Serial.println("currentRing == 2 && direction == 1 && currentPixel == 4");
          if (random(CHANCE) == 0) {
            currentRing = 1; currentPixel = 23; direction = -1;
          } else {
            currentRing = 2; currentPixel = 4; direction = 1;
          }
        }
        else if (currentRing == 1 && direction == -1 && currentPixel == 3) {
          //Serial.println("currentRing == 1 && direction == -1 && currentPixel == 3");
          if (random(CHANCE) == 0) {
            currentRing = 0; currentPixel = 0; direction = 1;
          } else {
            currentRing = 1; currentPixel = 3; direction = -1;
          }
        }
      }

      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) {
        savedC[i] = savedC[i - 1];
        //Serial.print(ring[i]);Serial.print(" ");
      }
      savedC[0] = pixels[currentRing].getPixelColor(currentPixel);
      // Turn on the new pixel
      pixels[currentRing].setPixelColor(currentPixel, addC(currentRing, currentPixel, c));
    }
};
Worm wormRed, wormGreen, wormBlue;

void setup() {
  for (int i = 0; i < NUM_RINGS; i++) {
    pixels[i].begin();
    pixels[i].show();    // Initialize all pixels to off
  }
  Serial.begin(9600);

  //randomSeed(analogRead(0));
  delay(500);

  wormRed.currentPixel = 8;
  wormRed.currentRing = 0;
  wormRed.direction = 1;
  wormRed.c = pixels[0].Color(200, 0, 0);
  wormGreen.currentPixel = 8;
  wormGreen.currentRing = 1;
  wormGreen.direction = 1;
  wormGreen.c = pixels[0].Color(0, 200, 0);
  wormBlue.currentPixel = 8;
  wormBlue.currentRing = 2;
  wormBlue.direction = 1;
  wormBlue.c = pixels[0].Color(0, 0, 255);

  // See where the start is. Can be romoved later
  // pixels[0].setPixelColor(23, pixels[0].Color(0, 255, 0));
  // pixels[0].setPixelColor(0, pixels[0].Color(255, 0, 0));
  // pixels[0].setPixelColor(3, pixels[0].Color(255, 0, 0));
  // pixels[0].setPixelColor(4, pixels[0].Color(0, 0, 255));
  // pixels[0].show();
  // pixels[1].setPixelColor(23, pixels[0].Color(255, 255, 0));
  // pixels[1].setPixelColor(0, pixels[0].Color(255, 0, 0));
  // pixels[1].setPixelColor(3, pixels[0].Color(255, 0, 0));
  // pixels[1].setPixelColor(4, pixels[0].Color(0, 255, 0));
  // pixels[1].show();
  // pixels[2].setPixelColor(23, pixels[0].Color(0, 0, 255));
  // pixels[2].setPixelColor(0, pixels[0].Color(255, 0, 0));
  // pixels[2].setPixelColor(3, pixels[0].Color(255, 0, 0));
  // pixels[2].setPixelColor(4, pixels[0].Color(255, 255, 0));
  // pixels[2].show();

  //delay(1000000);
}

void loop() {
  delay(250);
  wormRed.moveOn();
  wormGreen.moveOn();
  //wormBlue.moveOn();
  // Update all NeoPixel rings
  for (int i = 0; i < NUM_RINGS; i++) {
    pixels[i].show();
  }

}

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