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


uint16_t minDurata; //misurazione minima tra SX e DX
uint16_t durataS; //misurazione sensore SX
uint16_t durataD; //misurazione sensore DX
uint32_t MYTICKus = 0;
volatile bool buzzerState = 0;
void setup() {
  Serial.begin(115200);
  sbi(DDRB,2); //in output il pin trig sensore S
  cbi(DDRD,2); //in input il pin echo sensore S
  cbi(DDRD,3); //in input il pin echo sensore D
  sbi(DDRD,5); // in output il pin per LED D (OC0A)
  sbi(DDRD,6); // in output il pin per LED S (OC0B)
  TCCR0A = (1 << WGM00) | (1 << WGM01) | (1 << COM0A1) | (1 << COM0B1); // Fast PWM, clear OC0A/OC0B on compare match, set OC0A/OC0B at BOTTOM
  TCCR0B = (1 << CS00) | (1 << CS01); // Prescaler 64
  TCCR1A |= (1 << COM1A0) | (1 << WGM11); // Toggle OC1A on Compare Match, Fast PWM
  TCCR1B |= (1 << WGM12) | (1 << WGM13); // Fast PWM ICR1 on top
  ICR1=130;
  DDRB |= (1 << PB1); //in output pin buzzer
  TCCR1B |= (1 << CS10) | (1 << CS11);// Inizia il timer con prescaler 64  
  EICRA|=(1<<ISC00);// Configura INT0 per attivarsi su qualsiasi cambiamento di stato logico
  EICRA|=(1<<ISC10);// Configura INT1 per attivarsi su qualsiasi cambiamento di stato logico
  EIMSK|=(1<<INT0); // Abilita l'interrupt INT0 (pin 2)
  EIMSK|=(1<<INT1); // Abilita l'interrupt INT1 (pin 3)
  OCR0A = 0; // Duty cycle iniziale per LED D
  OCR0B = 0; // Duty cycle iniziale per LED S
  TCCR2A=0;
  TCCR2B=(1<<CS21); // Prescaler 8
  TIMSK2=(1<<TOIE2); //overflow interrupt
   sei();
}

void loop() {
sensor();
buzzer();
LED();
Display();
}


//sensori 
void sensor() {
  uint32_t tempo=millis();
  sbi(PORTB, 2);
  while(millis()<tempo+24){};
  cbi(PORTB, 2);
  if ((int)durataS / 58.3 < (int)durataD / 58.3) {
    minDurata=(int)durataS/58.3;
  } else {
    minDurata=(int)durataD/58.3;
  }
}

//interrupt sensore Sx
ISR(INT0_vect) {
  static uint32_t Time=0;
  if (PIND & (1<<PIND2)) {
    Time=MYTICKus;
  } else {
    durataS=MYTICKus-Time;
  }
}

// Interrupt sensore Dx
ISR(INT1_vect) {
  static uint32_t Time2=0;
  if (PIND & (1<<PIND3)) {
    Time2=MYTICKus;
  } else {
    durataD=MYTICKus-Time2;
  }
}






//cicalino
void buzzer() {
  static uint32_t lastToggleTime = 0;
  uint32_t currentTime = MYTICKus; 
  if (minDurata <= 5) {
    OCR1A = 125; // 1 KHz
    buzzerState = 1;
  } else if (minDurata < 50) {
    if (currentTime - lastToggleTime >= 71000) { // 7 Hz
      buzzerState = !buzzerState;
      lastToggleTime = currentTime;
    }
    OCR1A = buzzerState ? 125 : 0;
  } else if (minDurata < 80) {
    if (currentTime - lastToggleTime >= 167000) { // 3 Hz
      buzzerState = !buzzerState;
      lastToggleTime = currentTime;
    }
    OCR1A = buzzerState ? 125 : 0;
  } else if (minDurata < 100) {
    if (currentTime - lastToggleTime >= 500000) { // 1 Hz
      buzzerState = !buzzerState;
      lastToggleTime = currentTime;
    }
    OCR1A = buzzerState ? 125 : 0;
  } else {
    OCR1A = 0;
  }
}


//Led
void LED() {
  uint8_t dutyCycleD = 0, dutyCycleS = 0;
  if ((int)(durataD/58.3) < (int)(durataS/58.3)) {
    dutyCycleD = 255; // LED D al 100%
    if ((int)(durataS / 58.3) > 2 * (int)(durataD/58.3)) {
      dutyCycleS = 0; // LED S spento
    } else {
      dutyCycleS=(uint8_t)((2.0 - ((float)(durataS/58.3) / (float)(durataD/58.3)))* 255); // LED S a (2 - dS/dD)%
    }
  } else {
    dutyCycleS = 255; // LED S al 100%
    if ((int)(durataD/58.3) > 2 * (int)(durataS/58.3)) {
      dutyCycleD=0; // LED D spento
    } else {
      dutyCycleD=(uint8_t)((2.0 - ((float)(durataD/58.3)/ (float)(durataS/58.3)))*255); // LED D a (2 - dD/dS)%
    }
  }
  if (dutyCycleS == 0) {
    cbi(TCCR0A, COM0B1); // Disabilita PWM su OC0B
  } else {
    sbi(TCCR0A, COM0B1); // Abilita PWM su OC0B
    OCR0B = dutyCycleS;
  }

  if (dutyCycleD == 0) {
    cbi(TCCR0A, COM0A1); // Disabilita PWM su OC0A
  } else {
    sbi(TCCR0A, COM0A1); // Abilita PWM su OC0A
    OCR0A = dutyCycleD;
  }
//debug
  Serial.print("dD: "); Serial.print((int)(durataD/58.3)); 
  Serial.print(", dS: "); Serial.print((int)(durataS/58.3));
  Serial.print(", DutyCycleD: "); Serial.print(dutyCycleD);
  Serial.print(", DutyCycleS: "); Serial.println(dutyCycleS);
}





//display
void Display(){
  void (*displayDigitDx[10])() = {ZeroDx, UnoDx, DueDx, TreDx, QuattroDx, CinqueDx, SeiDx, SetteDx, OttoDx, NoveDx};
  void (*displayDigitSx[10])() = {ZeroSx, UnoSx, DueSx, TreSx, QuattroSx, CinqueSx, SeiSx, SetteSx, OttoSx, NoveSx};
  if(minDurata<100){
    displayDigitDx[minDurata%10]();
    displayDigitSx[(int)minDurata/10]();
}else{clearBitSx(); PORTC=0b00000000;}
}





//setup display sinistro
void ZeroSx(){
  clearBitSx();
  sbi(PORTC,0); 
  PORTB |= (1 << PINB5) | (1 << PINB4) | (1 << PINB0);
  PORTD |= (1 << PIND4) | (1 << PIND7);
}

void UnoSx(){
  clearBitSx();
  sbi(PORTC,0); 
  sbi(PORTD,4); 
}

void DueSx(){
  clearBitSx(); 
  PORTB |= (1 << PINB5) | (1 << PINB3)| (1 << PINB0);
  sbi(PORTC,0); 
  sbi(PORTD,7); 
  sbi(PORTB,0); 
}

void TreSx(){
  clearBitSx();
  PORTB |= (1 << PINB5) | (1 << PINB3)| (1 << PINB0);
  sbi(PORTC,0); 
  sbi(PORTD,4); 
}

void QuattroSx(){
  clearBitSx();
  PORTB |= (1 << PINB4) | (1 << PINB3);
  sbi(PORTC,0); 
  sbi(PORTD,4);
}

void CinqueSx(){
  clearBitSx();
  PORTB |= (1 << PINB5) | (1 << PINB4) | (1 << PINB3)| (1 << PINB0);
  sbi(PORTD,4);
}

void SeiSx(){
  clearBitSx();
  PORTB |= (1 << PINB5) | (1 << PINB4) | (1 << PINB3)| (1 << PINB0);
  PORTD |= (1 << PIND4) | (1 << PIND7);
}

void SetteSx(){
  clearBitSx();
  sbi(PORTC,0); 
  sbi(PORTB,5); 
  sbi(PORTD,4);
}

void OttoSx(){
  clearBitSx();
  PORTB |= (1 << PINB5) | (1 << PINB4) | (1 << PINB3)| (1 << PINB0);
  PORTD |= (1 << PIND4) | (1 << PIND7);
  sbi(PORTC,0); 
}

void NoveSx(){
  clearBitSx();
  PORTB |= (1 << PINB5) | (1 << PINB4) | (1 << PINB3)| (1 << PINB0);
  sbi(PORTD,4); 
  sbi(PORTC,0); 
}

void clearBitSx(){
  PORTB &= ~((1<<PINB5) | (1<<PINB4) | (1<<PINB3)| (1<<PINB0));
  PORTD &= ~((1<<PIND7) | (1<<PIND4));
  cbi(PORTC,0);
}

//setup display destro
void ZeroDx(){
PORTC=0b11111100;
}
void UnoDx(){
PORTC=0b10010000;
}
void DueDx(){
PORTC=0b01111010;  
}
void TreDx(){
PORTC=0b11011010;  
}
void QuattroDx(){
PORTC=0b10010110;   
}
void CinqueDx(){
PORTC=0b11001110;   
}
void SeiDx(){
PORTC=0b11101110; 
}
void SetteDx(){
PORTC=0b10011000;   
}
void OttoDx(){
PORTC=0b11111110;   
}
void NoveDx(){
PORTC=0b11011110; 
}


ISR(TIMER2_OVF_vect) {
  MYTICKus+=128;
}