// See also:
// GPIO test https://wokwi.com/projects/412403579090783233
// Oscillator Test https://wokwi.com/projects/412405035088333825
// PWMtest https://wokwi.com/projects/412407132298758145
// Servotest
// Uses https://github.com/bonnyr/wokwi-pca9685-custom-chip
// https://www.adafruit.com/product/815
//https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf
// Addresses 0x40+{A0:A5} -(avoid AllCall: 0xE0 & Reset 0x06)

// XXX Doesn't seem to work

// Custom chips playground
// See https://link.wokwi.com/custom-chips-alpha for more info


// ======================= Copied Definitions ====================
// Copied some adafruit servo driver code rather than import the library - will do that later
// This explains the defines below etc...



/***************************************************
  This is an example for our Adafruit 16-channel PWM & Servo driver
  Servo test - this will drive 8 servos, one after the other on the
  first 8 pins of the PCA9685

  Pick one up today in the adafruit shop!
  ------> http://www.adafruit.com/products/815

  These drivers use I2C to communicate, 2 pins are required to
  interface.

  Adafruit invests time and resources providing this open source code,
  please support Adafruit and open-source hardware by purchasing
  products from Adafruit!

  Written by Limor Fried/Ladyada for Adafruit Industries.
  BSD license, all text above must be included in any redistribution
 ****************************************************/

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

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x5f);
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);

// Depending on your servo make, the pulse width min and max may vary, you
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

// our servo # counter
uint8_t servonum = 0;

uint8_t state = 0;
int counter = 0;
const byte ButtonPin = 2;
bool initialized = false;

void setup() {
  Serial.begin(115200);
  Serial.println("8 channel Servo test!");
  pinMode(ButtonPin, INPUT_PULLUP);

  pwm.begin();
  /*
     In theory the internal oscillator (clock) is 25MHz but it really isn't
     that precise. You can 'calibrate' this by tweaking this number until
     you get the PWM update frequency you're expecting!
     The int.osc. for the PCA9685 chip is a range between about 23-27MHz and
     is used for calculating things like writeMicroseconds()
     Analog servos run at ~50 Hz updates, It is importaint to use an
     oscilloscope in setting the int.osc frequency for the I2C PCA9685 chip.
     1) Attach the oscilloscope to one of the PWM signal pins and ground on
        the I2C PCA9685 chip you are setting the value for.
     2) Adjust setOscillatorFrequency() until the PWM update frequency is the
        expected value (50Hz for most ESCs)
     Setting the value here is specific to each individual I2C PCA9685 chip and
     affects the calculations for the PWM update frequency.
     Failure to correctly set the int.osc value will cause unexpected PWM results
  */
  pwm.setOscillatorFrequency(27000000);
  pwm.sleep();
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
  pwm.wakeup();
  setServoPulse(0, 0.010);
  pwm.setPWM(0, 102, 204);
  reportParameters();

  delay(10000);
}

// You can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;

  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= SERVO_FREQ;   // Analog servos run at ~60 Hz updates
  Serial.print(pulselength); Serial.println(" us per period");
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength); Serial.println(" us per bit");
  pulse *= 1000000;  // convert input seconds to us
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
}

void loop() {
  userInterface();

  switch (state) {
    case 0:
      if (! initialized) {
        // Drive each servo one at a time using setPWM()\
        Serial.print("servo:");
        Serial.println(servonum);
        Serial.print("pulselen+ from ");
        Serial.print(SERVOMIN);
        Serial.print(" to ");
        Serial.print(SERVOMAX);
        initialized = true;
        counter = SERVOMIN;
      }
      pwm.setPWM(servonum, 0, counter);
//Serial.print("+");
      delay(5);
      ++counter;
      if (counter >= SERVOMAX) {
        counter = SERVOMIN;
      }
      break;
    case 1:
      if (! initialized) {
        // Drive each servo one at a time using setPWM()\
        Serial.print("servo:");
        Serial.println(servonum);
        Serial.print("pulselen+");
        initialized = true;
        counter = SERVOMAX;
      }
      pwm.setPWM(servonum, 0, counter);
     // Serial.print("-");
      --counter;
      if (counter <= SERVOMIN) {
        counter = SERVOMAX;
      }

      break;
    case 2:

      if (! initialized) {

        Serial.print("writeMicroseconds");
        // Drive each servo one at a time using writeMicroseconds(), it's not precise due to calculation rounding!
        // The writeMicroseconds() function is used to mimic the Arduino Servo library writeMicroseconds() behavior.
        Serial.print("servo:");
        Serial.println(servonum);
        Serial.print("Resetting to uS:");
        Serial.print(USMIN);
        Serial.print("US +");
        initialized = true;
        counter = USMIN;
        pwm.writeMicroseconds(servonum, counter);
        reportParams();
      }
      pwm.writeMicroseconds(servonum, counter);
      //Serial.print("+");
      delay(50);
      ++counter;
      if (counter >= USMAX) {
        initialized = false;
      }
      break;
    case 3:
      delay(500);
      for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
        pwm.writeMicroseconds(servonum, microsec);
        Serial.print("-");
        Serial.print(microsec);
        delay(50);
      }
      break;
    default:
      Serial.print("bad state");
      Serial.print(state);
      state = 0;
  }
  servonum++;
  if (servonum > 7 * 0) servonum = 0; // Testing the first 8 servo channels
}


void userInterface() {
  uint32_t now = millis();
  static uint32_t last;
  const uint32_t interval = 100;
  int button = digitalRead(ButtonPin) == LOW; // active low
  static int lastButton = false; // inactive
  if (now - last >= interval) {
    last += interval;
    if (button != lastButton) {
      if (button) {
        Serial.print("Changing State from");
        Serial.print(state);
        Serial.print(" to ");
        ++state;
        initialized = false;
        if ( state >= 5 ) {
          state = 0;
        }
        Serial.println(state);
      }
      lastButton = button;
    }
  }
}


void reportParams() {
  Serial.print(pwm.getOscillatorFrequency());
  Serial.print("Hz oscillator\n");
  Serial.print(pwm.readPrescale());
  Serial.print(" = Prescale\n");
}