/*
 * Created by ArduinoGetStarted.com
 *
 * https://youtu.be/lLEiT9PeHSg zombi
 *
 * This example code is in the public domain
 *
 * Tutorial page:
 * https://arduinogetstarted.com/tutorials/arduino-joystick-servo-motor
 *
 */

#include <Servo.h>
#include <ezButton.h>

#define PROP 1
//#define DEBUG 1

#define VRX1_PIN      A0 // Arduino pin connected to VRX pin
#define VRY1_PIN      A1 // Arduino pin connected to VRY pin
#define SW1_PIN   2  // Arduino pin connected to SW  pin
#define SERVO_X1_PIN  11  // Arduino pin connected to Servo motor 1
#define SERVO_Y1_PIN  10  // Arduino pin connected to Servo motor 2

#define VRX2_PIN      A2 // Arduino pin connected to VRX pin
#define VRY2_PIN      A3 // Arduino pin connected to VRY pin
#define SW2_PIN   4  // Arduino pin connected to SW  pin
#define SERVO_X2_PIN  9  // Arduino pin connected to Servo motor 1
#define SERVO_Y2_PIN  6  // Arduino pin connected to Servo motor 2
#define SERVO_MIN 544   // Минимальный импульс   = 0°
#define SERVO_MAX 2400  // Максимальный импульс  = 180°


#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 UPDATE_INTERVAL 5 // 100ms

ezButton button1(SW1_PIN);
ezButton button2(SW2_PIN);

Servo x1Servo;  // create servo object to control a servo 1
Servo y1Servo;  // create servo object to control a servo 2
Servo x2Servo;  // create servo object to control a servo 3
Servo y2Servo;  // create servo object to control a servo 4

int x1Value = 0 ; // To store value of the X axis
int y1Value = 0 ; // To store value of the Y axis
int x1Angle = 90; // the center position of servo #1
int y1Angle = 90; // the center position of servo #2
int command1 = COMMAND_NO;

int x2Value = 0 ; // To store value of the X axis
int y2Value = 0 ; // To store value of the Y axis
int x2Angle = 90; // the center position of servo #1
int y2Angle = 90; // the center position of servo #2
int command2 = COMMAND_NO;


unsigned long lastUpdateTime = 0;

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

  x1Servo.attach(SERVO_X1_PIN);
  y1Servo.attach(SERVO_Y1_PIN);
  x2Servo.attach(SERVO_X2_PIN);
  y2Servo.attach(SERVO_Y2_PIN);

  button1.setDebounceTime(50); // set debounce time to 50 milliseconds
  button2.setDebounceTime(50); // set debounce time to 50 milliseconds
  
}

void loop() {
  button1.loop(); // MUST call the loop() function first
  button2.loop(); // MUST call the loop() function first

  if (millis() - lastUpdateTime > UPDATE_INTERVAL) {
    lastUpdateTime = millis() ;

    // read analog X and Y analog values
    x1Value = analogRead(VRX1_PIN);
    y1Value = analogRead(VRY1_PIN);
    x2Value = analogRead(VRX2_PIN);
    y2Value = analogRead(VRY2_PIN);

    // converts the analog value to commands
    // reset commands
    command1 = COMMAND_NO;
    command2 = COMMAND_NO;

    // check left/right commands
    if (x1Value < LEFT_THRESHOLD)
      command1 = command1 | COMMAND_LEFT;
    else if (x1Value > RIGHT_THRESHOLD)
      command1 = command1 | COMMAND_RIGHT;
    if (x2Value < LEFT_THRESHOLD)
      command2 = command2 | COMMAND_LEFT;
    else if (x2Value > RIGHT_THRESHOLD)
      command2 = command2 | COMMAND_RIGHT;

    // check up/down commands
    if (y1Value < UP_THRESHOLD)
      command1 = command1 | COMMAND_UP;
    else if (y1Value > DOWN_THRESHOLD)
      command1 = command1 | COMMAND_DOWN;
    if (y2Value < UP_THRESHOLD)
      command2 = command2 | COMMAND_UP;
    else if (y2Value > DOWN_THRESHOLD)
      command2 = command2 | COMMAND_DOWN;

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

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

    if (command1 & COMMAND_RIGHT) {
     // Serial.println("COMMAND RIGHT");
      x1Angle++;
    }
    if (command2 & COMMAND_RIGHT) {
     // Serial.println("COMMAND RIGHT");
      x2Angle++;
    }

    if (command1 & COMMAND_UP) {
     // Serial.println("COMMAND UP");
      y1Angle--;
    }
    if (command2 & COMMAND_UP) {
     // Serial.println("COMMAND UP");
      y2Angle--;
    }

    if (command1 & COMMAND_DOWN) {
    //  Serial.println("COMMAND DOWN");
      y1Angle++;
    }
    if (command2 & COMMAND_DOWN) {
    //  Serial.println("COMMAND DOWN");
      y2Angle++;
    }
  }

  if (button1.isPressed()) {
   // Serial.println("The button1 is pressed");
    x1Angle = 90; // the center position of servo #1
    y1Angle = 90; // the center position of servo #2
    }
  if (button2.isPressed()) {
   // Serial.println("The button2 is pressed");
    x2Angle = 90; // the center position of servo #1
    y2Angle = 90; // the center position of servo #2
    }

  #ifdef PROP
  x1Servo.write(map(x1Value,0,1023,SERVO_MIN,SERVO_MAX)); // rotate servo motor 1
  y1Servo.write(map(y1Value,0,1023,SERVO_MIN,SERVO_MAX)); // rotate servo motor 2
  x2Servo.write(map(x2Value,0,1023,SERVO_MIN,SERVO_MAX)); // rotate servo motor 4
  y2Servo.write(map(y2Value,0,1023,SERVO_MIN,SERVO_MAX)); // rotate servo motor 2
 
  #else
  x1Servo.write(x1Angle); // rotate servo motor 1
  y1Servo.write(y1Angle); // rotate servo motor 2
  x2Servo.write(x2Angle); // rotate servo motor 3
  y2Servo.write(y2Angle); // rotate servo motor 4
  #endif

  #ifdef DEBUG
  //print data to Serial Monitor on Arduino IDE
  Serial.print("Servo Motor's Angle: ");
  Serial.print(x1Angle);
  Serial.print("°, ");
  Serial.print(y1Angle);
  Serial.println("°");

  //print data to Serial Monitor on Arduino IDE
  Serial.print("Servo Motor's Angle: ");
  Serial.print(x2Angle);
  Serial.print("°, ");
  Serial.print(y2Angle);
  Serial.println("°");
  #endif

}