//Library Used
#include <Pushbutton.h>
#include <Adafruit_PWMServoDriver.h>
#include <Wire.h>

#define buttonPin 2 //Pin for the Button

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); //Servo Driver library initialization
Pushbutton pushButton(buttonPin); //Push Button library Initialization

#define SERVOMIN 150 //Minimum angle Value from the library
#define SERVOMAX 600 //Maximum angle Value from the library


uint8_t servoNum1 = 0, servoNum2 = 1, servoNum3 = 2; //Pins for the Servo in the Servo Shield

//Potentiometer pins
int potPin1 = A0, potPin2 = A1, potPin3 = A2;
//Potentiometer Value Storage
int potVal1, potVal2, potVal3;

//Joystick Pins
int joystickXPin = A6;
int joystickYPin = A7;
int joystick2XPin = A3;
//Joystick Value Storage
int joystickXVal, joystickYVal, joystick2XVal;

int counter = 0; //push button counter

void setup() {
    Serial.begin(9600); //Standard Serial interface initialization
    pwm.begin(); //Start the Servo Driver Library
    pwm.setPWMFreq(60); //Operating frequency of the Servo Shield & Servos
    Serial.println("Press the button to begin");
}

void loop() {
    //Code from the pushbutton library to read if the button is pressed and how many times it is pressed
    if (pushButton.isPressed()) {
        counter = counter + 1; //increase the counter once the button is released
        delay(350);
        //-------------------------------------Serial codes----------------------//
        if (counter == 1 || counter == 2) {
            Serial.print("Counter Value: ");
            Serial.println(counter);
            if (counter == 1) {
                Serial.println("Potentiometer Servo Controll");
                Serial.println(" ");
            }
            if (counter == 2) {
                Serial.println("Joystick Servo Controll");
                Serial.println(" ");
            }
        }
        if (counter == 3) {
            Serial.println("Counter Value: 1");
            Serial.println("Potentiometer Servo Controll");
            Serial.println(" ");
        }
        //-------------------------------------Serial codes----------------------//
    }
    //Reaction if the counter value is 1
    if (counter == 1) {
        potVal1 = analogRead(potPin1); //Reading the pin of the 1st potentiometer
        potVal1 = map(potVal1, 0, 1023, SERVOMIN, SERVOMAX); //Scaling the analog value into a readable value for the library
        pwm.setPWM(servoNum1, 0, potVal1); //Setting the Servo in pin 0 (servoNum1) to move according to the Scaled Value
        delay(15);

        potVal2 = analogRead(potPin2); //Reading the pin of the 2nd potentiometer
        potVal2 = map(potVal2, 0, 1023, SERVOMIN, SERVOMAX); //Scaling the analog value into a readable value for the library
        pwm.setPWM(servoNum2, 0, potVal2); //Setting the Servo in pin 1 (servoNum2) to move according to the Scaled Value
        delay(15);

        potVal3 = analogRead(potPin3); //Reading the pin of the 3rd potentiometer
        potVal3 = map(potVal3, 0, 1023, SERVOMIN, SERVOMAX); //Scaling the analog value into a readable value for the library
        pwm.setPWM(servoNum3, 0, potVal3); //Setting the Servo in pin 2 (servoNum3) to move according to the Scaled Value
        delay(15);
    }
    //Reaction if the counter value is 2
    if (counter == 2) {
        joystickXVal = analogRead(joystickXPin) ; //Reading the X axis pin of the joystick
        joystickXVal = map (joystickXVal, 0, 1023, SERVOMIN, SERVOMAX); //Scaling the analog value into a readable value for the library
        pwm.setPWM(servoNum1, 0, joystickXVal); //Setting the Servo in pin 0 (servoNum1) to move according to the Scaled Value
        delay(15);

        joystickYVal = analogRead(joystickYPin) ; //Reading the Y axis pin of the joystick
        joystickYVal = map (joystickYVal, 0, 1023, SERVOMIN, SERVOMAX); //Scaling the analog value into a readable value for the library
        pwm.setPWM(servoNum2, 0, joystickYVal); //Setting the Servo in pin 1 (servoNum2) to move according to the Scaled Value
        delay(15);

        forservoNum3(); //Calling the class for the servo number 3
    }
    //Reaction if counter is 3
    if (counter == 3) {
        counter = 1; //Go back to reaction #1
    }
}

void forservoNum3 () {
    joystick2XVal = analogRead(joystick2XPin) ; //Reading the X axis pin of the 2nd joystick
    joystick2XVal = map (joystick2XVal, 0, 1023, SERVOMIN, SERVOMAX); //Scaling the analog value into a readable value for the library

    //Code for the 360 continous servo to stop at a given scaled value
    if (joystick2XVal >= 360 && joystick2XVal <= 380) {
        joystick2XVal = 398;
        pwm.setPWM(servoNum3, 0, joystick2XVal); //Setting the Servo in pin 2 (servoNum3) to move according to the Scaled Value
        delay(15);
        joystick2XVal = analogRead(joystick2XPin) ; //Reading the X axis pin again to exit the IF function
        joystick2XVal = map (joystick2XVal, 0, 1023, SERVOMIN, SERVOMAX); //Scaling the analog value into a readable value for the library
    }
    else
        pwm.setPWM(servoNum3, 0, joystick2XVal); //Setting the Servo in pin 2 (servoNum3) to move according to the Scaled Value
    delay(15);
}