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

// Arduino ATtiny85 @ 8 MHz internal clock
// Drives up to 6 servos with Timer0 in CTC mode
// PORTB &= mask to set & reset pins
// Pulse resolution: 250 µs steps



#define NUM_SERVOS 6
// Arduino pin numbers (ATtiny85 pin mapping)
// PB0=0, PB1=1, PB2=2, PB3=3, PB4=4, PB5=5
const uint8_t servoPin[NUM_SERVOS] = {0, 1, 2, 3, 4, 5};

// Pulse widths in ticks of 250 µs
// Example: 2=0.5ms, 6=1.5ms, 10=2.5ms
// volatile uint8_t servoPulse[NUM_SERVOS] = {3, 4, 5, 6, 7, 8};
volatile uint8_t servoPulse[NUM_SERVOS] = {10, 10, 10, 10, 10, 10};

volatile int8_t servoTicks = 0;
volatile int8_t frameTicks = 0;
volatile int8_t frameCounter = 0;    // counts 20ms frames
volatile int8_t currentServo = 0;
volatile uint8_t currentServoMask;
volatile int8_t nextServo = 1;
volatile uint8_t nextServoMask;


ISR(TIMER0_COMPA_vect) {
  servoTicks++;

  if (servoTicks == servoPulse[currentServo]) {
    // End pulse
    PORTB &= ~currentServoMask;
  }

  // === Frame reset every 20 ms ===
  if (servoTicks == 10) { // 10 × 250 µs = 2.5 ms
    // Start pulse for current servo
    PORTB |= nextServoMask;

    currentServoMask = nextServoMask;
    currentServo = nextServo;
    if (nextServo && nextServo < NUM_SERVOS - 1) {
      nextServo++;
      nextServoMask = 1 << servoPin[nextServo];
    }
    else {
      nextServo = 0;
      nextServoMask = 0;
    }
    servoTicks = 0;
    frameTicks++;
    if (frameTicks ==  8) {
      currentServoMask = 1 << servoPin[0];
      PORTB |= currentServoMask;
      nextServo = 1;
      nextServoMask = 1 << servoPin[1];
      frameTicks = 0;
      frameCounter++;
    }
  }
}


void setup() {
  PORTB &= ~((1 << PB0) | (1 << PB1) | (1 << PB2) | (1 << PB3) | (1 << PB4) | (1 << PB5)); // start LOW
  DDRB |= (1 << PB0) | (1 << PB1) | (1 << PB2) | (1 << PB3) | (1 << PB4) | (1 << PB5); // 6 outputs
  // ATtiny85 only has PB0..PB5, so PB5 is RESET unless configured as I/O

  // Setup Timer0
  TCCR0A = (1 << WGM01); // CTC mode
  TCCR0B = (1 << CS01);  // prescaler 8
  OCR0A = 249;           // 250us
  TIMSK = (1 << OCIE0A); // enable compare match A interrupt

  currentServoMask = 1 << servoPin[0];
  nextServoMask = 1 << servoPin[1];

  sei();
}

void loop() {
  if (frameCounter = 25) { // ~0.5 seconds
    frameCounter = 0;
    // uint8_t s = random(NUM_SERVOS);
    // uint8_t pw = random(2, 10); // 0.5 ms (2 ticks) to 2.5 ms (10 ticks)
    // servoPulse[s] = pw;
  }
}