#include <Button.h>
#include <Adafruit_NeoPixel.h>

constexpr bool KEEPALIVE_ENABLE = false;
constexpr int KEEPALIVE_TIMEOUT = 5000; // ms

constexpr int LEDS_TAIL_PIN = PA6;
constexpr int LEDS_TAIL_LENGTH = 10;
constexpr int LEDS_TAIL_BRIGHTNESS = 255;

constexpr int LEDS_LEFT_PIN = PA5;
constexpr int LEDS_RIGHT_PIN = PA7;

constexpr int LEDS_TOP_LENGTH = 11;
constexpr int LEDS_SIDE_LENGTH = 2;
constexpr int LEDS_BOTTOM_LENGTH = 10;
constexpr int LEDS_RING_LENGTH = 16;
constexpr int LEDS_EDGE_LENGTH = LEDS_TOP_LENGTH + LEDS_SIDE_LENGTH + LEDS_BOTTOM_LENGTH;
constexpr int LEDS_HEAD_LENGTH = LEDS_TOP_LENGTH + LEDS_SIDE_LENGTH + LEDS_BOTTOM_LENGTH + LEDS_RING_LENGTH;
constexpr int LEDS_HEAD_BRIGHTNESS = 100;

constexpr int LEDS_DASH_PIN = PB0;
constexpr int LEDS_DASH_LENGTH = 10;
constexpr int LEDS_DASH_BRIGHTNESS = 255;

constexpr int PIN_ALIVE = PA2;
constexpr int PIN_BUCK = PA1;

constexpr int PIN_PEDAL = PA0;
constexpr int PIN_PEDAL_FORWARD = PA3;
constexpr int PIN_PEDAL_REVERSE = PA4;

constexpr int PIN_BUTTON_MODE = PB8;
constexpr int PIN_BUTTON_LEFT = PB7;
constexpr int PIN_BUTTON_RIGHT = PB9;

constexpr int LED_ON = LOW;
constexpr int LED_OFF = HIGH;

constexpr int PEDAL_ON = HIGH;
constexpr int PEDAL_OFF = LOW;

constexpr int BUCK_ON = HIGH;
constexpr int BUCK_OFF = LOW;

constexpr int ANIMATION_FPS = 30;
constexpr int ANIMATION_INTERVAL = lround(1000 / ANIMATION_FPS);

Button pedal(PIN_PEDAL);
Button pedalForward(PIN_PEDAL_FORWARD);
Button pedalReverse(PIN_PEDAL_REVERSE);

Button buttonMode(PIN_BUTTON_MODE);
Button buttonLeft(PIN_BUTTON_LEFT);
Button buttonRight(PIN_BUTTON_RIGHT);

Adafruit_NeoPixel ledsTail(LEDS_TAIL_LENGTH, LEDS_TAIL_PIN, NEO_GRBW + NEO_KHZ800);
Adafruit_NeoPixel ledsHeadLeft(LEDS_HEAD_LENGTH, LEDS_LEFT_PIN, NEO_GRBW + NEO_KHZ800);
Adafruit_NeoPixel ledsHeadRight(LEDS_HEAD_LENGTH, LEDS_RIGHT_PIN, NEO_GRBW + NEO_KHZ800);
Adafruit_NeoPixel ledsDash(LEDS_DASH_LENGTH, LEDS_DASH_PIN, NEO_GRBW + NEO_KHZ800);

const uint32_t COLOR_WHITE = ledsTail.Color(0, 0, 0, 255);
const uint32_t COLOR_RED = ledsTail.Color(255, 0, 0, 0);
const uint32_t COLOR_AMBER = ledsTail.Color(255, 50, 0, 0);
const uint32_t COLOR_POLICE_BLUE = ledsTail.Color(0, 0, 255, 0);
const uint32_t COLOR_POLICE_RED = ledsTail.Color(255, 0, 0, 0);

void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(PIN_ALIVE, OUTPUT);  
  pinMode(PIN_BUCK, OUTPUT); 
  pinMode(PIN_PEDAL, INPUT_PULLUP);

  pinMode(PIN_PEDAL_FORWARD, INPUT_PULLUP);
  pinMode(PIN_PEDAL_REVERSE, INPUT_PULLUP);

  pinMode(PIN_BUTTON_MODE, INPUT_PULLUP);
  pinMode(PIN_BUTTON_LEFT, INPUT_PULLUP);
  pinMode(PIN_BUTTON_RIGHT, INPUT_PULLUP);

  // digitalWrite(LED_BUILTIN, LED_ON);
  digitalWrite(PIN_ALIVE, HIGH);
  digitalWrite(PIN_BUCK, BUCK_ON);
  digitalWrite(LED_BUILTIN, LED_OFF);

  ledsTail.setBrightness(LEDS_TAIL_BRIGHTNESS);
  ledsHeadLeft.setBrightness(LEDS_HEAD_BRIGHTNESS);
  ledsHeadRight.setBrightness(LEDS_HEAD_BRIGHTNESS);
  ledsDash.setBrightness(LEDS_DASH_BRIGHTNESS);

  ledsTail.begin();
  ledsHeadLeft.begin();
  ledsHeadRight.begin();
  ledsDash.begin();

  pedal.begin();
  pedalForward.begin();
  pedalReverse.begin();

  buttonMode.begin();
  buttonLeft.begin();
  buttonRight.begin();

  Serial.begin(9600);

  ledsTail.clear();
  ledsHeadLeft.clear();
  ledsHeadRight.clear();
  ledsDash.clear();
}

enum Mode {
  MODE_HEAD,
  MODE_POLICE,
  MODE_HAZARD,
  MODE_RAINBOW,
  MODE_COUNT,
};

Mode activeMode = MODE_HEAD;

unsigned long animationFrame = 0;
unsigned long lastFrame = 0;

void animateHeadlights() {
  ledsHeadLeft.fill(COLOR_WHITE);
  ledsHeadRight.fill(COLOR_WHITE);

  ledsDash.fill(ledsDash.Color(0, 255, 0, 0));
}

void animateIndicator(Adafruit_NeoPixel& ledsTarget) {
  Serial.println("ANIMATING INDICATOR");

  for (int ledIndex = LEDS_EDGE_LENGTH; ledIndex < LEDS_EDGE_LENGTH + LEDS_RING_LENGTH; ledIndex += 1) {
    ledsTarget.setPixelColor(ledIndex, COLOR_AMBER);
  }
}

unsigned long lastPedalPress = millis();

unsigned long lastIndicatingLeft = 0;
unsigned long lastIndicatingRight = 0;
bool isIndicatingLeft = false;
bool isIndicatingRight = false;

void animate(long now) {
  if (pedalForward.read() == Button::PRESSED) {
    ledsTail.fill(ledsTail.Color(255, 0, 0, 0));
    lastPedalPress = now;

    Serial.println("PEDAL FORWARD");
  } else if (pedalReverse.read() == Button::PRESSED) {
    ledsTail.fill(ledsTail.Color(0, 0, 0, 255));
    lastPedalPress = now;

    Serial.println("PEDAL REVERSE");
  } else {
    ledsTail.fill(ledsTail.Color(100, 0, 0, 0));
  }

  // always animate headlights, let other modes override
  animateHeadlights();

  // we need the boolean to prevent the indicators from turning on right after boot
  if (isIndicatingLeft && animationFrame - lastIndicatingLeft < 100) {
    animateIndicator(ledsHeadLeft);
  }

  if (isIndicatingRight && animationFrame - lastIndicatingRight < 100) {
    animateIndicator(ledsHeadRight);
  }
}

// the loop function runs over and over again forever
void loop() {
  const unsigned long now = millis();

  if (buttonMode.pressed()) {
    ledsTail.fill(ledsTail.Color(0, 0, 255, 0));

    activeMode = (Mode)((activeMode + 1) % MODE_COUNT);

    Serial.print("BUTTON MODE ");
    Serial.println(activeMode);
  }

  if (buttonLeft.pressed()) {
    isIndicatingLeft = true;
    lastIndicatingLeft = animationFrame;

    Serial.println("BUTTON LEFT");
  }

  if (buttonRight.pressed()) {
    isIndicatingRight = true;
    lastIndicatingRight = animationFrame;

    Serial.println("BUTTON RIGHT");
  }

  if (now - lastFrame >= ANIMATION_INTERVAL) {
    lastFrame += ANIMATION_INTERVAL;
    animationFrame += 1;

    animate(now);

    ledsTail.show();
    ledsHeadLeft.show();
    ledsHeadRight.show();
    ledsDash.show();
  }

  if (KEEPALIVE_ENABLE && now - lastPedalPress > KEEPALIVE_TIMEOUT) {
    // digitalWrite(PIN_BUCK, BUCK_OFF);
    Serial.println("SHUTTING DOWN");

    Serial.println("BUCK OFF");
    digitalWrite(PIN_BUCK, LOW);

    delay(100);

    Serial.println("SELF OFF, GOODBYE");
    digitalWrite(PIN_ALIVE, LOW);
  }
}