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

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

long getDistance(uint8_t trigChannel, uint8_t echoChannel);


// Konstanta ukuran ruangan (cm)
#define ROOM_LENGTH 100
#define ROOM_WIDTH 30
#define ROOM_HEIGHT 40

// 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

// Define the ultrasonic sensor channels on the MUX
#define SENSOR_1_TRIG_CHANNEL 0  // C0
#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  // C1
#define SENSOR_2_ECHO_CHANNEL 3  // C3
#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 



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

  // Setup pin MUX
  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 distance1 = getDistance(SENSOR_1_TRIG_CHANNEL);
  long distance2 = getDistance(SENSOR_2_TRIG_CHANNEL);
  long distance3 = getDistance(SENSOR_3_TRIG_CHANNEL);
  long distance4 = getDistance(SENSOR_4_TRIG_CHANNEL);
  long distance5 = getDistance(SENSOR_5_TRIG_CHANNEL);
  long distance6 = getDistance(SENSOR_6_TRIG_CHANNEL);
  long distance7 = getDistance(SENSOR_7_TRIG_CHANNEL);      
  long distance8 = getDistance(SENSOR_8_TRIG_CHANNEL);
  

  long lebar   = ROOM_WIDTH - (((distance1 + distance2)/2)+((distance3+distance4/2)));
  long panjang = ROOM_LENGTH - (distance6 + distance5);
  long tinggi  = ROOM_HEIGHT - (distance7 + distance8);

  long dimensi = panjang * lebar * tinggi;
  long threshold = 15000;

  lcd.begin(16, 2);
  lcd.setCursor(0, 0);
  lcd.print("Dimensi:");
  lcd.setCursor(0, 1);
  lcd.print(dimensi);
  lcd.print(" cm3");
  delay(1500);

  lcd.clear();

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

  
  delay(2000); // Waktu tampil status
}