#define Trig   PB6
#define Echo   PB1

#define LED_Rouge  PB0
#define LED_orange  PB7
#define LED_VERTE  LED_BUILTIN

#define Switch  A2
#define Buzzer PA8

// ✅ PINS 74HC595 (corrigé pour NUCLEO L031K6)
#define DATA   PB0  // Data
#define Clock PA1  // Clock
#define LATCH  PA3   // Latch

#define MR PA4
#define PWM PA5

// ---------- 7-seg (common cathode) ----------
// bit0=A, bit1=B, bit2=C, bit3=D, bit4=E, bit5=F, bit6=G, bit7=DP
const uint8_t DIGITS[10] = {
  0b00111111, //0
  0b00000110, //1
  0b01011011, //2
  0b01001111, //3
  0b01100110, //4
  0b01101101, //5
  0b01111101, //6
  0b00000111, //7
  0b01111111, //8
  0b01101111  //9
};

static inline void latch595() {
  digitalWrite(RCLK, LOW);
  delayMicroseconds(2);
  digitalWrite(RCLK, HIGH);
  delayMicroseconds(2);
}

static inline void shiftByte(uint8_t data) {
  shiftOut(SER, SRCLK, MSBFIRST, data);
}

// sr2 = dizaine, sr1 = unité
// On envoie d'abord unité puis dizaine (car sr2 est "avant" sr1 dans la chaine)
void display00to99(int value) {
  if (value < 0) value = 0;
  if (value > 99) value = 99;

  int tens = value / 10;
  int ones = value % 10;

  shiftByte(DIGITS[ones]); // sr1 (unités)
  shiftByte(DIGITS[tens]); // sr2 (dizaines)
  latch595();
}

// Affiche la distance en mètres avec une décimale (ex: 1.2 pour 123 cm)
// La distance est limitée à 400 cm (4.0 m) pour rester dans la plage spécifiée
void displayDistanceInMeters(float d_cm) {
  if (d_cm < 0) d_cm = 0;
  if (d_cm > 400) d_cm = 400; // limite à 4.0 m

  // Conversion en dixièmes de mètre (décimètres) avec arrondi
  int tenths_total = (int)((d_cm + 5) / 10); // arrondi au décimètre le plus proche
  int meters = tenths_total / 10;
  int tenths = tenths_total % 10;

  // Sécurité : normalement meters <= 4, tenths <= 9
  if (meters > 9) meters = 9; // ne devrait pas arriver

  // Premier digit (dizaines) = mètres + point décimal allumé
  uint8_t pattern_meters = DIGITS[meters] | 0b10000000;
  // Second digit (unités) = dixièmes
  uint8_t pattern_tenths = DIGITS[tenths];

  // Envoi : d'abord les unités (dixièmes) puis les dizaines (mètres)
  shiftByte(pattern_tenths); // sr1
  shiftByte(pattern_meters); // sr2
  latch595();
}

float readDistanceCm() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  unsigned long duration = pulseIn(ECHO_PIN, HIGH, 30000UL);
  if (duration == 0) return -1;

  return (duration * 0.0343f) / 2.0f;
}

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(LED_ROUGE, OUTPUT);
  pinMode(LED_JAUNE, OUTPUT);
  pinMode(LED_VERTE, OUTPUT);

  pinMode(PIN_INTER, INPUT_PULLUP);
  pinMode(PIN_BUZZER, OUTPUT);

  pinMode(SER, OUTPUT);
  pinMode(SRCLK, OUTPUT);
  pinMode(RCLK, OUTPUT);

  digitalWrite(SER, LOW);
  digitalWrite(SRCLK, LOW);
  digitalWrite(RCLK, HIGH);

  Serial.begin(9600);
  Serial.println("Systeme de stationnement Pret");

  // Test d'affichage : 88 (tous segments sans point)
  display00to99(88);
  delay(800);
}

void loop() {
  bool enabled = (digitalRead(PIN_INTER) == LOW);

  if (!enabled) {
    digitalWrite(LED_ROUGE, LOW);
    digitalWrite(LED_JAUNE, LOW);
    digitalWrite(LED_VERTE, LOW);
    noTone(PIN_BUZZER);
    displayDistanceInMeters(0); // Affiche 0.0
    delay(100);
    return;
  }

  float d = readDistanceCm();
  if (d < 0) {
    noTone(PIN_BUZZER);
    displayDistanceInMeters(0);
    delay(100);
    return;
  }

  Serial.print("Distance: ");
  Serial.print(d / 100.0f, 2);
  Serial.println(" m");

  // Affichage sur les 7 segments en mètres
  displayDistanceInMeters(d);

  if (d < 30) {
    digitalWrite(LED_ROUGE, HIGH);
    digitalWrite(LED_JAUNE, LOW);
    digitalWrite(LED_VERTE, LOW);
    tone(PIN_BUZZER, 440);
  } else if (d <= 120) {
    digitalWrite(LED_ROUGE, LOW);
    digitalWrite(LED_JAUNE, HIGH);
    digitalWrite(LED_VERTE, LOW);

    int intervalle = map((int)d, 30, 120, 50, 800);
    tone(PIN_BUZZER, 1000);
    delay(40);
    noTone(PIN_BUZZER);
    delay(intervalle);
  } else {
    digitalWrite(LED_ROUGE, LOW);
    digitalWrite(LED_JAUNE, LOW);
    digitalWrite(LED_VERTE, HIGH);
    noTone(PIN_BUZZER);
  }

  delay(60);
}