/*
Tanggerang 26/12/2023
Project Robot Eyes
Archixvo
Gilang Maulana
*/

#include <LedControl.h>
#include <Servo.h>

LedControl lc=LedControl(12, 11, 10, 1);
Servo servoA;
Servo servoB;

const int triggerPin = 7; // Sambungkan pin trigger sensor ultrasonik ke pin 7
const int echoPin = 6;    // Sambungkan pin echo sensor ultrasonik ke pin 6

const uint8_t IMAGES[][8] = {
{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01110010,
  0b01110010,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01110010,
  0b01110010,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01110010,
  0b01110010,
  0b01111110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01110010,
  0b01110010,
  0b01111110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01100110,
  0b01100110,
  0b01111110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01100110,
  0b01100110,
  0b01111110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01001110,
  0b01001110,
  0b01111110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01001110,
  0b01001110,
  0b01111110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01001110,
  0b01001110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01001110,
  0b01001110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01111110,
  0b01001110,
  0b01001110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01111110,
  0b01001110,
  0b01001110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00000000,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
},{
  0b00000000,
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110,
  0b00000000
},{
  0b00000000,
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110,
  0b00000000
},{
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110,
  0b00000000,
  0b00000000
},{
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110,
  0b00000000,
  0b00000000
},{
  0b00000000,
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110,
  0b00000000
},{
  0b00000000,
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110,
  0b00000000
},{
  0b00000000,
  0b00000000,
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110
},{
  0b00000000,
  0b00000000,
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110
},{
  0b00000000,
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110,
  0b00000000
},{
  0b00000000,
  0b00011110,
  0b00111111,
  0b00110011,
  0b00110011,
  0b00111111,
  0b00011110,
  0b00000000
},{
  0b00000000,
  0b00111100,
  0b01111110,
  0b01100110,
  0b01100110,
  0b01111110,
  0b00111100,
  0b00000000
}};
const int IMAGES_LEN = sizeof(IMAGES)/8;

int servoSpeed = 15;

void setup() {
  lc.shutdown(0, false);
  lc.setIntensity(0, 8);
  lc.clearDisplay(0);

  servoA.attach(9); // Sambungkan servo A ke pin 9
  servoB.attach(8); // Sambungkan servo B ke pin 10
  servoA.write(0); // Set servo A ke posisi awal (0 derajat)
  servoB.write(0); // Set servo B ke posisi awal (0 derajat)

  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void displayImage(const uint8_t image[8]) {
  for (int i = 0; i < 8; i++) {
    lc.setRow(0, i, image[i]);
  }
}

float getDistance() {
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  return pulseIn(echoPin, HIGH) * 0.0343 / 2;
}

void loop() {
  for (int i = 0; i < IMAGES_LEN; i++) {
    displayImage(IMAGES[i]);
    delay(100); // Tampilkan gambar selama 1 detik (sesuaikan dengan kebutuhan Anda)

    if (i == 5) {
      // Pada detik ke-5, gerakan servo A
      servoA.write(180);
      delay(1500); // Tunggu sebentar sebelum kembali ke posisi awal
      servoA.write(0);
    }

    if (i == IMAGES_LEN - 1) {
      // Saat gambar terakhir, gerakan servo B
      servoB.write(180);
      delay(1000); // Tunggu sebentar sebelum kembali ke posisi awal
      servoB.write(0);
    }

    // Cek jarak menggunakan sensor ultrasonik
    float distance = getDistance();
    if (distance < 20.0) {  // Sesuaikan dengan jarak yang diinginkan
      // Saat ada objek mendekat, aktifkan servo A dan servo B
      servoA.write(180);
      delay(1000);
      servoB.write(180);
      delay(1500);  // Tahan selama 1 detik (sesuaikan dengan kebutuhan Anda)
      servoA.write(-180);
      delay(1500);
      servoB.write(-180);
      delay(1000);
    }
  }
}