#define IN1     2
#define IN2     3
#define IN3     4
#define IN4     5
#define OUT     10  // PB6, OC1B
#define TCCR1B_MODE ((1 << WGM13) | (1 << WGM12))

volatile uint16_t counter;
volatile uint8_t tccr1b_setup;

void setup() {
  TIMSK1 |= (1 << OCIE1B);
  TCCR1B = 0;
  resetTimer1(HIGH);

  pinMode(IN1, INPUT_PULLUP);
  pinMode(IN2, INPUT_PULLUP);
  pinMode(IN3, INPUT_PULLUP);
  pinMode(IN4, INPUT_PULLUP);
  pinMode(OUT, OUTPUT);

  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);         // VCC close to PB6 for indicator LED active LOW  
}

void loop() {
  static uint32_t last;
  uint32_t now;

  // cooperative multi tasking
  // process keys every 5ms
  
  now = millis();
  if ((now-last) >= 5) {
    last = now;
    process_keys();
  }

  // do a lot of other things here!
}

void process_keys(void) {
  static uint8_t taste_alt;
  uint8_t taste;
  
  taste = 0;
  if (!digitalRead(IN1))      taste = 1; // highest priority
  else if (!digitalRead(IN2)) taste = 2;
  else if (!digitalRead(IN3)) taste = 3;
  else if (!digitalRead(IN4)) taste = 4; // lowest priority
  
  if (taste != taste_alt) {
    switch (taste) {
      case 1:  initTimer1( 40,    80,  1, 1, 2);  break;
      case 2:  initTimer1(640,  3200,  1, 0, 0);  break;
      case 3:  initTimer1(250, 25000, 64, 0, 0);  break;
      case 4:  resetTimer1(LOW);                  break;
      default: resetTimer1(HIGH);                 break;
    }
    taste_alt = taste;
  }
}

void initTimer1 (uint16_t duty, uint16_t periode, uint16_t prescaler, bool pulse_limit, uint16_t pulse_cnt) {
  uint8_t tccr1b_tmp;
  
  //uint16_t periode = periode in ms / prescaler * 16.000
  //uint16_t duty    =    duty in ms * prescaler / 16.000

  resetTimer1(HIGH);
  TCCR1A = (1 << WGM11) | (1 << WGM10) | (1 << COM1B0) | (1 << COM1B1);  
  tccr1b_tmp = TCCR1B_MODE;
  if (duty > periode) duty = periode;
  OCR1A = periode - 1;
  OCR1B = duty - 1;
  
  switch (prescaler) {
    case    1 : tccr1b_tmp |= (1 << CS10);                break;
    case    8 : tccr1b_tmp |= (1 << CS11);                break;
    case   64 : tccr1b_tmp |= (1 << CS11) | (1 << CS10);  break;
    case  256 : tccr1b_tmp |= (1 << CS12);                break;
    case 1024 : tccr1b_tmp |= (1 << CS12) | (1 << CS10);  break;
    default   : break;                                    // otherwise timer remains stopped
  }
  tccr1b_setup = tccr1b_tmp; 
  
  if (pulse_limit && pulse_cnt > 0) {
    counter = pulse_cnt - 1;
    if (pulse_cnt == 1) tccr1b_setup = TCCR1B_MODE; // TCCR1B prescaler bits = 0 on next interrupt to stop timer 
  } else {
    counter = 0;
  }

  TCCR1B = tccr1b_tmp;    // start timer 1
}

void resetTimer1 (bool polarity) {

  tccr1b_setup = TCCR1B_MODE;                       // stop timer 1, keep mode config
  while ((TCCR1B & 0x7) != 0);                      // wait for synchronous stop of timer 1
  counter = 0;

  if (polarity) {
    TCCR1A = (1 << WGM11) | (1 << WGM10) | (1 << COM1B0) | (1 << COM1B1);  // Set on match
  } else {
    TCCR1A = (1 << WGM11) | (1 << WGM10) | (1 << COM1B1);   // Clear on match
  } 
  TCCR1C = (1 << FOC1B);                            // Force match, switch IO pin

  OCR1A  = 0;                                       // Reset periode
  OCR1B  = 0;                                       // Reset duty
  TCNT1  = 0;                                       // initialize counter value to 0
}

ISR(TIMER1_COMPB_vect) {                            // Interrupt when duty is reached
  TCCR1B = tccr1b_setup;
  uint16_t tmp = counter;                           // temporary copy for faster access to volatile variable
  if (tmp) {
    tmp--;
    if (!tmp) tccr1b_setup = TCCR1B_MODE;           // clear prescaler bits, stop timer on next ISR call
    counter = tmp; 
  }
}