#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

#define PCA9685_ADDRESS  0x40


// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PCA9685_ADDRESS);

#define SERVOMIN 120    // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 500   // This is the 'maximum' pulse length count (out of 4096)
#define SERVO_FREQ 50  // Analog servos run at ~50 Hz updates

// Adjust these angles according to Servo360_PCA_AngleFInd code that I have provided
int leftAngle = 0;     // angel for motor to move left (If the motor move in right you can change the name to rightAngle)
int rightAngle = 0;  // angel for motor to move right (If the motor move in left you can change the name to leftAngle)
int stopAngle = 120;    // Angle when motor stops

// our servo # counter
byte totalServos = 1;  // Define how many servos we are using here

// Define the time (in milliseconds 1 second = 1000 millisecond) for each servo to move in right direction
int rightMoveTime[13] = { 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 };  // Wait for this millisecond when motor move right
int stopWaitTime[13] = { 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 };   // Wait for this millisecond after motor has reached it's point
// int leftMoveTime[13] = { 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 };   // Wait for this millisecond when motor move left

void setup() {
  Serial.begin(115200);
  pwm.begin();  // Initialize PWM instance
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  // Stop all servos
  for (int servoNum = 0; servoNum < totalServos; servoNum++) {  // initially stop all servo motors
    // pwm.setPWM(servoNum, 0, getAnglePulse(stopAngle));
    for (int ang = rightAngle; ang < stopAngle; ang++) {
      pwm.setPWM(servoNum, 0, getAnglePulse(ang));  // First move servo to right side
      delay(5); // with this delay we can control smoothness of movement
    }
  }
  // Serial.println("90");
  // pwm.setPWM(0, 0, getAnglePulse(90));
  //    delay(1500);
  // Serial.println("180");
  //      pwm.setPWM(0, 0, getAnglePulse(180));
  //    delay(1500);
  // Serial.println("0");
  //      pwm.setPWM(0, 0, getAnglePulse(0));
  //    delay(1500);

  // for (int x = 0; x < 180; x++){
  //    pwm.setPWM(0, 0, getAnglePulse(x));
  //    delay(50);
  // }
  // for (int x = 180; x > 0; x--){
  //    pwm.setPWM(0, 0, getAnglePulse(x));
  //    delay(50);
  // }

}

void loop() {
  // Run one servo at a time
  for (int currentServo = 0; currentServo < totalServos; currentServo++) {
    Serial.print("Current Servo:");
    Serial.println(currentServo);
    int tempAng = 0;
    // Serial.println("Start moving right");
    for (int ang = stopAngle; ang > rightAngle; ang--) {
      pwm.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop This servo
      tempAng = ang;
      delay(5); // with this delay we can control smoothness of movement
    }
    delay(rightMoveTime[currentServo]);                                    // move motor in right for this time defined in start

    // pwm.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop servo at this point
    // delay(stopWaitTime);                                    // Keep motor stopped at this position defined in start

    // pwm.setPWM(currentServo, 0, getAnglePulse(leftAngle));   // Now move servo to left side back
    // delay(leftMoveTime[currentServo]);                                     // move motor in left for this time defined in start
    // Serial.println("Start moving Stop");
    for (int ang = rightAngle; ang < stopAngle; ang++) {
      pwm.setPWM(currentServo, 0, getAnglePulse(ang));  // First move servo to right side
      delay(5); // with this delay we can control smoothness of movement
    }
    delay(stopWaitTime[currentServo]);                                               // wait for 50 milliseconds before moving next servo
    // Serial.println("Start Next");
  }
}
int getAnglePulse(int inpAngle) {
  int retPulse = map(inpAngle, 0, 180, SERVOMIN, SERVOMAX);
  return retPulse;
}