#define DEBUGLEDPIN 13
#define DEBUGSPEED 15624
#define CHARLIESPEED 24

volatile int brightnessPerLED[4] = { 0x000, 0x000, 0x000, 0x000 };

void setup() {
  pinMode(DEBUGLEDPIN, OUTPUT);
  configureInterupt_ArduinoUno();
  //configureInterupt_ATtiny85();
}

void loop() {
  recalculateColorBrightnessLED();
  delay(200);
}

//*****************Interrupt code**********************************
void configureInterupt_ArduinoUno() {
  //cli();
  noInterrupts();
  
  TCCR1A = 0; //set entire TCCR1A register to 0
  TCCR1B = 0; //same

  //set compare match register to desired timer count:
  OCR1A = CHARLIESPEED;
  //int timer1_counter = 34286;
  //TCNT1 = 34286;

  //turn on CTC Mode
  TCCR1B |= (1 << WGM12);
  //TCCR1B = 1<<WGM12 | 7<<CS10;

  //Set CS10 and CS12 bits for 1024 prescaler
  TCCR1B |= (1 << CS10);
  //TCCR1B |= (1 << CS12);

  //enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);

  //enable timer overflow interrupt
  //TIMSK1 |= (1 << TOIE1);

  interrupts();
  //sei(); //enable global interrupts

  //digitalWrite(DEBUGLEDPIN, HIGH);  
}

void configureInterupt_ATtiny85() {
  // Set up Timer/Counter1 to multiplex the display

  //UNCOMMENT THIS:
  
  //TCCR1 = 1<<CTC1 | 7<<CS10;    // CTC mode; divide by 64
  //OCR1C = CHARLIESPEED;                   // Divide by 25 -> 5kHz
  //TIMSK = TIMSK | 1<<OCIE1A;    // Enable overflow interrupt
}

//Timer/Counter1 interrupt - multiplexes display

//ATTiny versie: 
//ISR(TIM1_COMPA_vect) {
//Arduino versie:
ISR (TIMER1_COMPA_vect) {
  displayNextLEDRow();
}


//*****************LED aanstuur code**********************************
void recalculateColorBrightnessLED() { //called by loop at certain frequency
  static int Step = 0;

  for (int i=0; i<4; i++) {
    brightnessPerLED[i] = green(Step + i*12)<<8 | blue(Step + i*12)<<4 | red(Step + i*12);
  }
  Step++;
}

void displayNextLEDRow() { //called by interrupt at certain frequency
  //digitalWrite(DEBUGLEDPIN, digitalRead(DEBUGLEDPIN) ^ 1); 

  static int cycle = 0;
  DDRB = DDRB & ~(1<<(cycle & 0x03));
  cycle = (cycle + 1) & 0x3F;   // 64 cycles
  int led = cycle & 0x03;
  int count = cycle>>2;
  int rgb = brightnessPerLED[led];
  int r = rgb & 0x0F;
  int b = rgb>>4 & 0x0F;
  int g = rgb>>8 & 0x0F;
  int bits = (count < r) | (count < b)<<1 | (count < g)<<2;
  bits = bits + (bits & 0x07<<led);
  DDRB = (DDRB & 0xF0) | bits;
  PORTB = (PORTB & 0xF0) | bits;
  DDRB = DDRB | 1<<led;  
}


//*****************Helper functies**********************************
int red (int x) {
  int y = x % 48;
  if (y > 15) y = 31 - y;
  return max(y, 0);
}

int green (int x) { return red(x + 32); }
int blue (int x) { return red(x + 64); }