#include <AccelStepper.h>
#include <Keypad.h>
#include <U8g2lib.h>
#include <Wire.h>

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

// Inisialisasi pin motor stepper 1
#define STEPPER1_STEP_PIN 2
#define STEPPER1_DIR_PIN 3

// Inisialisasi pin motor stepper 2
#define STEPPER2_STEP_PIN 4
#define STEPPER2_DIR_PIN 5

// Inisialisasi pin keypad
const byte ROW_NUM    = 4;
const byte COLUMN_NUM = 4;

char keys[ROW_NUM][COLUMN_NUM] = {
  {'1','2','3', 'A'},
  {'4','5','6', 'B'},
  {'7','8','9', 'C'},
  {'*','0','#', 'D'}
};

byte pin_rows[ROW_NUM] = {6, 7, 8, 9};
byte pin_column[COLUMN_NUM] = {10, 11, 12, 13};

Keypad keypad = Keypad(makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COLUMN_NUM);

AccelStepper stepper1(AccelStepper::DRIVER, STEPPER1_STEP_PIN, STEPPER1_DIR_PIN);
AccelStepper stepper2(AccelStepper::DRIVER, STEPPER2_STEP_PIN, STEPPER2_DIR_PIN);

void setup() {
  Serial.print(F("This is a constant string stored in PROGMEM"));

  Serial.begin(115200);

  // Set up stepper motor 1
  stepper1.setMaxSpeed(1000.0);
  stepper1.setAcceleration(500.0);

  // Set up stepper motor 2
  stepper2.setMaxSpeed(1000.0);
  stepper2.setAcceleration(500.0);

  // Initialize OLED display
  u8g2.begin();
  u8g2.setFont(u8g2_font_ncenB08_tr);
}

void loop() {
  char key = keypad.getKey();
  
  if (key) {
    // Handle keypad input
    switch (key) {
      case '2':
        stepper1.move(100);
        break;
      case '8':
        stepper1.move(-100);
        break;
      case '6':
        stepper2.move(100);
        break;
      case '4':
        stepper2.move(-100);
        break;
    }
  }

  // Update stepper positions
  stepper1.run();
  stepper2.run();

  // Draw arrow on OLED
  drawArrow();
}

void drawArrow() {
  u8g2.firstPage();
  do {
    u8g2.setCursor(0, 20);
    u8g2.print("Motor 1");
    drawArrowInternal(40, 30, stepper1.currentPosition() < 0);

    u8g2.setCursor(80, 20);
    u8g2.print("Motor 2");
    drawArrowInternal(120, 30, stepper2.currentPosition() < 0);
  } while (u8g2.nextPage());
}

void drawArrowInternal(int x, int y, bool reverse) {
  if (reverse) {
    u8g2.drawTriangle(x, y, x + 10, y + 20, x - 10, y + 20);
  } else {
    u8g2.drawTriangle(x, y + 20, x + 10, y, x - 10, y);
  }
}