/*
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_AUTO[][8] = {
    { 0b00000000, 0b00111100, 0b01111110, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 0
    { 0b00000000, 0b00111100, 0b01111110, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 1
    { 0b00000000, 0b00000000, 0b00000000, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 2
    { 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b01111110, 0b00111100, 0b00000000 }, // 3
    { 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00111100, 0b00000000 }, // 4
    { 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00111100, 0b00000000 }, // 5
    { 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b01111110, 0b00111100, 0b00000000 }, // 6
    { 0b00000000, 0b00000000, 0b00000000, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 7
    { 0b00000000, 0b00111100, 0b01111110, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 8
    { 0b00000000, 0b00111100, 0b01111110, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 9
    { 0b00000000, 0b00111100, 0b01111110, 0b01110010, 0b01110010, 0b01111110, 0b00111100, 0b00000000 }, // 10
    { 0b00000000, 0b00111100, 0b01111110, 0b01110010, 0b01110010, 0b01111110, 0b00111100, 0b00000000 }, // 11
    { 0b00000000, 0b00111100, 0b01110010, 0b01110010, 0b01111110, 0b01111110, 0b00111100, 0b00000000 }, // 12
    { 0b00000000, 0b00111100, 0b01110010, 0b01110010, 0b01111110, 0b01111110, 0b00111100, 0b00000000 }, // 13
    { 0b00000000, 0b00111100, 0b01100110, 0b01100110, 0b01111110, 0b01111110, 0b00111100, 0b00000000 }, // 14
    { 0b00000000, 0b00111100, 0b01100110, 0b01100110, 0b01111110, 0b01111110, 0b00111100, 0b00000000 }, // 15
    { 0b00000000, 0b00111100, 0b01001110, 0b01001110, 0b01111110, 0b01111110, 0b00111100, 0b00000000 }, // 16
    { 0b00000000, 0b00111100, 0b01001110, 0b01001110, 0b01111110, 0b01111110, 0b00111100, 0b00000000 }, // 17
    { 0b00000000, 0b00111100, 0b01111110, 0b01001110, 0b01001110, 0b01111110, 0b00111100, 0b00000000 }, // 18
    { 0b00000000, 0b00111100, 0b01111110, 0b01001110, 0b01001110, 0b01111110, 0b00111100, 0b00000000 }, // 19
    { 0b00000000, 0b00111100, 0b01111110, 0b01111110, 0b01001110, 0b01001110, 0b00111100, 0b00000000 }, // 20
    { 0b00000000, 0b00111100, 0b01111110, 0b01111110, 0b01001110, 0b01001110, 0b00111100, 0b00000000 }, // 21
    { 0b00000000, 0b00111100, 0b01111110, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 22
    { 0b00000000, 0b00111100, 0b01111110, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 23
    { 0b00000000, 0b00111100, 0b01111110, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 24
    { 0b00000000, 0b00000000, 0b00000000, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 25
    { 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b01111110, 0b00111100, 0b00000000 }, // 26
    { 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00111100, 0b00000000 }, // 27
    { 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00111100, 0b00000000 }, // 28
    { 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b01111110, 0b00111100 }, // 29
    { 0b00000000, 0b00000000, 0b00000000, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 30
    { 0b00000000, 0b00111100, 0b01111110, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }, // 31
    { 0b00000000, 0b00111100, 0b01111110, 0b01100110, 0b01100110, 0b01111110, 0b00111100, 0b00000000 }  // 32
};


const uint8_t IMAGES_MANUAL[][8] = {
    {0b00111100, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000},
    {0b01000010, 0b00111100, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000},
    {0b11111111, 0b01000010, 0b00111100, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000},
    {0b00000000, 0b11111111, 0b01000010, 0b00111100, 0b00000000, 0b00000000, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b11111111, 0b01000010, 0b00111100, 0b00000000, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01000010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01000010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01000010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01000010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01111110, 0b00000000, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01111110, 0b00000000, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01010010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01100010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01010010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01001010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01001010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01001010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01001010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01000010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01111110, 0b00000000, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01111110, 0b00000000, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01000010, 0b00111100, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01000110, 0b00111110, 0b00000000, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01000110, 0b00111110, 0b00000010, 0b00000000},
    {0b00000000, 0b00000000, 0b00000000, 0b11111111, 0b01000110, 0b00111110, 0b00000010, 0b00000010}
};

const int IMAGES_LEN = sizeof(IMAGES_AUTO)/8;


int servoSpeed = 15;

const int joyXPin = A0; // Pin analog joystick sumbu X
const int joyYPin = A1; // Pin analog joystick sumbu Y
const int switchPin = 2; // Pin slide switch
const int ledPin = 13;   // Pin LED

int switchState = 0; // Variabel untuk menyimpan status switch
int joyXValue, joyYValue;

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 8
  servoA.write(0); // Set servo A ke posisi awal (0 derajat)
  servoB.write(0); // Set servo B ke posisi awal (0 derajat)

  pinMode(switchPin, INPUT); // Set pin switch sebagai input
  pinMode(ledPin, OUTPUT);   // Set pin LED sebagai output
}

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

void moveServos(int angleA, int angleB) {
  servoA.write(angleA);
  servoB.write(angleB);
}

void loop() {
  switchState = digitalRead(switchPin); // Baca status switch

  if (switchState == HIGH) {
    // Mode otomatis
    for (int i = 0; i < IMAGES_LEN; i++) {
      displayImage(IMAGES_AUTO[i]);
      delay(100);
     

      if (i == 5) {
        servoA.write(90);
        delay(5000);
        servoA.write(0);
      }

      if (i == IMAGES_LEN - 1) {
        servoB.write(-90);
        delay(3000);
        servoB.write(0);
      }
    }

  } else {
    // Mode manual menggunakan analog joystick
    joyXValue = analogRead(joyXPin);
    joyYValue = analogRead(joyYPin);

    // Map nilai joystick ke rentang sudut servo (0-180)
    int angleA = map(joyXValue, 0, 1023, 0, 180);
    int angleB = map(joyYValue, 0, 1023, 0, 180);

    moveServos(angleA, angleB);

    // Menyalakan LED untuk menunjukkan mode manual
    digitalWrite(ledPin, HIGH);
    delay(50); // Tambahkan delay untuk stabilitas
  }
  }