#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// Define PCA9685 addresses
#define PCA9685_ADDRESS_1  0x40
#define PCA9685_ADDRESS_2  0x41
#define PCA9685_ADDRESS_3  0x42
#define PCA9685_ADDRESS_4  0x43



Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(PCA9685_ADDRESS_1, Wire);
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(PCA9685_ADDRESS_2, Wire);
Adafruit_PWMServoDriver pwm3 = Adafruit_PWMServoDriver(PCA9685_ADDRESS_3, Wire);
Adafruit_PWMServoDriver pwm4 = Adafruit_PWMServoDriver(PCA9685_ADDRESS_4, Wire);
Adafruit_PWMServoDriver PWMs[4]={pwm1, pwm2, pwm3, pwm4};


void setup() {

  for (int x = 0;x < 4; x++) {
    PWMs[x].begin();
    PWMs[x].setPWMFreq(1000);
  }
  delay(10);

}

/*
  chipWireTest
  trafficAdvisorL
  trafficAdvisorR
  trafficAdvisorM
  trafficAdvisorS
  unnamedPattern
  unnamedPattern2
  doubleDual
*/

void loop() {



}

void setLED(uint8_t ledIndex, uint8_t Rvalue, uint8_t Gvalue, uint8_t Bvalue, Adafruit_PWMServoDriver &pwm) {
  Rvalue = constrain(Rvalue, 0, 255);
  Gvalue = constrain(Gvalue, 0, 255);
  Bvalue = constrain(Bvalue, 0, 255);

  uint16_t redPWM = (Rvalue == 0) ? 4096 : map(Rvalue, 0, 255, 0, 4095);
  uint16_t greenPWM = (Gvalue == 0) ? 4096 : map(Gvalue, 0, 255, 0, 4095);
  uint16_t bluePWM = (Bvalue == 0) ? 4096 : map(Bvalue, 0, 255, 0, 4095);

  pwm.setPWM((ledIndex - 1) * 3, 4096, redPWM);
  pwm.setPWM((ledIndex - 1) * 3 + 1, 4096, greenPWM);
  pwm.setPWM((ledIndex - 1) * 3 + 2, 4096, bluePWM);
}


void checkSwitch() {


  if (digitalRead(11) == 0)
  {
    allOff();
  }
}

//-------------------
//PATTERNS
//-------------------

void chipWireTest()
{
  setLED(1, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm1);
  setLED(3, 255, 0, 0, pwm1);
  setLED(4, 255, 0, 0, pwm1);
  setLED(5, 255, 0, 0, pwm1);
  delay(300);
  setLED(1, 255, 0, 0, pwm2);
  setLED(2, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm2);
  setLED(4, 255, 0, 0, pwm2);
  setLED(5, 255, 0, 0, pwm2);
  delay(300);
  setLED(1, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm3);
  setLED(3, 255, 0, 0, pwm3);
  setLED(4, 255, 0, 0, pwm3);
  setLED(5, 255, 0, 0, pwm3);
  delay(300);
  setLED(1, 255, 0, 0, pwm4);
  setLED(2, 255, 0, 0, pwm4);
  setLED(3, 255, 0, 0, pwm4);
  setLED(4, 255, 0, 0, pwm4);
  setLED(5, 255, 0, 0, pwm4);
  delay(300);
  setLED(1, 0, 255, 0, pwm1);
  setLED(2, 0, 255, 0, pwm1);
  setLED(3, 0, 255, 0, pwm1);
  setLED(4, 0, 255, 0, pwm1);
  setLED(5, 0, 255, 0, pwm1);
  delay(300);
  setLED(1, 0, 255, 0, pwm2);
  setLED(2, 0, 255, 0, pwm2);
  setLED(3, 0, 255, 0, pwm2);
  setLED(4, 0, 255, 0, pwm2);
  setLED(5, 0, 255, 0, pwm2);
  delay(300);
  setLED(1, 0, 255, 0, pwm3);
  setLED(2, 0, 255, 0, pwm3);
  setLED(3, 0, 255, 0, pwm3);
  setLED(4, 0, 255, 0, pwm3);
  setLED(5, 0, 255, 0, pwm3);
  delay(300);
  setLED(1, 0, 255, 0, pwm4);
  setLED(2, 0, 255, 0, pwm4);
  setLED(3, 0, 255, 0, pwm4);
  setLED(4, 0, 255, 0, pwm4);
  setLED(5, 0, 255, 0, pwm4);
  delay(300);
  setLED(1, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm1);
  setLED(3, 0, 0, 255, pwm1);
  setLED(4, 0, 0, 255, pwm1);
  setLED(5, 0, 0, 255, pwm1);
  delay(300);
  setLED(1, 0, 0, 255, pwm2);
  setLED(2, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm2);
  setLED(4, 0, 0, 255, pwm2);
  setLED(5, 0, 0, 255, pwm2);
  delay(300);
  setLED(1, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm3);
  setLED(3, 0, 0, 255, pwm3);
  setLED(4, 0, 0, 255, pwm3);
  setLED(5, 0, 0, 255, pwm3);
  delay(300);
  setLED(1, 0, 0, 255, pwm4);
  setLED(2, 0, 0, 255, pwm4);
  setLED(3, 0, 0, 255, pwm4);
  setLED(4, 0, 0, 255, pwm4);
  setLED(5, 0, 0, 255, pwm4);
  delay(300);
}

void trafficAdvisorL()
{
  setLED(3, 255, 255, 0, pwm3);
  delay(250);
  setLED(4, 255, 255, 0, pwm3);
  delay(250);
  setLED(5, 255, 255, 0, pwm3);
  delay(250);
  setLED(1, 255, 255, 0, pwm4);
  delay(250);
  setLED(2, 255, 255, 0, pwm4);
  delay(250);
  setLED(3, 255, 255, 0, pwm4);
  delay(250);
  setLED(3, 0, 0, 0, pwm3);
  setLED(4, 0, 0, 0, pwm3);
  setLED(5, 0, 0, 0, pwm3);
  setLED(1, 0, 0, 0, pwm4);
  setLED(2, 0, 0, 0, pwm4);
  setLED(3, 0, 0, 0, pwm4);
}

void trafficAdvisorR()
{
  setLED(3, 255, 255, 0, pwm4);
  delay(250);
  setLED(2, 255, 255, 0, pwm4);
  delay(250);
  setLED(1, 255, 255, 0, pwm4);
  delay(250);
  setLED(5, 255, 255, 0, pwm3);
  delay(250);
  setLED(4, 255, 255, 0, pwm3);
  delay(250);
  setLED(3, 255, 255, 0, pwm3);
  delay(250);
  setLED(3, 0, 0, 0, pwm3);
  setLED(4, 0, 0, 0, pwm3);
  setLED(5, 0, 0, 0, pwm3);
  setLED(1, 0, 0, 0, pwm4);
  setLED(2, 0, 0, 0, pwm4);
  setLED(3, 0, 0, 0, pwm4);
}

void trafficAdvisorM()
{
  setLED(5, 255, 255, 0, pwm3);
  setLED(1, 255, 255, 0, pwm4);
  delay(250);
  setLED(4, 255, 255, 0, pwm3);
  setLED(2, 255, 255, 0, pwm4);
  delay(250);
  setLED(3, 255, 255, 0, pwm3);
  setLED(3, 255, 255, 0, pwm4);
  delay(250);
  setLED(3, 0, 0, 0, pwm3);
  setLED(4, 0, 0, 0, pwm3);
  setLED(5, 0, 0, 0, pwm3);
  setLED(1, 0, 0, 0, pwm4);
  setLED(2, 0, 0, 0, pwm4);
  setLED(3, 0, 0, 0, pwm4);
  delay(250);
}



void trafficAdvisorS()
{
  subTAS1();
  subTAS1();
  subTAS1();
  subTAS1();
  subTAS1();
  subTAS2();
  subTAS2();
  subTAS2();
  subTAS2();
  subTAS2();
  subTAS3();
  subTAS3();
  subTAS3();
  subTAS3();
  subTAS3();
}

void allOff()
{
  setLED(1, 0, 0, 0, pwm1);
  setLED(2, 0, 0, 0, pwm1);
  setLED(3, 0, 0, 0, pwm1);
  setLED(4, 0, 0, 0, pwm1);
  setLED(5, 0, 0, 0, pwm1);
  setLED(1, 0, 0, 0, pwm2);
  setLED(2, 0, 0, 0, pwm2);
  setLED(3, 0, 0, 0, pwm2);
  setLED(4, 0, 0, 0, pwm2);
  setLED(5, 0, 0, 0, pwm2);
  setLED(1, 0, 0, 0, pwm3);
  setLED(2, 0, 0, 0, pwm3);
  setLED(3, 0, 0, 0, pwm3);
  setLED(4, 0, 0, 0, pwm3);
  setLED(5, 0, 0, 0, pwm3);
  setLED(1, 0, 0, 0, pwm4);
  setLED(2, 0, 0, 0, pwm4);
  setLED(3, 0, 0, 0, pwm4);
  setLED(4, 0, 0, 0, pwm4);
  setLED(5, 0, 0, 0, pwm4);

}


void allRed()
{
  setLED(1, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm1);
  setLED(3, 255, 0, 0, pwm1);
  setLED(4, 255, 0, 0, pwm1);
  setLED(5, 255, 0, 0, pwm1);
  setLED(1, 255, 0, 0, pwm2);
  setLED(2, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm2);
  setLED(4, 255, 0, 0, pwm2);
  setLED(5, 255, 0, 0, pwm2);
  setLED(1, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm3);
  setLED(3, 255, 0, 0, pwm3);
  setLED(4, 255, 0, 0, pwm3);
  setLED(5, 255, 0, 0, pwm3);
  setLED(1, 255, 0, 0, pwm4);
  setLED(2, 255, 0, 0, pwm4);
  setLED(3, 255, 0, 0, pwm4);
  setLED(4, 255, 0, 0, pwm4);
  setLED(5, 255, 0, 0, pwm4);
}

void allBlue()
{
  setLED(1, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm1);
  setLED(3, 0, 0, 255, pwm1);
  setLED(4, 0, 0, 255, pwm1);
  setLED(5, 0, 0, 255, pwm1);
  setLED(1, 0, 0, 255, pwm2);
  setLED(2, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm2);
  setLED(4, 0, 0, 255, pwm2);
  setLED(5, 0, 0, 255, pwm2);
  setLED(1, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm3);
  setLED(3, 0, 0, 255, pwm3);
  setLED(4, 0, 0, 255, pwm3);
  setLED(5, 0, 0, 255, pwm3);
  setLED(1, 0, 0, 255, pwm4);
  setLED(2, 0, 0, 255, pwm4);
  setLED(3, 0, 0, 255, pwm4);
  setLED(4, 0, 0, 255, pwm4);
  setLED(5, 0, 0, 255, pwm4);
}

void leftSideRed()
{
  setLED(1, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm1);
  setLED(3, 255, 0, 0, pwm1);
  setLED(4, 255, 0, 0, pwm1);
  setLED(5, 255, 0, 0, pwm1);
  setLED(1, 255, 0, 0, pwm4);
  setLED(2, 255, 0, 0, pwm4);
  setLED(3, 255, 0, 0, pwm4);
  setLED(4, 255, 0, 0, pwm4);
  setLED(5, 255, 0, 0, pwm4);
}

void rightSideRed()
{
  setLED(1, 255, 0, 0, pwm2);
  setLED(2, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm2);
  setLED(4, 255, 0, 0, pwm2);
  setLED(5, 255, 0, 0, pwm2);
  setLED(1, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm3);
  setLED(3, 255, 0, 0, pwm3);
  setLED(4, 255, 0, 0, pwm3);
  setLED(5, 255, 0, 0, pwm3);
}

void leftSideBlue()
{
  setLED(1, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm1);
  setLED(3, 0, 0, 255, pwm1);
  setLED(4, 0, 0, 255, pwm1);
  setLED(5, 0, 0, 255, pwm1);
  setLED(1, 0, 0, 255, pwm4);
  setLED(2, 0, 0, 255, pwm4);
  setLED(3, 0, 0, 255, pwm4);
  setLED(4, 0, 0, 255, pwm4);
  setLED(5, 0, 0, 255, pwm4);
}

void rightSideBlue()
{
  setLED(1, 0, 0, 255, pwm2);
  setLED(2, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm2);
  setLED(4, 0, 0, 255, pwm2);
  setLED(5, 0, 0, 255, pwm2);
  setLED(1, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm3);
  setLED(3, 0, 0, 255, pwm3);
  setLED(4, 0, 0, 255, pwm3);
  setLED(5, 0, 0, 255, pwm3);
}

//------------------------------------------------

void patternTest()
{
  allRed();
  checkSwitch();
  delay(50);
  checkSwitch();
  allOff();
  checkSwitch();
  delay(15);
  checkSwitch();
  allRed();
  checkSwitch();
  delay(50);
  checkSwitch();
  allOff();
  checkSwitch();
  delay(15);
  checkSwitch();
  allRed();
  checkSwitch();
  delay(50);
  checkSwitch();
  allOff();
  checkSwitch();
  delay(150);
  checkSwitch();
  allBlue();
  checkSwitch();
  delay(50);
  checkSwitch();
  allOff();
  checkSwitch();
  delay(15);
  checkSwitch();
  allBlue();
  checkSwitch();
  delay(50);
  checkSwitch();
  allOff();
  checkSwitch();
  delay(15);
  checkSwitch();
  allBlue();
  checkSwitch();
  delay(50);
  checkSwitch();
  allOff();
  checkSwitch();
  delay(150);
  checkSwitch();

}


void doubleDual()
{
  leftSideRed();
  delay(60);
  allOff();
  delay(20);
  leftSideRed();
  delay(60);
  allOff();
  delay(20);
  rightSideRed();
  delay(60);
  allOff();
  rightSideRed();
  delay(60);
  allOff();
  rightSideRed();
  delay(60);
  allOff();
  leftSideBlue();
  delay(60);
  allOff();
  delay(20);
  leftSideBlue();
  delay(60);
  allOff();
  delay(20);
  rightSideBlue();
  delay(60);
  allOff();
  rightSideBlue();
  delay(60);
  allOff();
  rightSideBlue();
  delay(60);
  allOff();

}
void unnamedPattern()
{
  setLED(1, 255, 0, 0, pwm1);
  setLED(3, 255, 0, 0, pwm1);
  setLED(5, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm2);
  setLED(4, 255, 0, 0, pwm2);
  setLED(6, 255, 0, 0, pwm2);
  setLED(1, 255, 0, 0, pwm3);
  setLED(3, 255, 0, 0, pwm3);
  setLED(5, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm4);
  setLED(4, 255, 0, 0, pwm4);
  setLED(6, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 255, 0, 0, pwm1);
  setLED(3, 255, 0, 0, pwm1);
  setLED(5, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm2);
  setLED(4, 255, 0, 0, pwm2);
  setLED(6, 255, 0, 0, pwm2);
  setLED(1, 255, 0, 0, pwm3);
  setLED(3, 255, 0, 0, pwm3);
  setLED(5, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm4);
  setLED(4, 255, 0, 0, pwm4);
  setLED(6, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 255, 0, 0, pwm1);
  setLED(3, 255, 0, 0, pwm1);
  setLED(5, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm2);
  setLED(4, 255, 0, 0, pwm2);
  setLED(6, 255, 0, 0, pwm2);
  setLED(1, 255, 0, 0, pwm3);
  setLED(3, 255, 0, 0, pwm3);
  setLED(5, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm4);
  setLED(4, 255, 0, 0, pwm4);
  setLED(6, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(2, 255, 0, 0, pwm1);
  setLED(4, 255, 0, 0, pwm1);
  setLED(6, 255, 0, 0, pwm1);
  setLED(1, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm2);
  setLED(5, 255, 0, 0, pwm2);
  setLED(2, 255, 0, 0, pwm3);
  setLED(4, 255, 0, 0, pwm3);
  setLED(6, 255, 0, 0, pwm3);
  setLED(1, 255, 0, 0, pwm4);
  setLED(3, 255, 0, 0, pwm4);
  setLED(5, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(2, 255, 0, 0, pwm1);
  setLED(4, 255, 0, 0, pwm1);
  setLED(6, 255, 0, 0, pwm1);
  setLED(1, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm2);
  setLED(5, 255, 0, 0, pwm2);
  setLED(2, 255, 0, 0, pwm3);
  setLED(4, 255, 0, 0, pwm3);
  setLED(6, 255, 0, 0, pwm3);
  setLED(1, 255, 0, 0, pwm4);
  setLED(3, 255, 0, 0, pwm4);
  setLED(5, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(2, 255, 0, 0, pwm1);
  setLED(4, 255, 0, 0, pwm1);
  setLED(6, 255, 0, 0, pwm1);
  setLED(1, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm2);
  setLED(5, 255, 0, 0, pwm2);
  setLED(2, 255, 0, 0, pwm3);
  setLED(4, 255, 0, 0, pwm3);
  setLED(6, 255, 0, 0, pwm3);
  setLED(1, 255, 0, 0, pwm4);
  setLED(3, 255, 0, 0, pwm4);
  setLED(5, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 0, 0, 255, pwm1);
  setLED(3, 0, 0, 255, pwm1);
  setLED(5, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm2);
  setLED(4, 0, 0, 255, pwm2);
  setLED(6, 0, 0, 255, pwm2);
  setLED(1, 0, 0, 255, pwm3);
  setLED(3, 0, 0, 255, pwm3);
  setLED(5, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm4);
  setLED(4, 0, 0, 255, pwm4);
  setLED(6, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 0, 0, 255, pwm1);
  setLED(3, 0, 0, 255, pwm1);
  setLED(5, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm2);
  setLED(4, 0, 0, 255, pwm2);
  setLED(6, 0, 0, 255, pwm2);
  setLED(1, 0, 0, 255, pwm3);
  setLED(3, 0, 0, 255, pwm3);
  setLED(5, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm4);
  setLED(4, 0, 0, 255, pwm4);
  setLED(6, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 0, 0, 255, pwm1);
  setLED(3, 0, 0, 255, pwm1);
  setLED(5, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm2);
  setLED(4, 0, 0, 255, pwm2);
  setLED(6, 0, 0, 255, pwm2);
  setLED(1, 0, 0, 255, pwm3);
  setLED(3, 0, 0, 255, pwm3);
  setLED(5, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm4);
  setLED(4, 0, 0, 255, pwm4);
  setLED(6, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(2, 0, 0, 255, pwm1);
  setLED(4, 0, 0, 255, pwm1);
  setLED(6, 0, 0, 255, pwm1);
  setLED(1, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm2);
  setLED(5, 0, 0, 255, pwm2);
  setLED(2, 0, 0, 255, pwm3);
  setLED(4, 0, 0, 255, pwm3);
  setLED(6, 0, 0, 255, pwm3);
  setLED(1, 0, 0, 255, pwm4);
  setLED(3, 0, 0, 255, pwm4);
  setLED(5, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(2, 0, 0, 255, pwm1);
  setLED(4, 0, 0, 255, pwm1);
  setLED(6, 0, 0, 255, pwm1);
  setLED(1, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm2);
  setLED(5, 0, 0, 255, pwm2);
  setLED(2, 0, 0, 255, pwm3);
  setLED(4, 0, 0, 255, pwm3);
  setLED(6, 0, 0, 255, pwm3);
  setLED(1, 0, 0, 255, pwm4);
  setLED(3, 0, 0, 255, pwm4);
  setLED(5, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(2, 0, 0, 255, pwm1);
  setLED(4, 0, 0, 255, pwm1);
  setLED(6, 0, 0, 255, pwm1);
  setLED(1, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm2);
  setLED(5, 0, 0, 255, pwm2);
  setLED(2, 0, 0, 255, pwm3);
  setLED(4, 0, 0, 255, pwm3);
  setLED(6, 0, 0, 255, pwm3);
  setLED(1, 0, 0, 255, pwm4);
  setLED(3, 0, 0, 255, pwm4);
  setLED(5, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
}


void unnamedPattern2()
{
  setLED(1, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm1);
  setLED(5, 255, 0, 0, pwm1);
  setLED(1, 255, 0, 0, pwm2);
  setLED(4, 255, 0, 0, pwm2);
  setLED(5, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm3);
  setLED(4, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm4);
  setLED(3, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm1);
  setLED(5, 255, 0, 0, pwm1);
  setLED(1, 255, 0, 0, pwm2);
  setLED(4, 255, 0, 0, pwm2);
  setLED(5, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm3);
  setLED(4, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm4);
  setLED(3, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm1);
  setLED(5, 255, 0, 0, pwm1);
  setLED(1, 255, 0, 0, pwm2);
  setLED(4, 255, 0, 0, pwm2);
  setLED(5, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm3);
  setLED(4, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm4);
  setLED(3, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(3, 255, 0, 0, pwm1);
  setLED(4, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm2);
  setLED(1, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm3);
  setLED(5, 255, 0, 0, pwm3);
  setLED(1, 255, 0, 0, pwm4);
  setLED(4, 255, 0, 0, pwm4);
  setLED(5, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(3, 255, 0, 0, pwm1);
  setLED(4, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm2);
  setLED(1, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm3);
  setLED(5, 255, 0, 0, pwm3);
  setLED(1, 255, 0, 0, pwm4);
  setLED(4, 255, 0, 0, pwm4);
  setLED(5, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(3, 255, 0, 0, pwm1);
  setLED(4, 255, 0, 0, pwm1);
  setLED(2, 255, 0, 0, pwm2);
  setLED(3, 255, 0, 0, pwm2);
  setLED(1, 255, 0, 0, pwm3);
  setLED(2, 255, 0, 0, pwm3);
  setLED(5, 255, 0, 0, pwm3);
  setLED(1, 255, 0, 0, pwm4);
  setLED(4, 255, 0, 0, pwm4);
  setLED(5, 255, 0, 0, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm1);
  setLED(5, 0, 0, 255, pwm1);
  setLED(1, 0, 0, 255, pwm2);
  setLED(4, 0, 0, 255, pwm2);
  setLED(5, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm3);
  setLED(4, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm4);
  setLED(3, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm1);
  setLED(5, 0, 0, 255, pwm1);
  setLED(1, 0, 0, 255, pwm2);
  setLED(4, 0, 0, 255, pwm2);
  setLED(5, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm3);
  setLED(4, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm4);
  setLED(3, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(1, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm1);
  setLED(5, 0, 0, 255, pwm1);
  setLED(1, 0, 0, 255, pwm2);
  setLED(4, 0, 0, 255, pwm2);
  setLED(5, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm3);
  setLED(4, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm4);
  setLED(3, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(3, 0, 0, 255, pwm1);
  setLED(4, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm2);
  setLED(1, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm3);
  setLED(5, 0, 0, 255, pwm3);
  setLED(1, 0, 0, 255, pwm4);
  setLED(4, 0, 0, 255, pwm4);
  setLED(5, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(3, 0, 0, 255, pwm1);
  setLED(4, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm2);
  setLED(1, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm3);
  setLED(5, 0, 0, 255, pwm3);
  setLED(1, 0, 0, 255, pwm4);
  setLED(4, 0, 0, 255, pwm4);
  setLED(5, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
  setLED(3, 0, 0, 255, pwm1);
  setLED(4, 0, 0, 255, pwm1);
  setLED(2, 0, 0, 255, pwm2);
  setLED(3, 0, 0, 255, pwm2);
  setLED(1, 0, 0, 255, pwm3);
  setLED(2, 0, 0, 255, pwm3);
  setLED(5, 0, 0, 255, pwm3);
  setLED(1, 0, 0, 255, pwm4);
  setLED(4, 0, 0, 255, pwm4);
  setLED(5, 0, 0, 255, pwm4);
  delay(60);
  allOff();
  delay(20);
}





//SUB

void subTAS1()
{
  setLED(2, 0, 0, 0, pwm4);
  setLED(5, 0, 0, 0, pwm3);
  setLED(3, 0, 0, 0, pwm3);
  setLED(3, 255, 255, 0, pwm4);
  setLED(1, 255, 255, 0, pwm4);
  setLED(4, 255, 255, 0, pwm3);
  delay(250);
  setLED(3, 0, 0, 0, pwm4);
  setLED(1, 0, 0, 0, pwm4);
  setLED(4, 0, 0, 0, pwm3);
  setLED(2, 255, 255, 0, pwm4);
  setLED(5, 255, 255, 0, pwm3);
  setLED(3, 255, 255, 0, pwm3);
  delay(250);
}

void subTAS2()
{
  setLED(2, 0, 0, 0, pwm4);
  setLED(5, 0, 0, 0, pwm3);
  setLED(3, 0, 0, 0, pwm3);
  setLED(3, 255, 255, 0, pwm4);
  setLED(1, 255, 255, 0, pwm4);
  setLED(4, 255, 255, 0, pwm3);
  delay(100);
  setLED(3, 0, 0, 0, pwm4);
  setLED(1, 0, 0, 0, pwm4);
  setLED(4, 0, 0, 0, pwm3);
  setLED(2, 255, 255, 0, pwm4);
  setLED(5, 255, 255, 0, pwm3);
  setLED(3, 255, 255, 0, pwm3);
  delay(100);
}

void subTAS3()
{
  setLED(1, 0, 0, 0, pwm4);
  setLED(2, 0, 0, 0, pwm4);
  setLED(3, 0, 0, 0, pwm4);
  setLED(3, 255, 255, 0, pwm3);
  setLED(4, 255, 255, 0, pwm3);
  setLED(5, 255, 255, 0, pwm3);
  delay(200);
  setLED(3, 0, 0, 0, pwm3);
  setLED(4, 0, 0, 0, pwm3);
  setLED(5, 0, 0, 0, pwm3);
  setLED(1, 255, 255, 0, pwm4);
  setLED(2, 255, 255, 0, pwm4);
  setLED(3, 255, 255, 0, pwm4);
  delay(200);
}