#include <FastLED.h>
#include <pt.h>

#define slowflash 250
#define fastflash 200
#define NUMPIXELS_PASSENGERROOF 4
#define NUMPIXELS_DRIVERROOF 4
#define NUMPIXELS_PASSENGERBUMPER 2
#define NUMPIXELS_DRIVERBUMPER 2
#define NUMPIXELS_PASSENGERBED 2
#define NUMPIXELS_DRIVERBED 2

CRGB PASSENGERROOF[NUMPIXELS_PASSENGERROOF];
CRGB DRIVERROOF[NUMPIXELS_DRIVERROOF];
CRGB PASSENGERBUMPER[NUMPIXELS_PASSENGERBUMPER];
CRGB DRIVERBUMPER[NUMPIXELS_DRIVERBUMPER];
CRGB PASSENGERBED[NUMPIXELS_PASSENGERBED];
CRGB DRIVERBED[NUMPIXELS_DRIVERBED];


static struct pt pt1, pt2, pt3;

void setup() {
  FastLED.addLeds<NEOPIXEL, 0>(PASSENGERROOF, NUMPIXELS_PASSENGERROOF);
  FastLED.addLeds<NEOPIXEL, 1>(DRIVERROOF, NUMPIXELS_DRIVERROOF);
  FastLED.addLeds<NEOPIXEL, 2>(PASSENGERBUMPER, NUMPIXELS_PASSENGERBUMPER);
  FastLED.addLeds<NEOPIXEL, 3>(DRIVERBUMPER, NUMPIXELS_DRIVERBUMPER);
  FastLED.addLeds<NEOPIXEL, 4>(PASSENGERBED, NUMPIXELS_PASSENGERBED);
  FastLED.addLeds<NEOPIXEL, 5>(DRIVERBED, NUMPIXELS_DRIVERBED);


  PT_INIT(&pt1);
  PT_INIT(&pt2);
  PT_INIT(&pt3);

}

static int roofbars(struct pt *pt)
{
  static unsigned long lastTimeBlink = 0;
  PT_BEGIN(pt);

  while (1) {
 
    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    PASSENGERROOF[0] = CRGB::Black; PASSENGERROOF[1] = CRGB::Black; PASSENGERROOF[2] = CRGB::Black; PASSENGERROOF[3] = CRGB::Red;
    DRIVERROOF[0] = CRGB::Black; DRIVERROOF[1] = CRGB::Black; DRIVERROOF[2] = CRGB::Black; DRIVERROOF[3] = CRGB::Blue;
    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    PASSENGERROOF[0] = CRGB::Black; PASSENGERROOF[1] = CRGB::Black; PASSENGERROOF[2] = CRGB(180, 75, 0); PASSENGERROOF[3] = CRGB::Black;
    DRIVERROOF[0] = CRGB::Black; DRIVERROOF[1] = CRGB::Black; DRIVERROOF[2] = CRGB(180, 75, 0); DRIVERROOF[3] = CRGB::Black;
    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    PASSENGERROOF[0] = CRGB::Black; PASSENGERROOF[1] = CRGB::Red; PASSENGERROOF[2] = CRGB::Black; PASSENGERROOF[3] = CRGB::Black;
    DRIVERROOF[0] = CRGB::Black; DRIVERROOF[1] = CRGB::Blue; DRIVERROOF[2] = CRGB::Black; DRIVERROOF[3] = CRGB::Black;
    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    PASSENGERROOF[0] = CRGB(180, 75, 0); PASSENGERROOF[1] = CRGB::Black; PASSENGERROOF[2] = CRGB::Black; PASSENGERROOF[3] = CRGB::Black;
    DRIVERROOF[0] = CRGB(180, 75, 0); DRIVERROOF[1] = CRGB::Black; DRIVERROOF[2] = CRGB::Black; DRIVERROOF[3] = CRGB::Black;

  }
  PT_END(pt);
}

static int bumperlights(struct pt *pt)
{
  static unsigned long lastTimeBlink = 0;
  PT_BEGIN(pt);

  while (1) {

    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    PASSENGERBUMPER[0] = CRGB(180, 75, 0); PASSENGERBUMPER[1] = CRGB::Black;
    DRIVERBUMPER[1] = CRGB(180, 75, 0); DRIVERBUMPER[0] = CRGB::Black;

    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    PASSENGERBUMPER[1] = CRGB(180, 75, 0); PASSENGERBUMPER[0] = CRGB::Black;
    DRIVERBUMPER[0] = CRGB(180, 75, 0); DRIVERBUMPER[1] = CRGB::Black;

    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    PASSENGERBUMPER[1] = CRGB::Black; PASSENGERBUMPER[0] = CRGB(85, 85, 85);
    DRIVERBUMPER[0] = CRGB::Black; DRIVERBUMPER[1] = CRGB(85, 85, 85);

    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    PASSENGERBUMPER[0] = CRGB::Black; PASSENGERBUMPER[1] = CRGB(85, 85, 85);
    DRIVERBUMPER[1] = CRGB::Black; DRIVERBUMPER[0] = CRGB(85, 85, 85);

  }
  PT_END(pt);
}

static int bedlights(struct pt *pt)
{
  static unsigned long lastTimeBlink = 0;
  PT_BEGIN(pt);

  while (1) {
    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    DRIVERBED[0] = CRGB::Red; DRIVERBED[1] = CRGB::Black;
    PASSENGERBED[0] = CRGB(0, 0, 0); PASSENGERBED[1] = CRGB::Blue;
    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    DRIVERBED[0] = CRGB(0, 0, 0); DRIVERBED[1] = CRGB::Red;
    PASSENGERBED[0] = CRGB::Blue; PASSENGERBED[1] = CRGB::Black;
    
    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    DRIVERBED[0] = CRGB(180, 75, 0); DRIVERBED[1] = CRGB::Black;
    PASSENGERBED[0] = CRGB(0, 0, 0); PASSENGERBED[1] = CRGB(180, 75, 0);
    lastTimeBlink = millis(); PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > slowflash);
    DRIVERBED[0] = CRGB(0, 0, 0); DRIVERBED[1] = CRGB(180, 75, 0);
    PASSENGERBED[0] = CRGB(180, 75, 0); PASSENGERBED[1] = CRGB::Black;
  }
  PT_END(pt);
}


void loop() {
  FastLED.show();
  roofbars(&pt1);
  bumperlights(&pt2);
  bedlights(&pt3);
}