#include <avr/io.h>
#include <avr/interrupt.h>
#include "wiring_private.h"

#define PORT PORTB
#define DDR  DDRB
#define NBIT 0             
#define NPIX 32            
#define RSPACE (NPIX * 3)  

#define CANALE_X 1
#define CANALE_Y 0
#define MAX_X 3
#define MIN_X 0
#define MAX_Y 7
#define MIN_Y 0

#define N 2 // Soglia 1s (2 tick da 524ms)

volatile uint8_t direzione_X = 0; 
volatile uint8_t direzione_Y = 0; 

volatile uint8_t variazione_X = 0;
volatile uint8_t variazione_Y = 0;

volatile uint8_t X = 0;
volatile uint8_t Y = 0;
volatile uint8_t R = 0;
volatile uint8_t G = 0;
volatile uint8_t B = 0;
volatile uint8_t output_da_aggiornare = 0;

uint8_t vpix[RSPACE];

volatile void pix_show(uint8_t nb, uint8_t *vp) {
  sbi(DDR, NBIT);    
  cbi(PORT, NBIT);   
  cli();             
  
  uint8_t cb, buf;
  asm volatile (
  "lwhile0:\n\t"              
    "ld %[buf], %a[vp]+ \n\t"  
    "ldi %[cb], 8 \n\t"        
  "lwhile:\n\t"
    "nop  \n\t"                
    "sbi %[port],%[nbit] \n\t" 
    "rjmp .+0 \n\t"            
    "rjmp .+0 \n\t"            
    "sbrs %[buf], 7  \n\t"     
    "cbi %[port],%[nbit] \n\t" 
    "add  %[buf], %[buf]  \n\t"
    "subi %[cb], 1 \n\t"       
    "breq vnextb \n\t"         
    "rjmp .+0 \n\t"            
    "cbi %[port],%[nbit] \n\t" 
    "rjmp .+0 \n\t"            
    "nop  \n\t"                
    "rjmp lwhile  \n\t"        
  "vnextb:\n\t"                
    "subi %[nb], 1 \n\t"       
    "cbi %[port],%[nbit] \n\t" 
    "brne lwhile0 \n\t"        
    : [vp] "+e" (vp)
    : [nb] "d" (nb), 
      [port] "i" (_SFR_IO_ADDR(PORT)), 
      [nbit] "i" (NBIT),
      [cb] "r" (cb), [buf] "r" (buf) 
  );  

  sei();             
  cbi(PORT, NBIT);   
}

ISR(ADC_vect) {
  static uint8_t canale_corrente = CANALE_X;
  uint8_t stato_bit = ADCH >> 5;

  if (canale_corrente == CANALE_X) {
    if (stato_bit <= 0b001)      variazione_X = 0b11; // -1 (DESTRA)
    else if (stato_bit >= 0b110) variazione_X = 0b01; // +1 (SINISTRA)
    else                         variazione_X = 0b00; 

    ADMUX = (ADMUX & 0xF0) | CANALE_Y;
    canale_corrente = CANALE_Y;
  } else {
    if (stato_bit <= 0b001)      variazione_Y = 0b11; // -1 (BASSO)
    else if (stato_bit >= 0b110) variazione_Y = 0b01; // +1 (ALTO)
    else                         variazione_Y = 0b00; 

    ADMUX = (ADMUX & 0xF0) | CANALE_X;
    canale_corrente = CANALE_X;

    if (variazione_X) direzione_X = variazione_X;
    if (variazione_Y) direzione_Y = variazione_Y;
  }
  
  ADCSRA |= (1 << ADSC);
}

ISR(PCINT0_vect) {
  static uint8_t indice_cifra = 0;
  static uint8_t buffer_temporaneo[5];

  PCICR &= ~(1 << PCIE0);

  uint8_t attivi = (~(PINB >> 1)) & 0x0F;

  if (attivi == 0) {
    if (indice_cifra < 5) {
      PORTD &= ~0xF0;       
      PCIFR |= (1 << PCIF0); 
      PCICR |= (1 << PCIE0); 
    }
    return;
  }

  uint8_t colonna = 0;
  if (attivi & 0b0010)      colonna = 1;
  else if (attivi & 0b0100) colonna = 2;
  else if (attivi & 0b1000) colonna = 3;

  uint8_t bit_colonna = (1 << (PB1 + colonna));
  int8_t riga = -1;

  PORTD = (PORTD | 0xF0) & ~(1 << PD4);
  if (!(PINB & bit_colonna)) {
    riga = 0;
  } else {
    PORTD = (PORTD | 0xF0) & ~(1 << PD5);
    if (!(PINB & bit_colonna)) {
      riga = 1;
    } else {
      PORTD = (PORTD | 0xF0) & ~(1 << PD6);
      if (!(PINB & bit_colonna)) {
        riga = 2;
      } else {
        PORTD = (PORTD | 0xF0) & ~(1 << PD7);
        if (!(PINB & bit_colonna)) {
          riga = 3;
        }
      }
    }
  }

  PORTD &= ~0xF0;

  if (riga != -1) {
    buffer_temporaneo[indice_cifra++] = (uint8_t)((riga << 2) | colonna);

    if (indice_cifra == 5) {
      X = 3 - (buffer_temporaneo[0] & 0b11);
      Y = buffer_temporaneo[1] & 0b111;
      R = (buffer_temporaneo[2] << 4) | buffer_temporaneo[2];
      G = (buffer_temporaneo[3] << 4) | buffer_temporaneo[3];
      B = (buffer_temporaneo[4] << 4) | buffer_temporaneo[4];
      
      // Accensione iniziale pixel configurato
      uint8_t offset = ((Y & 0x07) << 2) | (X & 0x03);
      offset = (offset << 1) + offset;
      vpix[offset]     = G;
      vpix[offset + 1] = R;
      vpix[offset + 2] = B;

      pix_show(RSPACE, vpix);

      output_da_aggiornare = 1; // Abilita il Modulo Timer
      indice_cifra = 0;
    } else {
      PCIFR |= (1 << PCIF0);
      PCICR |= (1 << PCIE0);
    }
  } else {
    if (indice_cifra < 5) {
      PCIFR |= (1 << PCIF0);
      PCICR |= (1 << PCIE0);
    }
  }
}

ISR(TIMER1_COMPA_vect) {
  static uint8_t contatore_pressione = 0;
  uint8_t spostamento_richiesto = 0;

  if (!output_da_aggiornare) return;

  uint8_t in_sollecitazione = (variazione_X != 0b00) || (variazione_Y != 0b00);

  if (in_sollecitazione) {
    contatore_pressione++;
    
    if (contatore_pressione == N) {
      spostamento_richiesto = 1; // Soglia 1s raggiunta
    } 
    else if ((contatore_pressione > N) && ((contatore_pressione % (N >> 1)) == 0)) {
      spostamento_richiesto = 1; // Ripetizione ogni N/2 (512ms)
    }
  } 
  else if (contatore_pressione > 0) {
    if (contatore_pressione < N) {
      spostamento_richiesto = 1;
    }
    contatore_pressione = 0;
  }

  if (spostamento_richiesto) {
    // Calcola l'offset vecchio e spegne il LED corrente
    uint8_t offset = ((Y & 0x07) << 2) | (X & 0x03);
    offset = (offset << 1) + offset;
    vpix[offset]     = 0;
    vpix[offset + 1] = 0;
    vpix[offset + 2] = 0;

    

    if (direzione_X == 0b01 && X < MAX_X)      X++;
    else if (direzione_X == 0b11 && X > MIN_X) X--;

    if (direzione_Y == 0b01 && Y < MAX_Y)      Y++;
    else if (direzione_Y == 0b11 && Y > MIN_Y) Y--;

    direzione_X = 0b00;
    direzione_Y = 0b00;

    uint8_t offset_nuovo = ((Y & 0x07) << 2) | (X & 0x03);
    offset = (offset_nuovo << 1) + offset_nuovo;
    vpix[offset]     = G;
    vpix[offset + 1] = R;
    vpix[offset + 2] = B;

    pix_show(RSPACE, vpix);
  }
}

void setup() {
  cli();

  // Configurazione Tastierino (PB1..PB4 Ingressi, PD4..PD7 Uscite)
  DDRB  &= ~0x1E; 
  PORTB |=  0x1E; 
  DDRD  |=  0xF0; 
  PORTD &= ~0xF0; 
  
  output_da_aggiornare = 0;

  PCIFR  |= (1 << PCIF0); 
  PCMSK0 |= 0x1E;         
  PCICR  |= (1 << PCIE0); 

  // Configurazione ADC (Joystick A0, A1)
  DIDR0  |= (1 << ADC0D) | (1 << ADC1D); 
  ADMUX   = (1 << REFS0) | (1 << ADLAR) | CANALE_X; 
  ADCSRA  = (1 << ADEN) | (1 << ADIE) | 0x07;       
  ADCSRA |= (1 << ADSC);                            

  // Configurazione TIMER1 (CTC Mode, Prescaler 1024, Tick 524ms)
  TCCR1A = 0;             
  TCCR1B = 0;             
  TCNT1  = 0;             
  OCR1A  = 8191;          // Tick a circa 524ms
  TCCR1B |= (1 << WGM12); // CTC Mode
  TCCR1B |= (1 << CS12) | (1 << CS10); // Prescaler 1024
  TIMSK1 |= (1 << OCIE1A);             // Abilita Interrupt Timer1 COMPA

  sei(); // Attiva Interrupt Globali
}

void loop() {
}