#define IN1     2
#define IN2     3
#define IN3     4
#define IN4     5

uint8_t taste, taste_alt, pulselimit;
volatile uint16_t counter = 0;
volatile bool limit_pulses;
volatile uint8_t tccr1b_setup, tccr1b_tmp;

void setup() {
  sei();                          // allow interrupts

  TIMSK1 = 0 | (1 << OCIE1B);

  resetTimer1();

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

  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);         // VCC close to PB6 for indicator LED active LOW
  
  DDRB  |= (1 << PB6);            // Enable OUTPUT PB6
}

void loop() {
  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(); PORTB &= (0 << PB6);    break;
      default: resetTimer1();                         break;
    }
    taste_alt = taste;
  }
  delay(5);
}

void initTimer1 (uint16_t duty, uint16_t periode, uint16_t prescaler, bool limit_pulses_in, uint16_t pulselimit_in) {

  //uint16_t periode = periode in ms / prescaler * 16.000
  //uint16_t duty    =    duty in ms * prescaler / 16.000

  //duty <= periode
  if (pulselimit_in > 1){
    pulselimit = pulselimit_in - 1;           // Pulselimit -1
  }
  else {
    pulselimit = 0;
  }
  limit_pulses = limit_pulses_in;           // Limit Pulses N/Y

  resetTimer1();

  TCCR1A = 0 | (1 << WGM11) | (1 << WGM10) | (1 << COM1B0) | (1 << COM1B1);  
  tccr1b_setup = 0 | (1 << WGM13) | (1 << WGM12);

  OCR1A = periode - 1;
  OCR1B = duty - 1;

  switch (prescaler) {
    case    1 : tccr1b_setup |= (1 << CS10);                break;  // prescaler 1
    case    8 : tccr1b_setup |= (1 << CS11);                break;  // prescaler 8
    case   64 : tccr1b_setup |= (1 << CS11) | (1 << CS10);  break;  // prescaler 64
    case  256 : tccr1b_setup |= (1 << CS12);                break;  // prescaler 256
    case 1024 : tccr1b_setup |= (1 << CS12) | (1 << CS10);  break;  // prescaler 1024
    default   : resetTimer1();                        break;        // otherwise timer remains stopped
  }
  
  tccr1b_tmp = tccr1b_setup;
  
  if (counter >= pulselimit & limit_pulses == 1) {
    tccr1b_setup = 0;                               // TCCR1B = 0 on next interrupt to stop timer
  }

  TCCR1B = tccr1b_tmp;
}

void resetTimer1 () {
  PORTB |= (1 << PB6);                              // PulsePin HIGH
  
  TCCR1B = 0;                                       // Reset, stop timer first
  TCCR1A = 0 | (1 << COM1B0) | (1 << COM1B1);       // Set on match
  TCCR1C = (1 << FOC1B);                            // Force match

  TCCR1A = 0;                                       // Reset

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

//  Serial.println(counter);                          // Print counter value
  counter = 0;                                      // Reset counter
}

ISR(TIMER1_COMPB_vect) {                            // Interrupt if duty is reached
  TCCR1B = tccr1b_setup;
  counter++;
  if (counter >= pulselimit & limit_pulses == 1) {
    tccr1b_setup = 0;                               // TCCR1B = 0 on next interrupt to stop timer
    if (TCCR1B == 0) {
      TCCR1A = 0 | (1 << COM1B0) | (1 << COM1B1);   // Set on match
      TCCR1C = (1 << FOC1B);                        // Force match
    }
  }
}