#include <Servo.h>
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_ADDRESS 0x3c //I2C Address 

#define VERT_PIN A0
#define HORZ_PIN A1
#define SEL_PIN 8

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

Servo servo[5];
int servo_pin[] = {3, 5, 6, 7, 9};

int pos[] = {90, 90, 90, 90, 90};
int i = 1;

void setup(){
  for(int a = 0; a < 5; a++){
    servo[a].attach(servo_pin[a]);
    servo[a].write(pos[a]);
  }
  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);

  Serial.begin(9600);
  
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)){
    Serial.println(F("SSD1306 allocation failed"));
    while (true);
  }
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  Serial.println("Type to User Input : [1(End) - 5(Base)] [Degree]");
}

void loop() {
  if (Serial.available()) {
    int servo_num = Serial.parseInt();
    int servo_pos = Serial.parseInt();

    while (Serial.available()) Serial.read();
    
    if (servo_num >= 1 && servo_num <= 5) {
      if (servo_pos >= 0 && servo_pos <= 180) {
        servo[servo_num - 1].write(servo_pos);
        pos[servo_num - 1] = servo_pos;
        Serial.println("Done!");
        delay(2000);
      } else {
        Serial.println("Degree : Wrong");
        delay(2000);
      }
    } else {
      Serial.println("Servo number : Wrong");
      delay(2000);
    }
  }

  // ควบคุมด้วย joystick (Manual)
  int vert = analogRead(VERT_PIN);
  int horz = analogRead(HORZ_PIN);
  bool selPressed = digitalRead(SEL_PIN) == LOW;

// horz goes from 0 (right) to 1023 (left)
// vert goes from 0 (bottom) to 1023 (top)
// selPressed is true if the joystick is pressed

  if (selPressed == false) {
    if (vert == 1023) {
      i = 1;
    }
    if (horz == 0) {
      i = 2;
    }
    if (vert == 0) {
      i = 3;
    }
    if (horz == 1023) {
      i = 4;
    }
  }
  if (selPressed == true) {
    if (horz > 700 && vert == 512) {
      pos[0] += 1;
      if (pos[0] > 180) {
        pos[0] = 180;
      }
      servo[0].write(pos[0]);
      delay(10);
    }
    if (horz < 300 && vert == 512) {
      pos[0] -= 1;
      if (pos[0] < 0) {
        pos[0] = 0;
      }
      servo[0].write(pos[0]);
      delay(10);
    }
    if (vert > 700 && horz == 512) {
      pos[i] += 1;
      if (pos[i] > 180) {
        pos[i] = 180;
      }
      servo[i].write(pos[i]);
      delay(10);
    }
    if (vert < 300 && horz == 512) {
      pos[i] -= 1;
      if (pos[i] < 0) {
        pos[i] = 0;
      }
      servo[i].write(pos[i]);
      delay(10);
    }
  }
  UpdateOLEDDisplay();
} 

void UpdateOLEDDisplay() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Manual Control");
  display.print("[Horizen] Servo: ");
  display.println(i + 1);
  display.print("[Vertical Servo]: ");
  
  for (int j = 0; j < 5; j++) {
    display.print("Servo ");
    display.print(j + 1);
    display.print(": ");
    display.print(pos[j]);
    display.println(" Degree");
  }
  
  display.display();
}