// 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...

#include <Wire.h>

// #define ENABLE_DEBUG_OUTPUT 

// REGISTER ADDRESSES
#define PCA9685_MODE1    0x00           // Mode Register 1
#define PCA9685_PRESCALE 0xFE           // Prescaler for PWM output frequency

// MODE1 bits
#define MODE1_ALLCAL     0x01           // respond to LED All Call I2C-bus address
#define MODE1_SUB3       0x02           // respond to I2C-bus subaddress 3
#define MODE1_SUB2       0x04           // respond to I2C-bus subaddress 2
#define MODE1_SUB1       0x08           // respond to I2C-bus subaddress 1
#define MODE1_SLEEP      0x10           // Low power mode. Oscillator off
#define MODE1_AI         0x20           // Auto-Increment enabled
#define MODE1_EXTCLK     0x40           // Use EXTCLK pin clock
#define MODE1_RESTART    0x80           // Restart enabled


#define FREQUENCY_OSCILLATOR 25000000   // Int. osc. frequency in datasheet

#define PCA9685_PRESCALE_MIN        3   // minimum prescale value
#define PCA9685_PRESCALE_MAX      255   // maximum prescale value

// ======================= End of Copied Definitions ====================

#undef DEBUG

// chip address. must match the wiring in diagram.json
const uint8_t _i2caddr = 0x5F;


static void begin(uint8_t prescale=0);
static void reset();
static void restart();
static void setPWMFreq(float f);

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // initialise the PCA9685 chip
  begin();

  setPWM(_i2caddr, 0, 1500, 2000);

  delay(5000);

  reset();

  delay(1000);


  setPWM(_i2caddr, 0, 1000, 3000);
  delay(5000);
  
  restart();
  // setPWM(_i2caddr, 1, 4000, 3000);


}

void loop() {
  // static uint16_t ndx;

  // for (uint16_t i = 0; i < 8; i++) {
  //   Serial.print(ndx);
  //   Serial.print(" spwm ");   Serial.print(i);
  //   Serial.print(" on: " );   Serial.print(i * 256);
  //   Serial.print(" off: ");   Serial.println((i+ 1 + (ndx %10))*256);
  //
  //   setPWM(_i2caddr, i, i * 256, (i+ 1 + (ndx & 7))*256);
  // }
  // ndx++;
  // delay(500);
}

void begin(uint8_t prescale=0) {
  restart();
  setPWMFreq(1);                        // set a default frequency
}

/*!
 *  @brief  Sends a reset command to the PCA9685 chip over I2C
 */
void restart() {
  write8(PCA9685_MODE1, MODE1_RESTART);
  delay(10);
}


void setPWMFreq(float freq) {


  if (freq < 1)                         // Range output modulation frequency is dependant on oscillator
    freq = 1;
  if (freq > 3500)                      // Datasheet limit is 3052=50MHz/(4*4096)
    freq = 3500;

  float prescaleval = ((FREQUENCY_OSCILLATOR / (freq * 4096.0)) + 0.5) - 1;

  if (prescaleval < PCA9685_PRESCALE_MIN)    prescaleval = PCA9685_PRESCALE_MIN;
  if (prescaleval > PCA9685_PRESCALE_MAX)    prescaleval = PCA9685_PRESCALE_MAX;

  uint8_t prescale = (uint8_t)prescaleval;

// #ifdef ENABLE_DEBUG_OUTPUT
  Serial.print("Final pre-scale: ");
  Serial.println(prescale);
// #endif

  uint8_t oldmode = read8(PCA9685_MODE1);
  uint8_t newmode = (oldmode & ~MODE1_RESTART) | MODE1_SLEEP;     // sleep
  write8(PCA9685_MODE1, newmode);                                 // go to sleep
  write8(PCA9685_PRESCALE, prescale);                             // set the prescaler
  write8(PCA9685_MODE1, oldmode);
  delay(5);

  write8(PCA9685_MODE1, oldmode | MODE1_RESTART | MODE1_AI);      // This sets the MODE1 register to turn on auto increment.

#ifdef ENABLE_DEBUG_OUTPUT
  Serial.print("Mode now 0x");
  Serial.println(read8(PCA9685_MODE1), HEX);
#endif
}


void setPWM(int addr, int chan,  uint16_t on, uint16_t off ) {
#ifdef ENABLE_DEBUG_OUTPUT
  Serial.print("Setting PWM ");  Serial.print(chan);
  Serial.print(": "          );  Serial.print(on);
  Serial.print("->"          );  Serial.println(off);
#endif

  Wire.beginTransmission(addr);
  Wire.write(6 + 4 * chan);
  Wire.write(on);
  Wire.write(on >> 8);
  Wire.write(off);
  Wire.write(off >> 8);
  Wire.endTransmission();

}


uint8_t read8(uint8_t addr) {
  Wire.beginTransmission(_i2caddr);
  Wire.write(addr);
  Wire.endTransmission();

  Wire.requestFrom((uint8_t)_i2caddr, (uint8_t)1);
  return Wire.read();
}

void write8(uint8_t addr, uint8_t d) {
  Wire.beginTransmission(_i2caddr);
  Wire.write(addr);
  Wire.write(d);
  Wire.endTransmission();
}

void reset() {
  Wire.beginTransmission(0);
  Wire.write(6);
  Wire.endTransmission();
}