#include <Servo.h> //import necessary libraries
#include <ezButton.h>

#define VRX_PIN      A0 // Arduino pin connected to VRX pin (x)
#define VRY_PIN      A1 // Arduino pin connected to VRY pin (y)
#define SW_PIN   2  // Arduino pin connected to SW  pin (button (state)) maybe unnecessary - could potentially use as a stop button in future

#define NOTHING     0x00 //defining commands to use below - nothing = no movement
#define SERVO4   0x01 // servo# = that servo is moving
#define SERVO2  0x02
#define SERVO1     0x04
#define SERVO3   0x08

#define LEFT_THRESHOLD  400 //set thresholds for what is considered right/left/up/down on the joystick
#define RIGHT_THRESHOLD 800 //middle area should not satisfy these thresholds = nothing (hopefully)
#define UP_THRESHOLD    400
#define DOWN_THRESHOLD  800

#define UPDATE_INTERVAL 100 // 100ms - time between retrieving information from the joystick/controller

ezButton button(SW_PIN); //idk what this does or if i even need it tbh, maybe could be used to return servos to original position?
Servo s1;  // create servo object to control a servo
Servo s2;  //s1 is front facing fans - others follow clockwise
Servo s3;
Servo s4;
int pos1 = 90;
int pos2 = 90;
int pos3 = 90;
int pos4 = 90;    // variable to store the servo position
int xValue = 0 ; // To store value of the X axis
int yValue = 0 ; // To store value of the Y axis
int command = NOTHING;
const int switchPin = 11; //the switch connect to pin 11
int switchState = 0;   // variable for reading the pushbutton status

unsigned long lastUpdateTime = 0;

void setup() {
  // put your setup code here, to run once:
 
  pinMode(switchPin, INPUT); //initialize the swtichPin as input

  s1.attach(9); //attach pins to each servo
  s2.attach(7);
  s3.attach(5);
  s4.attach(3);
 
  Serial.begin(9600) ; //this is the speed the monitor reads the data i think

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

void loop() {
  // put your main code here, to run repeatedly:
button.loop(); // MUST call the loop() function first

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

    xValue = analogRead(VRX_PIN); // read analog X and Y analog values every 100ms
    yValue = analogRead(VRY_PIN);
   
    command = NOTHING; // reset commands and convert analog values to commands below
    if (xValue < LEFT_THRESHOLD) //activate servo 4 if left threshold is met
      command = command | SERVO4;
    if (xValue > RIGHT_THRESHOLD) //activate servo 2 if right threshold is met
      command = command | SERVO2;
    if (yValue < UP_THRESHOLD) //activate servo 1 if up threshold is met
      command = command | SERVO1;
    if (yValue > DOWN_THRESHOLD) //activate servo 3 if down threshold is met
      command = command | SERVO3;

    // NOTE: AT A TIME, THERE MAY BE NO COMMAND, ONE COMMAND OR TWO COMMANDS
    // print command to serial and process command
    // while (command is met) = rotate servo - need else?
    
  }
switchState = digitalRead(switchPin); //read state of switch

  if (switchState == HIGH)  //if the switch is on - unwinding winch
  {
    if (command & SERVO4) {
      Serial.println("SERVO 4 - LEFT"); //print serial on serial monitor - maybe not necessary
      pos4++; //increase the servos angle
    }

    if (command & SERVO2) {
      Serial.println("SERVO 2 - RIGHT");
      pos2++;
    }

    if (command & SERVO1) {
      Serial.println("SERVO 1 - FRONT");
      pos1++;
    }

    if (command & SERVO3) {
      Serial.println("SERVO 3 - BACK");
      pos3++;
    }
  }
   else {
    if (command & SERVO4) {
      Serial.println("SERVO 4 - LEFT");
      pos4--; //decreases the servos angle
    }

    if (command & SERVO2) {
      Serial.println("SERVO 2 - RIGHT");
      pos2--;
    }

    if (command & SERVO1) {
      Serial.println("SERVO 1 - FRONT");
      pos1--;
    }

    if (command & SERVO3) {
      Serial.println("SERVO 3 - BACK");
      pos3--;
    }
   }
  s1.write(pos1);  // rotate servo motor 1
  s2.write(pos2);  // rotate servo motor 2
  s3.write(pos3);  // rotate servo motor 3
  s4.write(pos4);  // rotate servo motor 4 - can print data to IDE using code on one note if necessary
  }