#include <Toggle.h>

#define setPin   digitalWrite
#define getPin   digitalRead
#define prnt     Serial.print
#define prntln   Serial.println

const byte buttonPin = 2;
const byte LED = 12;
const byte LED2 = 3;
const unsigned int retrigTime = 500;

bool canTrig = false;

Toggle BTN_Trig(buttonPin);

void setup() {
  Serial.begin(9600);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(LED, OUTPUT);
  pinMode(LED2, OUTPUT);

  BTN_Trig.begin(buttonPin);
}


void Fire() {
  while (BTN_Trig.isPressed()) {
    BTN_Trig.poll();

    if ((BTN_Trig.getElapsedMs() % 1000) < 500) {
      digitalWrite(LED,HIGH);
      if ((BTN_Trig.getElapsedMs() % 1000) > 125) {
        if ((BTN_Trig.getElapsedMs() % 1000) < 250) 
          digitalWrite(LED2,HIGH);
        else 
          digitalWrite(LED2,LOW);
      }
      
    } else {
      digitalWrite(LED,LOW); digitalWrite(LED2,LOW);
    }
  }  
}

void CheckInputs() {
  BTN_Trig.poll();
  
  if (BTN_Trig.onPress()) {
    if (canTrig) {
      setPin(LED, HIGH);
      setPin(LED2, HIGH);
      BTN_Trig.clearTimer();
    }
  } else if (BTN_Trig.onRelease()) {
    setPin(LED, LOW);
    setPin(LED2, LOW);
    if (canTrig) BTN_Trig.clearTimer();
  }

  if (BTN_Trig.isPressed()) {
    if (BTN_Trig.getElapsedMs() < 1000) {
      if (BTN_Trig.pressedFor(250)) setPin(LED2, LOW);
      if (BTN_Trig.pressedFor(500)) setPin(LED, LOW);
    } else {
      Fire();
    }
  } else {
    if (BTN_Trig.releasedFor(500)) {
      setPin(LED_BUILTIN, HIGH);
      canTrig = true;
    } else {
      setPin(LED_BUILTIN, LOW);
      canTrig = false;
    }
  }
}

void loop() {
  CheckInputs();
  delay(1);
}

/*
bool inRange(uint16_t val, int min, int max){
  return ((min <= val) && (val <= max));
}
*/

/*
void Fire() {
  while (BTN_Trig.isPressed()) {
    BTN_Trig.poll();

    if ((BTN_Trig.getElapsedMs() % 1000) < 500) {
      digitalWrite(LED,HIGH);
      if ((BTN_Trig.getElapsedMs() % 1000) > 250) digitalWrite(LED2,HIGH);
    } else {
      digitalWrite(LED,LOW);digitalWrite(LED2,LOW);
    }
  }  
}
*/

/*
void CheckInputs() {
  BTN_Trig.poll();
  
  if (BTN_Trig.onPress()) {
    if (canTrig) {
      setPin(LED, HIGH);
      setPin(LED2, HIGH);
      BTN_Trig.clearTimer();
    }
  } else if (BTN_Trig.onRelease()) {
    setPin(LED, LOW);
    setPin(LED2, LOW);
    if (canTrig) BTN_Trig.clearTimer();
  }

  if (BTN_Trig.isPressed()) {
    if (BTN_Trig.getElapsedMs() < 1000) {
      if (BTN_Trig.pressedFor(250)) setPin(LED2, LOW);
      if (BTN_Trig.pressedFor(500)) setPin(LED, LOW);
    } else {
      Fire();
    }

    
    //if ( inRange(BTN_Trig.getElapsedMs(), 250, 500) ) setPin(LED2, LOW);
    //else if ( inRange(BTN_Trig.getElapsedMs(), 500, 1000) ) setPin(LED,  LOW); 
    //else if (BTN_Trig.getElapsedMs() > 1000) Fire();
    
    //if (BTN_Trig.pressedFor(250)) setPin(LED2, LOW);
    //if (BTN_Trig.pressedFor(500)) setPin(LED, LOW);
    //if (BTN_Trig.pressedFor(1000)) Fire();
    

  } else {
    if (BTN_Trig.releasedFor(500)) {
      setPin(LED_BUILTIN, HIGH);
      canTrig = true;
    } else {
      setPin(LED_BUILTIN, LOW);
      canTrig = false;
    }
  }
}
*/

/*
void CheckInputs() {
  BTN_Trig.poll();
  
  if (BTN_Trig.onPress()) {
    if (canTrig) BTN_Trig.clearTimer();
  } else if (BTN_Trig.onRelease()) {
    if (canTrig) BTN_Trig.clearTimer();
  }

  if (BTN_Trig.isPressed()) {
      if (BTN_Trig.pressedFor(500)) {
        setPin(LED, LOW);
      } else if (BTN_Trig.pressedFor(250)) {
        setPin(LED2, LOW);
      } else {
        if (canTrig) {
          setPin(LED, HIGH);
          setPin(LED2, HIGH);
        }
      } 
      Serial.println(BTN_Trig.getElapsedMs());
  } else {
    setPin(LED, LOW);
    setPin(LED2, LOW);
    if (BTN_Trig.releasedFor(500)) {
      setPin(LED_BUILTIN, HIGH);
      canTrig = true;
    } else {
      setPin(LED_BUILTIN, LOW);
      canTrig = false;
    }
  }
}
*/