//Diego

#include <Adafruit_NeoPixel.h>
#define PIN 6

int armStripLength = 45;
int torsoStripLength = 74;
int legStripLength = 60;
int totalStripLength = 179;
boolean ledsStatus[179];

//int armStripLength = 14;
//int torsoStripLength = 25;
//int legStripLength = 20;
//int totalStripLength = 59;
//boolean ledsStatus[59];

boolean clearNext = false;
boolean toClear = false;

int legStart = armStripLength + torsoStripLength;
int halfwayStart = totalStripLength/2;

int fsr1; //Chest
int fsr2; //Hip
int fsr3; //Back

boolean fsr1IsTouched = false;
boolean fsr2IsTouched = false;
boolean fsr3IsTouched = false;

int fsr1ArmCount = 0;
int fsr1TorsoCount = 0;
int fsr2TorsoCount = 0;
int fsr2LegCount = 0;
int fsr3UpCount = 0;
int fsr3DownCount = 0;

boolean allLedsOn = false;

long danceLength = 300000;
int lastStep;
int currentStep;
int stepLength;

Adafruit_NeoPixel strip = Adafruit_NeoPixel(totalStripLength, PIN, NEO_GRB + NEO_KHZ800);

float red = 34.0;
float green = 186.0;
float blue = 172.0;

int r, g, b;

float firstRed = (133-34) / 100.0;
float firstGreen = (196-186) / 100.0;
float firstBlue = (65-172) / 100.0;

float secondRed = (255-133) / 100.0;
float secondGreen = (235-196) / 100.0;
float secondBlue = (0-65) / 100.0;

int fsr1Pin = 2; //analog pin 0
int fsr2Pin = 4;
int fsr3Pin = 0;

int pulseNum = 0;
boolean pulseUp = true;

void setup() {
  Serial.begin(9600);
  strip.begin();
  strip.show();
  currentStep = 0;
  currentStep = millis();
  stepLength = 1000;
  strip.setBrightness(32);
}

void loop() {
  initStrip();
  clearLEDs();
  changeColor();
  checkSensors();
  updateStrips();
  strip.show();
  delay(10);
}

void initStrip() {
  if (!clearNext) {
     clearNext = false; 
    toClear = true;
  } else {
    if (toClear) {
      for (int i=0; i<strip.numPixels(); i++)
      {
        strip.setPixelColor(i, 0);
        ledsStatus[i] = false;
      }
      toClear = false;
    }
  }
  
  if (toClear) {
    for (int i=0; i<strip.numPixels(); i++)
    {
      strip.setPixelColor(i, 0);
      ledsStatus[i] = false;
    }
    toClear = false;
  }
}

void clearLEDs() {
  for (int i=0; i<strip.numPixels(); i++)
  {
    strip.setPixelColor(i, 0);
  }
}

void randomFlash(int fromLed, int toLed) {
  int numOfLeds = random(5); 
  
  for (int i=0; i < numOfLeds; i++) {
    ledsStatus[random(fromLed,toLed+1)] = true;
  }
  allLedsOn = true;
  for (int i=fromLed; i < toLed; i++) {
    if (!ledsStatus[i]) {
      allLedsOn = false;
      break;
    }
  }
  if (allLedsOn) {
    ledPulse(); 
  }
}

void ledPulse() {
   if (pulseUp) {
      if (pulseNum < 20) {
        pulseNum++;
      } else {
        pulseUp = false;
      }
   } else {
      if (pulseNum > -20) {
        pulseNum--;
      } else {
        pulseUp = true;
      }
   }
   
   if (r+pulseNum < 255 && r+pulseNum > 0) r+=pulseNum;
   if (g+pulseNum < 255 && g+pulseNum > 0) g+=pulseNum;
   if (b+pulseNum < 255 && b+pulseNum > 0) b+=pulseNum;
   
   delay(10);
}

void spark(int fromLed, int toLed) {
  for (int i=fromLed; i<=toLed; i++)
  {
    strip.setPixelColor(i,r,g,b);
    ledsStatus[i] = true;
  }
}

void updateStrips() {
  
  //Chest Sensor//
  
  if (fsr1 > 512) {
    if (fsr1IsTouched) {
      if (fsr1TorsoCount < torsoStripLength-1) {
        //randomFlash(0,legStart);
        fsr1TorsoCount++;
        ledsStatus[fsr1TorsoCount+armStripLength] = true;
        
        if (fsr1ArmCount > -armStripLength) {
          fsr1ArmCount--;
          ledsStatus[armStripLength+fsr1ArmCount] = true;
        }
      } else {
        ledPulse(); 
      }
    } else {
      ledsStatus[armStripLength+fsr1ArmCount] = true;
      ledsStatus[fsr1TorsoCount+armStripLength] = true;
      fsr1IsTouched = true;
    }
  } else if (fsr1IsTouched && fsr1 < 256) {
    if (fsr1TorsoCount < torsoStripLength) {
      spark(0, legStart-3); 
      clearNext = true;
    }
    
    toClear = true;
    fsr1IsTouched = false;
    fsr1ArmCount = 0;
    fsr1TorsoCount = 0;
  }
  
  //Hip Sensor//
  
  if (fsr2 > 512) {
    if (fsr2IsTouched) {
      if (fsr2TorsoCount > -torsoStripLength+1) {
        //randomFlash(armStripLength,totalStripLength);
        fsr2TorsoCount--;
        ledsStatus[legStart-1+fsr2TorsoCount] = true;
        
        if (fsr2LegCount < legStripLength) {
          fsr2LegCount++;
          ledsStatus[fsr2LegCount+legStart-1] = true;
        }
      } else {
        ledPulse(); 
      }
    } else {
      
      ledsStatus[legStart-1-fsr2TorsoCount] = true;
      ledsStatus[fsr2LegCount+legStart] = true;
      fsr2IsTouched = true;
    }
  } else if (fsr2IsTouched && fsr2 < 256) {
    if (fsr2TorsoCount > -torsoStripLength) {
      spark(armStripLength, totalStripLength); 
      clearNext = true;
    }
    
    toClear = true;
    fsr2IsTouched = false;
    fsr2TorsoCount = 0;
    fsr2LegCount = 0;
  }
  
  //Back Sensor//
  
  if (fsr3 > 512) {
    if (fsr3IsTouched) {
      if (fsr3UpCount > -halfwayStart) {
        //randomFlash(0,totalStripLength);
        fsr3UpCount--;
        ledsStatus[halfwayStart-1+fsr3UpCount] = true;   
        
        fsr3DownCount++;
        ledsStatus[fsr3DownCount+halfwayStart] = true;
      } else {
        ledPulse(); 
      }
    } else {
      ledsStatus[halfwayStart-1-fsr3UpCount] = true;
      ledsStatus[fsr3DownCount+halfwayStart] = true;
      fsr3IsTouched = true;
    }
  } else if (fsr3IsTouched && fsr3 < 256) {
    if (fsr3UpCount > -halfwayStart) {
      spark(0, totalStripLength); 
      clearNext = true;
    }
    
    toClear = true;
    fsr3IsTouched = false;
    fsr3UpCount = 0;
    fsr3DownCount = 0;
  }
  
  /////////////////////////////////////////////////
  
  for (int i=0; i<sizeof(ledsStatus); i++) {
    if (ledsStatus[i]) {
      strip.setPixelColor(i,r,g,b); 
    }
  }
}

void checkSensors() {
  fsr1 = analogRead(fsr1Pin);
  fsr2 = analogRead(fsr2Pin);
  fsr3 = analogRead(fsr3Pin);
  //fsr3 = 513;
  //Serial.println(fsr1);
  
}

void changeColor() {
  currentStep = millis();
  
  if ((currentStep - lastStep) > stepLength) {
    
    if (millis() < danceLength/3) {
      red += firstRed;
      green += firstGreen;
      blue += firstBlue; 
    } else if (millis() >= danceLength/3 && millis() < 2*(danceLength/3)) {
       red += secondRed;
       green += secondGreen;
       blue += secondBlue; 
    } else {
      Serial.println(millis());
      Serial.print(red);
      Serial.print(' ');
      Serial.print(green);
      Serial.print(' ');
      Serial.println(blue);
      
    }
    
    lastStep = currentStep;
  }
  
  
  r = red;
  g = green;
  b = blue;
}