// Importing necessary libraries for the Servo XY Control with Analog Joystick and OLED Screen

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

// For Fonts 
#include <Fonts/FreeSerif9pt7b.h>
#include <Fonts/FreeSans9pt7b.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define COMMAND_NO 0x00
#define COMMAND_LEFT 0x01
#define COMMAND_RIGHT 0x02
#define COMMAND_UP 0x04
#define COMMAND_DOWN 0x08

// #define LEFT_THRESHOLD 400
// #define RIGHT_THRESHOLD 800
// #define UP_THRESHOLD 400
// #define DOWN_THRESHOLD 800

#define LEFT_THRESHOLD 800
#define RIGHT_THRESHOLD 400
#define UP_THRESHOLD 800
#define DOWN_THRESHOLD 400

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

const int x_key = A1; // VRx of Analog Joystick Module to Analog Pin A1 of Arduino
const int y_key = A0; // VRy of Analog Joystick Module to Analog Pin A0 of Arduino
int x_pos; // variable declaration of storing x Angle in degrees unit
int y_pos; // variable declaration of storing y Angle in degrees unit
const int origin_key = 4; // SW of Analog Joystick Module to Digital Pin 4 of Arduino
const int servo1_pin = 5; // PWM pin of Servo1 to Digital PWM Pin 5 of Arduino
const int servo2_pin = 6; // PWM pin of Servo2 to Digital PWM Pin 6 of Arduino
int initial_position_x = 90; // Center of servo 1
int initial_position_y = 90; // Center of servo 2

int xValue = 0;   // To store value of the X axis
int yValue = 0;   // To store value of the Y axis
int xAngle = 90;  // the center position of servo #1
int yAngle = 90;  // the center position of servo #2
int command = COMMAND_NO;

// Creating an object instance for Servo motor
Servo servo1; // Servo Motor in X direction or axis
Servo servo2; // Servo Motor in Y direction or axis

// Creating an object instance for origin key
ezButton origin_button(origin_key);

// declaring variables for use in millis() function
int update_interval = 100; // 100ms
unsigned long lastUpdateTime = 0;

void setup ( ) {
  Serial.begin (9600) ;

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  delay(1000);
  display.setFont(&FreeSerif9pt7b);
  display.clearDisplay();

  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(10, 30);
  // Display static text
  display.println("WELCOME !");
  display.display();
  delay(4000); //Wait for 4 seconds to exit Welcome Screen

  display.setFont();
  display.clearDisplay(); // clears the display

  servo1.attach (servo1_pin) ;
  servo2.attach (servo2_pin) ;
  servo1.write (initial_position_x);
  servo2.write (initial_position_y);
  pinMode (x_key, INPUT) ;
  pinMode (y_key, INPUT) ;

  origin_button.setDebounceTime(50); // set debounce time to 50ms
}

void loop ( ) {
  // call the loop function for the ezButton object
  origin_button.loop();

  if (millis() - lastUpdateTime > update_interval) {
    // Read analog values of VRx and VRy of Analog Joystick Module
    x_pos = analogRead (x_key) ;
    y_pos = analogRead (y_key) ;
    Serial.print("X_POS: ");
    Serial.println(x_pos);
    Serial.print("Y_POS: ");
    Serial.println(y_pos);
    Serial.print("\n");

    // converts the analog value to commands
    // reset commands
    command = COMMAND_NO;

    // check left/right commands
    if (x_pos > LEFT_THRESHOLD)
      command = command | COMMAND_LEFT;
    else if (x_pos < RIGHT_THRESHOLD)
      command = command | COMMAND_RIGHT;

    // check up/down commands
    if (y_pos > UP_THRESHOLD)
      command = command | COMMAND_UP;
    else if (y_pos < DOWN_THRESHOLD)
      command = command | COMMAND_DOWN;

    // NOTE: AT A TIME, THERE MAY BE NO COMMAND, ONE COMMAND, OR TWO COMMANDS

    // print command to serial and process command
    if (command & COMMAND_LEFT) {
      Serial.println("COMMAND LEFT");
      xAngle--;
    }

    if (command & COMMAND_RIGHT) {
      Serial.println("COMMAND RIGHT");
      xAngle++;
    }

    if (command & COMMAND_UP) {
      Serial.println("COMMAND UP");
      yAngle--;
    }

    if (command & COMMAND_DOWN) {
      Serial.println("COMMAND DOWN");
      yAngle++;
    }

    // Constrain the servo angles to the range of 0 to 180 degrees
    xAngle = constrain(xAngle, 0, 180);
    yAngle = constrain(yAngle, 0, 180);
  }

  if (origin_button.isPressed()) {
    Serial.println("ORIGIN PRESSED !");
    xAngle = initial_position_x;  // the center position of servo #1
    yAngle = initial_position_y;  // the center position of servo #2
  }

  // Write angles on Servo 1 and Servo 2
  servo1.write(xAngle);
  servo2.write(yAngle);

  // print data to Serial Monitor on Arduino IDE
  Serial.print("Servo Motor's Angle: ");
  Serial.print(xAngle);
  Serial.print("°, ");
  Serial.print(yAngle);
  Serial.println("°");

  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  // Display static text
  display.println("Servo X: ");
  display.setCursor(60, 0);
  display.print(xAngle);
  display.setCursor(75, 0);
  display.print(" degrees");
  display.setCursor(0, 20);
  display.println("Servo Y: ");
  display.setCursor(60, 20);
  display.print(yAngle);
  display.setCursor(75, 20);
  display.print(" degrees");
  display.display();
  delay(update_interval);
  display.clearDisplay();
}