#include "wiring_private.h" // Funzioni di utility (sbi, cbi)
// Gestione flag binarie
#define RFLAG GPIOR0

// Definizioni per scheduler componenti
#define ADC_RATE 20
volatile uint8_t adc_counter = ADC_RATE;

// Lettore ADC
volatile union {
  uint32_t grb32;
  uint8_t grb_v[4];
} grb;

volatile uint32_t old_grb = 0;
#define FLAG_STAMPA 0 // Flag binaria per la stampa

void setup() {
  Serial.begin(9600);

  // inizializza ADC
  ADCSRB = 0;
  ADCSRA = _BV(ADEN) | _BV(ADIE) | 0x7;

  // Inizializza scheduler su COMPA
  OCR0A = 0;
  cbi(TCCR0A, WGM00); // Normal mode
  cbi(TCCR0A, WGM01); // Normal mode
  cbi(TCCR0B, WGM02); // Normal mode
  sbi(TIMSK0, OCIE0A);

  // Inizializzazione PWM su TIMER2
  TCNT2 = 0;
  TCCR2A = _BV(COM2B1) | _BV(WGM21) | _BV(WGM20); // Fast PWM
  TCCR2B = _BV(WGM22) | 0x1; // No prescaler (0x1)
  sbi(DDRD, DDD3); // OC2B su PD3
  sbi(DDRD, DDD2); // Monitor
  cbi(PORTD, PD3);
  OCR2A = 20;
  OCR2B = 12;
  sbi(TIMSK2, OCIE2B);
}

void loop() {
  if(bit_is_set(RFLAG, FLAG_STAMPA)) {
    Serial.println(grb.grb32, HEX);
    cbi(RFLAG, FLAG_STAMPA);
  }
}

ISR(TIMER0_COMPA_vect) {
  if(adc_counter) {
    if(--adc_counter == 0) {
      ADMUX = _BV(REFS0) | _BV(ADLAR) | 0x2;
      sbi(ADCSRA, ADSC); // Start ADC - Faccio partire il registro (gia' impostato)
      adc_counter = ADC_RATE;
    }
  }
}

ISR(ADC_vect) {
  uint8_t i = ADMUX & 0x3;
  grb.grb_v[i] = ADCH;
  if(i) {
    ADMUX = _BV(REFS0) | _BV(ADLAR) | --i;
    sbi(ADCSRA, ADSC);
  } else {
    if(old_grb != grb.grb32) {
      sbi(RFLAG, FLAG_STAMPA); // RFLAG = GPIOR0, lo setto a 1
      old_grb = grb.grb32;
    }
  }
}

ISR(TIMER2_COMPB_vect, ISR_NAKED) {
  sbi(PORTD, 0x2); // Monitor
  asm(
    "push r0  \n"
    "in r0, 0x3f \n"
    "push r0  \n"
    "out  0x3f, r0  \n"
    "pop  r0  \n"
    "reti \n"
  );
  cbi(PORTD, 0x2); // Monitor
}