#include <Adafruit_NeoPixel.h>


#define READPIN1       0
#define READPIN2       1
#define READPIN3       2
#define READPIN4       3

#define READDIGITALPIN1       2
#define READDIGITALPIN2       5


#define WRITEPIN1      6
#define NUMPIXELS      24

Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, WRITEPIN1, NEO_GRB + NEO_KHZ800);
int ledIndex = 0;
int frame = 0;
String in = "a";

void setup()
{
  pinMode(READDIGITALPIN1, INPUT);
   pixels.begin();
  Serial.begin(9600);
  Serial.setTimeout(100);
}

//from http://www.elcojacobs.com/eleminating-noise-from-sensor-readings-on-arduino-with-digital-filtering/
int analogReadFiltered(int sensorpin, int readCount){
   // read multiple values and sort them to take the mode
   int sampleCountHalf = readCount/6;
   int sortedValues[readCount];
   for(int i=0;i<readCount;i++){
     int value = analogRead(sensorpin);
     int j;
     if(value<sortedValues[0] || i==0){
        j=0; //insert at first position
     }
     else{
       for(j=1;j<i;j++){
          if(sortedValues[j-1]<=value && sortedValues[j]>=value){
            // j is insert position
            break;
          }
       }
     }
     for(int k=i;k>j;k--){
       // move all values higher than current reading up one position
       sortedValues[k]=sortedValues[k-1];
     }
     sortedValues[j]=value; //insert current reading
   }
   //return scaled mode of 10 values
   float returnval = 0;
   for(int i=readCount/2-sampleCountHalf;i<(readCount/2+sampleCountHalf);i++){
     returnval +=sortedValues[i];
   }
   return = returnval/(sampleCountHalf+sampleCountHalf);
}


void loop()
{
  // 0 - 1024 ( 0 - 1023 )
  int val1 = analogReadFiltered(READPIN1, 26);
  int val2 = analogReadFiltered(READPIN2, 12);
  int val3 = analogReadFiltered(READPIN3, 12);


  int digital1 = digitalRead(READDIGITALPIN1);
  int digital2 = digitalRead(READDIGITALPIN2);

  float values[] = {val1, val2, val3, digital1, digital2};
  int count = 5;

  Serial.flush();

  for(int i=0; i<count; i++)
  {
    Serial.print(values[i]);
    Serial.print(",");
  }

  Serial.println();


  if(frame % 2 == 0)
  {
    while (Serial.available() > 0)
    {
      in = Serial.readStringUntil('\n');
    }

    ledIndex++;

    int r,g,b;
    int skipSize = 8;

    if(in.charAt(0) == 'a')  //default
    {
      r = 0;
      g = 50;
      b = 200;
    }
    else if(in.charAt(0) == 'b')  //carry pallet
    {
      r = 0;
      g = 240;
      b = 50;
    }
    else if(in.charAt(0) == 'c')  //pallet delivered
    {
      r = 240;
      g = 200;
      b = 0;
      skipSize = 4;
      ledIndex--;
    }



    for(int i=0;i<NUMPIXELS;i++)
    {

      int index = (ledIndex+i) % NUMPIXELS;

      if(index % skipSize != 0)
      {
        pixels.setPixelColor(i, 0, 0, 0);
      }
      else
      {
        pixels.setPixelColor(i, r, g, b);
      }
    }

    pixels.show();
  }

 frame++;
  delay(10);
}