#include <Arduino.h>
#include <digitalWriteFast.h>

#include "timer.h"
#include "pindef.h"

void init_timer() {
  TCCR2A = 0; // set TCCRXA register to 0
  TCCR2B = 0; // set TCCRXB register to 0
  TCNT2  = 0; // reset counter value
  OCR2A = 255; //Compare Value
  TCCR2A |= (1 << WGM21); // enable timer2 CTC mode
  TIMSK2 |= (1 << OCIE2A); // enable timer2 compare interrupt
  TCCR2B |= (1 << CS20); //Prescale 1
}
volatile int i = 0;
volatile int j = 0;

volatile int led_value[3][7] = {
    {7, 0, 0, 7, 7, 0, 0},
    {0, 7, 0, 7, 0, 7, 0},
    {0, 0, 7, 7, 0, 0, 7}
};

//Multiplex / PWM
ISR(TIMER2_COMPA_vect) { // called by timer2
  i++;
  if(i > 6) {
    i = 0;
    j++;
    if(j>7) j=0;
  }

  pinModeFast(SEL_R, OUTPUT);
  pinModeFast(SEL_G, OUTPUT);
  pinModeFast(SEL_B, OUTPUT);

  digitalWriteFast(SEL_R, false);
  digitalWriteFast(SEL_G, false);
  digitalWriteFast(SEL_B, false);

  pinModeFast(LED_1, false);
  pinModeFast(LED_2, false);
  pinModeFast(LED_3, false);
  pinModeFast(LED_4, false);
  pinModeFast(LED_5, false);
  pinModeFast(LED_6, false);
  pinModeFast(LED_7, false);

  switch(i) {
    case 0 : pinModeFast(LED_1, true); break;
    case 1 : pinModeFast(LED_2, true); break;
    case 2 : pinModeFast(LED_3, true); break;
    case 3 : pinModeFast(LED_4, true); break;
    case 4 : pinModeFast(LED_5, true); break;
    case 5 : pinModeFast(LED_6, true); break;
    case 6 : pinModeFast(LED_7, true); break;
}

  if(j < led_value[0][i]) { digitalWriteFast(SEL_R, true); }
  if(j < led_value[1][i]) { digitalWriteFast(SEL_G, true); }
  if(j < led_value[2][i]) { digitalWriteFast(SEL_B, true); }
}
