#include <Wire.h>
#include <LiquidCrystal.h>

// Inisialisasi LCD (RS, E, D4, D5, D6, D7)
LiquidCrystal lcd(PA1, PA2, PA4, PA11, PA12, PB11);

// Konstanta ukuran ruangan (cm)
#define ROOM_LENGTH 1000
#define ROOM_WIDTH 800
#define ROOM_HEIGHT 1000

// MUX1 untuk TRIG
#define MUX1_S0 PB0
#define MUX1_S1 PB1
#define MUX1_S2 PB2
#define MUX1_S3 PB3
#define MUX1_SIG PA5

// MUX2 untuk ECHO
#define MUX2_S0 PB0
#define MUX2_S1 PB1
#define MUX2_S2 PB2
#define MUX2_S3 PB3
#define MUX2_SIG PA6

// Channel sensor ultrasonik
#define SENSOR_1_TRIG_CHANNEL 0
#define SENSOR_2_TRIG_CHANNEL 2
#define SENSOR_3_TRIG_CHANNEL 4
#define SENSOR_4_TRIG_CHANNEL 6
#define SENSOR_5_TRIG_CHANNEL 8
#define SENSOR_6_TRIG_CHANNEL 10
#define SENSOR_7_TRIG_CHANNEL 11
#define SENSOR_8_TRIG_CHANNEL 12

#define SENSOR_1_ECHO_CHANNEL 1
#define SENSOR_2_ECHO_CHANNEL 3
#define SENSOR_3_ECHO_CHANNEL 5
#define SENSOR_4_ECHO_CHANNEL 7
#define SENSOR_5_ECHO_CHANNEL 9
#define SENSOR_6_ECHO_CHANNEL 12
#define SENSOR_7_ECHO_CHANNEL 13
#define SENSOR_8_ECHO_CHANNEL 15

// Fungsi untuk memilih channel pada multiplexer
void selectMuxChannel(int mux, int channel) {
  int s0 = channel & 0x01;
  int s1 = (channel >> 1) & 0x01;
  int s2 = (channel >> 2) & 0x01;
  int s3 = (channel >> 3) & 0x01;

  if (mux == 1) {
    digitalWrite(MUX1_S0, s0);
    digitalWrite(MUX1_S1, s1);
    digitalWrite(MUX1_S2, s2);
    digitalWrite(MUX1_S3, s3);
  } else {
    digitalWrite(MUX2_S0, s0);
    digitalWrite(MUX2_S1, s1);
    digitalWrite(MUX2_S2, s2);
    digitalWrite(MUX2_S3, s3);
  }

  delay(2); // Delay untuk stabilisasi
}

// Fungsi untuk membaca jarak dari sensor
long getDistance(uint8_t trigChannel, uint8_t echoChannel) {
  selectMuxChannel(1, trigChannel);  // Pilih channel TRIG
  selectMuxChannel(2, echoChannel);  // Pilih channel ECHO

  digitalWrite(MUX1_SIG, LOW);
  delayMicroseconds(2);
  digitalWrite(MUX1_SIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(MUX1_SIG, LOW);

  long duration = pulseIn(MUX2_SIG, HIGH, 30000); // Timeout 30 ms
  long distance = duration * 0.034 / 2; // Konversi ke cm
  return distance;
}

void setup() {
  lcd.begin(16, 2);

  // Inisialisasi pin multiplexer
  pinMode(MUX1_S0, OUTPUT);
  pinMode(MUX1_S1, OUTPUT);
  pinMode(MUX1_S2, OUTPUT);
  pinMode(MUX1_S3, OUTPUT);
  pinMode(MUX1_SIG, INPUT);

  pinMode(MUX2_S0, OUTPUT);
  pinMode(MUX2_S1, OUTPUT);
  pinMode(MUX2_S2, OUTPUT);
  pinMode(MUX2_S3, OUTPUT);
  pinMode(MUX2_SIG, INPUT);


}

void loop() {
  long d1 = getDistance(SENSOR_1_TRIG_CHANNEL, SENSOR_1_ECHO_CHANNEL);
  long d2 = getDistance(SENSOR_2_TRIG_CHANNEL, SENSOR_2_ECHO_CHANNEL);
  long d3 = getDistance(SENSOR_3_TRIG_CHANNEL, SENSOR_3_ECHO_CHANNEL);
  long d4 = getDistance(SENSOR_4_TRIG_CHANNEL, SENSOR_4_ECHO_CHANNEL);
  long d5 = getDistance(SENSOR_5_TRIG_CHANNEL, SENSOR_5_ECHO_CHANNEL);
  long d6 = getDistance(SENSOR_6_TRIG_CHANNEL, SENSOR_6_ECHO_CHANNEL);
  long d7 = getDistance(SENSOR_7_TRIG_CHANNEL, SENSOR_7_ECHO_CHANNEL);
  long d8 = getDistance(SENSOR_8_TRIG_CHANNEL, SENSOR_8_ECHO_CHANNEL);

  // Hitung dimensi truk
  long lebar   = ROOM_WIDTH-(((d1 + d2)/2)+((d3 + d4)/2));
  long panjang = ROOM_LENGTH-((d5 + d6)/2);
  long tinggi  = ROOM_HEIGHT-((d7 + d8)/2);

  long dimensi = panjang*lebar*tinggi;

  // Tampilkan hasil di LCD
  lcd.setCursor(0, 0);
  lcd.print("Dimensi:");
  lcd.setCursor(0, 1);
  lcd.print(dimensi);
  lcd.print(" cm3");
  delay(2000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("-HASIL DIMENSI-");
  lcd.setCursor(0, 1);
  if (dimensi > 15000) {
    lcd.print("TRUK OVERLOAD");
  } else {
    lcd.print("AMAN");
  }
delay(3000);
lcd.clear();

}