// ------------------ Pins ------------------
const int trig = PA9;
const int echo = PA10;

#define LED_BUILTIN PA5
#define ORANGE_LED PB7
#define RED_LED PB0
#define buzzer PA12
#define SWITCH_PIN PB6

// 74HC595 séparés
#define DS_UNITS PA6     // Afficheur unités
#define DS_TENS  PA8     // Afficheur dizaines
#define SH_CP    PA7
#define ST_CP    PA4

long duration;
float distance;

// ------------------ Table ANODE COMMUNE ------------------
const byte SEG[10] = {
  0b00111111, // 0
  0b00000110, // 1
  0b01011011, // 2
  0b01001111, // 3
  0b01100110, // 4
  0b01101101, // 5
  0b01111101, // 6
  0b00000111, // 7
  0b01111111, // 8
  0b01101111  // 9
};

// ------------------ Setup ------------------
void setup() {
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(ORANGE_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);
  pinMode(buzzer, OUTPUT);

  pinMode(SWITCH_PIN, INPUT_PULLUP);

  pinMode(DS_UNITS, OUTPUT);
  pinMode(DS_TENS, OUTPUT);
  pinMode(SH_CP, OUTPUT);
  pinMode(ST_CP, OUTPUT);

  Serial.begin(115200);
}

// ------------------ Shift 1 byte ------------------
void shiftOutByte(byte data, int DS){
  for(int i=7;i>=0;i--){
    digitalWrite(DS, (data >> i) & 1);
    digitalWrite(SH_CP, HIGH);
    digitalWrite(SH_CP, LOW);
  }
}

// ------------------ Latch ------------------
void latch(){
  digitalWrite(ST_CP, HIGH);
  digitalWrite(ST_CP, LOW);
}

// ------------------ Affichage 2 digits ------------------
void displayTwoDigits(int tens, int units){
  byte unitsData = SEG[units] & 0b01111111; // point décimal ON
  byte tensData  = SEG[tens];

  // Envoyer d'abord dizaines puis unités
  shiftOutByte(tensData, DS_TENS);
  shiftOutByte(unitsData, DS_UNITS);
  latch();
}

// ------------------ Loop ------------------
void loop() {

  if(digitalRead(SWITCH_PIN) == HIGH){

    // ----------- Mesure distance -----------
    digitalWrite(trig, LOW);
    delayMicroseconds(2);
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);

    duration = pulseIn(echo, HIGH);
    distance = (duration * 0.0343) / 2; // cm

    // ----------- UART -----------
    Serial.print("Distance: ");
    Serial.print(distance / 100.0, 1);
    Serial.println(" m");

    // ----------- LEDs -----------
    if(distance > 120){
      digitalWrite(LED_BUILTIN, HIGH);
      digitalWrite(ORANGE_LED, LOW);
      digitalWrite(RED_LED, LOW);
    }
    else if(distance >= 30){
      digitalWrite(LED_BUILTIN, LOW);
      digitalWrite(ORANGE_LED, HIGH);
      digitalWrite(RED_LED, LOW);
    }
    else{
      digitalWrite(LED_BUILTIN, LOW);
      digitalWrite(ORANGE_LED, LOW);
      digitalWrite(RED_LED, HIGH);
    }

    // ----------- Affichage 7seg ---------
    int tens = 0;
    int units = 0;

    if(distance >= 100){
      tens = (int)(distance / 100);
      units = ((int)(distance / 10)) % 10;
    } else {
      tens = 0;
      units = (int)(distance / 10);
    }

    displayTwoDigits(tens, units);

    // ----------- Buzzer avec intervalle ---------
    if(distance <= 120){
      int interval = map(distance, 30, 120, 0, 1000);
      if(interval < 0) interval = 0;

      tone(buzzer, 440);
      delay(interval);
      noTone(buzzer);
      delay(interval);
    } else {
      noTone(buzzer);
    }

    // ----------- Delay lisible pour lecture ----------
    delay(500); // plus grand delay pour lire l'affichage

  } else {
    // OFF
    digitalWrite(LED_BUILTIN, LOW);
    digitalWrite(ORANGE_LED, LOW);
    digitalWrite(RED_LED, LOW);
    noTone(buzzer);

    displayTwoDigits(0, 0);
  }
}