/*
 * PCA9685 library for ESP8266
 * Copyright 2017 (c) Mark van Renswoude
*/
#include <stdint.h>
#include "./PCA9685.h"
#include <Wire.h>

#include <Arduino.h>


void PCA9685::setAddress(uint8_t address, uint8_t pinSDA, uint8_t pinSCL)
{
  Wire.begin(pinSDA, pinSCL);
  this->setAddress(address);
}


void PCA9685::setAddress(uint8_t address)
{
  this->mAddress = address;
}


uint8_t PCA9685::read(uint8_t registerAddress)
{
  uint8_t result = 0;

  Wire.beginTransmission(this->mAddress);
  Wire.write(registerAddress);
  Wire.endTransmission();

  Wire.requestFrom(this->mAddress, (uint8_t)1);
  if (Wire.available())
    result = Wire.read();

  return result;
}


void PCA9685::write(uint8_t registerAddress, uint8_t value)
{
  Wire.beginTransmission(this->mAddress);
  Wire.write(registerAddress);
  Wire.write(value);
  Wire.endTransmission();
}


inline void PCA9685::write(uint8_t data)
{
  Wire.write(data);
}


static const float prescaleValue = 25000000 / 4096;

void PCA9685::setPWMFrequency(float frequency)
{
  // Credit to the Adafruit PWM Servo Driver library for these calculations
  // https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/
  uint8_t prescale = floor(prescaleValue / (frequency * 0.9) - 0.5);

  // Sleep while changing the frequency
  uint8_t oldMode = this->read(PCA9685::RegisterMode1) & !PCA9685::Mode1Sleep;
  uint8_t newMode = (oldMode & !PCA9685::Mode1Restart) | PCA9685::Mode1Sleep;

  this->write(PCA9685::RegisterMode1, newMode);
  this->write(PCA9685::RegisterPrescale, prescale);
  this->write(PCA9685::RegisterMode1, oldMode);

  // According to the datasheet:
  // It takes 500 us max. for the oscillator to be up and running once
  // SLEEP bit has been set to logic 0
  //
  // The Adafruit library uses 5 milliseconds, so I'll stick to that as well.
  delay(5);

  // Restart and turn on auto-increment (required for SetPWM)
  this->write(PCA9685::RegisterMode1, oldMode | PCA9685::Mode1Restart | PCA9685::Mode1AI | PCA9685::Mode1AllCall);
}


void PCA9685::setPWM(uint8_t pin, uint16_t value)
{
  if (value < 0) value = 0;
  if (value > 4095) value = 4095;

  if (value == 4095)
    this->setPWM(pin, 4096, 0);
  else if (value == 0)
    this->setPWM(pin, 0, 4096);
  else
    this->setPWM(pin, 0, value);
}


void PCA9685::setPWM(uint8_t pin, uint16_t on, uint16_t off)
{
  Wire.beginTransmission(this->mAddress);
  this->write(PCA9685::RegisterLED0OnL + (4 * pin));
  this->write(on);
  this->write(on >> 8);
  this->write(off);
  this->write(off >> 8);
  Wire.endTransmission();
}



void PCA9685::setAll(uint16_t value)
{
  if (value < 0) value = 0;
  if (value > 4095) value = 4095;

  if (value == 4095)
    this->setAll(4096, 0);
  else if (value == 0)
    this->setAll(0, 4096);
  else
    this->setAll(0, value);
}


void PCA9685::setAll(uint16_t on, uint16_t off)
{
  Wire.beginTransmission(this->mAddress);
  this->write(PCA9685::RegisterAllLEDOnL);
  this->write(on);
  this->write(on >> 8);
  this->write(off);
  this->write(off >> 8);
  Wire.endTransmission();
}