// Attiny85 sleep, interrupt with servo
// https://forum.arduino.cc/t/attiny85-sleep-interrupt-with-servo/1399429

#include <stdint.h>

// Define the variadic macro
#define APPLY_BIT_1(x1)                       bit(x1)
#define APPLY_BIT_2(x1,x2)                    bit(x1) | APPLY_BIT_1(x2)
#define APPLY_BIT_3(x1,x2,x3)                 bit(x1) | APPLY_BIT_2(x2,x3)
#define APPLY_BIT_4(x1,x2,x3,x4)              bit(x1) | APPLY_BIT_3(x2,x3,x4)
#define APPLY_BIT_5(x1,x2,x3,x4,x5)           bit(x1) | APPLY_BIT_4(x2,x3,x4,x5)
#define APPLY_BIT_6(x1,x2,x3,x4,x5,x6)        bit(x1) | APPLY_BIT_5(x2,x3,x4,x5,x6)
#define APPLY_BIT_7(x1,x2,x3,x4,x5,x6,x7)     bit(x1) | APPLY_BIT_6(x2,x3,x4,x5,x6,x7)
#define APPLY_BIT_8(x1,x2,x3,x4,x5,x6,x7,x8)  bit(x1) | APPLY_BIT_7(x2,x3,x4,x5,x6,x7,x8)

#define GET_MACRO(_1,_2,_3,_4,_5,_6,_7,_8,NAME,...) NAME
#define APPLY_BITS(...) GET_MACRO(__VA_ARGS__, \
  APPLY_BIT_8, APPLY_BIT_7, APPLY_BIT_6, APPLY_BIT_5, \
  APPLY_BIT_4, APPLY_BIT_3, APPLY_BIT_2, APPLY_BIT_1)(__VA_ARGS__)

// Main macros
#define SetPinsAsOutput(port, ...) ((port) |= APPLY_BITS(__VA_ARGS__))
#define SetPinsAsInput(port, ...) ((port) &= ~(APPLY_BITS(__VA_ARGS__)))



#include <avr/sleep.h>

#define servoPin PB0
#define LED PB1
#define piezoSensor PB2
#define LED2 PB3
#define LED_ON_DURATION 2000
#define BLINK_RATE 200

volatile uint32_t switchLedOffNow = 0;

// Interrupt Service Routine for PCINT0
ISR(PCINT0_vect) {
  GIMSK &= ~bit(PCIE);  // Turn off pin change interrupts
  sleep_disable();
  // pinMode(servoPin, OUTPUT);
  // pinMode(LED, OUTPUT);
  // pinMode(LED2, OUTPUT);
  // DDRB |= bit(servoPin) | bit(LED) | bit(LED2); //  Do it in one go
  SetPinsAsOutput(DDRB, servoPin, LED, LED2);
  digitalWrite(LED, HIGH);
  switchLedOffNow = millis() + LED_ON_DURATION;
}

void sleep() {
  // Disable all OUTPUTs to save even more energy
  // pinMode(servoPin, INPUT);
  // pinMode(LED, INPUT);
  // pinMode(LED2, INPUT);
  // DDRB &= ~(bit(servoPin) | bit(LED) | bit(LED2)); //  Do it in one go
  SetPinsAsInput(DDRB, servoPin, LED, LED2);
  sleep_enable();
  cli();
  // Turn on pin change interrupts
  GIMSK |= 1 << PCIE;
  PCMSK |= 1 << piezoSensor ;

  // Enable global interrupts
  sei();
  sleep_cpu();
}


void setup() {
  // Debug.begin();
  // Debug.println(F("Hello Tiny!"));
  pinMode(servoPin, OUTPUT);
  digitalWrite(LED, LOW);
  pinMode(LED, OUTPUT);

  // Disable ADC - saves about 324.5uA in sleep mode!
  ADCSRA &= ~bit(ADEN);  // Disable ADC
  ACSR |= bit(ACD);      // Disable analog comparator
  set_sleep_mode(SLEEP_MODE_PWR_DOWN);

  sleep();
}

void blinkIfAlive() {
  static uint32_t nextBlink = 0;
  if (millis() > nextBlink) {
    digitalWrite(LED2, !digitalRead(LED2));
    nextBlink += BLINK_RATE;  // #define BLINK_RATE 200 or whatever
  }
}


// uint32_t i = 0;
void loop() {
  // static uint32_t nextSecond = millis() + 1000;

  // i++;
  // if (millis() > nextSecond) {
  //   Debug.print(i);
  //   Debug.println(F(" loops/s"));
  //   nextSecond += 1000;
  //   i = 0;
  // }

  if (digitalRead(LED)) {
    // Do whatever has to be done

    if (millis() > switchLedOffNow) {
      digitalWrite(LED, LOW);
      sleep();
    }
  }

  blinkIfAlive();
}