/**************************************************************************
  Copyright (C) 2013 SARVESH KULKARNI <sarvesh.kulkarni@villanova.edu>
                     Associate Professor, ECE Department
                     Villanova University, PA, USA.

  LEGAL NOTICE:
  This library is free software: you can redistribute it and/or modify it
  under the terms of the GNU General Public License as published by the
  Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This library is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  See the GNU General Public License for more details.

  You should have received a copy of the GNU General Public License along
  with this program. If not, see <http://www.gnu.org/licenses/>.

  -------------------------------------------------------------------------
  Library of routines for Adafruit's PCA9685 16-channel PWM breakout board.
  The routines should work for *any* PCA9685 based board as long as the
  correct I2C device address is passed to the constructor by the main program.
  Version 0.9, Released August 2013.

  The Adafruit PWM board for which this library is written, is an I2C slave.
  You need to connect at least 4 pins from your microcontroller to drive
  the board. On the Raspberry Pi, Rev.2, these pins are as follows:
     SDA = pin 3, SCL = pin 5
     VCC (3.3V) = pin 1, GND = pin 6
  In addition, should you want to control any device with a power draw
  greater than that of an LED (e.g. servo motor) using this PWM board, you
  do need to provide a 5V power supply with sufficient amperage at the +V
  and GND pins of the PWM board.
 *************************************************************************/

#include "pca9685_pwm.h"
#include <linux/i2c-dev.h>
#include <stdint.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <unistd.h>
#include <cmath>
#include <cstring>

#include <iostream>
using namespace std;


// Class Constructor
//   If board's I2C address is not supplied, address = 0x40 is presumed.

PWM_Board::PWM_Board(uint8_t i2c_addr) {

	my_i2caddr = i2c_addr;
	my_fd = initialize();  // Set up file descriptor for pwm board
	softReset();
}



// Initialize all higher level device support and return a file descriptor
//   for use by subsequent write() and read() calls in other member fns. If
//   either the open() or ioctl() call fails, a negative integer error code is
//   returned. At this time, the function does not report which call (i.e.
//   open()or ioctl()) failed! If that info is important to your needs, please
//   modify this code accordingly.

int PWM_Board::initialize(void) {

	int	retcode, file_descriptor;
	char	filename[20], adapter_no[4];

	// I2C device files are character device files with major number 89
	//   and a minor device no. The command "i2cdetect -l" in the i2c-tools
	//   package lists all minor device nos. in use in the format: i2c-X
	//   (e.g. i2c-0, i2c-1). The minor device number below is hardcoded
	//   below, but it is better to dynamically determine this no.
	strcpy(adapter_no, "1");

	// Copy "/path/i2cdevicename" to filename
	strcpy(filename, "/dev/i2c-");
	strcat(filename, adapter_no);

	// Open i2c device file using above filename
	file_descriptor = open(filename, O_RDWR);

	if (file_descriptor < 0) {  // ERROR - could not open the I2C device file
	   return file_descriptor;
	}

	// If I2C device file opened OK, then link the device addr to the
	//   file descriptor for subsequent use.
	retcode = ioctl(file_descriptor, I2C_SLAVE_FORCE, my_i2caddr);
 	if (retcode < 0) {  // ERROR - ioctl call failed
	   return  retcode;
	}

 	// I2C slave board is now set up for access. At this point, any read()
	// or write() from/to the slave does not need to specify the slave's addr
        // because the ioctl call has already associated the file descriptor with
	// the slave. So, use the file descriptor for all communication with the
        // slave.
	// HARDWARE NOTE: Any read/write from/to the slave's registers will cause
        // the kernel driver to automatically signal the I2C bus 'start' condition,
        // followed by the slave's address on the I2C bus --> this happens behind
	// the scenes. See timing diagrams in the PCA9685 datasheet to see how the
	// I2C bus master and slave devices operate.

	return	file_descriptor;
}



// Check to see if the board is initialized i.e. check if the file descriptor
//   stored in the object is non-zero.

bool PWM_Board::isInitialized() {

	if (my_fd > 0)	return	true;
	else		return	false;
}



// Read contents of a single specified pca9685's register.
//   Valid register address ranges are 0-69 and 250-255 (dec)
//   If you specify an invalid address, there is no telling what might happen!

int PWM_Board::readRegister(unsigned int reg_addr) {

	if (!isInitialized())	return  FD_UNINITIALIZED; // failed! my_fd is uninitialized

	uint8_t	buf = (uint8_t) reg_addr;

	// PCA9685 requires us to write before reading - see fig 22 on page 32 of
	// datasheet. If we omit the write transaction prior to issuing a read, we
	// get incorrect values off the I2C bus.
	write(my_fd, &buf, 1);
	if(read(my_fd, &buf, 1) != 1)  return  I2C_TRANSACTION_FAILED;
	else 
	    return  ((int) buf);
}



// Get PWM frequency in Hz
//   Reads the contents of the PRE_SCALE register and computes the frequency of the signal
//   that the PWM board's channels are supposed to be putting out. It is important to
//   remember that even if you set the PWM board to output an 'f' kHz waveform, the prescale
//   value formula will round off the computation and load the prescale register with a
//   rounded-off 8-bit integer. For instance, trying to set a 1 KHz waveform yields a
//   prescale value of 5; computing backwards, a prescale value of 5 results in a computed
//   value of 1017 Hz!
//   NOTE: The oscillator itself may not be thermally stable, and introduce inaccuracies of
//	   its own accord; but, of course, that is not verifiable in software and therefore
//	   cannot be accounted for in this function. In short, this function only reports
//	   what the hardware tells it -  and as you know, the hardware can lie!

int PWM_Board::getFreq(void) {

	int		prescale_val;

	prescale_val = readRegister(_PRE_SCALE_REG);
	if (prescale_val < 0)	return	prescale_val;	// error code returned by readRegister
	else 
	// See page 24 of PCA9685 Data Sheet for the prescale value formula - this gives us the
	//   actual frequency of the PWM signal that the board thinks it is generating. On
	//   checking against the frequencies reported by a calibrated oscilloscope, my copy
	//   of the board seems to have <= 5% error at 250 Hz. However, when the frequency is
	//   set to > 250 Hz, the actual generated frequency deviates by as much as 10% from
	//   the freq computed by the formula below.
	     return (int) floorf((OSC_CLK / (4096*((float) prescale_val + 1)))  + 0.5);
}



// Set PWM frequency in Hz
// Valid range of frequencies is from 24 Hz -- 1.6 kHz
//   This translates to prescale values from 253 -- 3
//   At frequencies below 24 Hz, the 8-bit prescale value rolls under, and
//   at frequencies above 1.6 kHz, the board either cannot generate the freq,
//   or the prescale counter rolls over.
//   IMPORTANT NOTES:
//	1. Precise frequency selection is not possible due to the inexactness
//	   in the prescale value formula; you could be off by as much as 10%
//	2. If the chip is in sleep mode, this function will awaken it!
//	3. If the frequency is changed, the pulse width (on time) *will change* silently.
//	   If you using the PWM channels to drive servo motors, the consequences can be
//	   disastrous for your servos since you may violate the servos' min or max pulse
//	   width spec. It is strongly suggested that you "lock" the frequency for such
//	   an application in order to prevent inadvertent changes to it.

int PWM_Board::setFreq(float freq) {  // freq is in Hz

	if (!isInitialized())	return  FD_UNINITIALIZED; // failed! my_fd is uninitialized

	uint8_t	buf[MAXSIZE], prescale_val;
	
	if (freq < 24)    freq = 24;	// sane default on extreme lower range
	if (freq > 1600)  freq = 1600;	// sane default on extreme upper range

	// See page 24 of PCA9685 Data Sheet for the prescale value formula
	prescale_val = (uint8_t) floorf((OSC_CLK / (4096*freq))+ 0.5) - 1;

	buf[0] = _MODE1_REG;
	buf[1] = MODE1_REG_DEFAULT|0x10;  // must sleep first, otherwise writes
					  //   to prescale register are blocked
					  // sleep: low power mode, oscillator off
	if(write(my_fd, buf, 2) != 2)	return  I2C_TRANSACTION_FAILED;		    

	buf[0] = _PRE_SCALE_REG;
	buf[1] = prescale_val;		  // load prescale register
	if(write(my_fd, buf, 2) !=2 )	return  I2C_TRANSACTION_FAILED;

	// See section 7.3.1.1 on page 14 of pca9685 datasheet. Note that since we did put
	//   the chip to sleep without explicitly stopping any PWM channels, the 'Restart' bit
	//   (i.e. bit 7) of Mode1 register is most certainly set; there is no need to check
	//   if it is at logic 1. All we need to do is to clear the 'Sleep' bit, allow the
	//   oscillator to stabilize for 500us, and write logic 1 to the 'Restart' bit.
	buf[0] = _MODE1_REG;
 	buf[1] = MODE1_REG_DEFAULT;	  // clear bit 4 (sleep bit) to awaken chip
	if(write(my_fd, buf, 2) != 2)  return  I2C_TRANSACTION_FAILED;

	usleep(500);		// insert delay; internal oscillator needs
				//   up to 500 us to restart & stabilize

	buf[1] = MODE1_REG_DEFAULT|0x80; // Logic 1 written to Restart bit (bit 7) enables restart (pg 13)
	if(write(my_fd, buf, 2) != 2)  return  I2C_TRANSACTION_FAILED;	// and restores pwm waveform on all
									// channels at new frequency

	return	0;		// success!
	
}



// Set PWM Duty Cycle
//    Duty cycle and delay are in %ages; channel_num = 0xff means ALL outputs
//    NOTE: If the chip is in sleep mode, this function will awaken it!!!

int PWM_Board::setDutyCycle(float duty_cycle, unsigned int channel_num, float delay) {

	if (!isInitialized())	return  FD_UNINITIALIZED; // failed! my_fd is uninitialized

	uint8_t		led_on_l, led_on_h, led_off_l, led_off_h, buf[MAXSIZE];
	uint16_t	led_on, led_off;

	// Validate inputs first
	if(duty_cycle > 100)	return  DUTY_CYCLE_ILLEGAL;	// duty cycle error
	if(delay > 100)		return  DELAY_ILLEGAL;		// led ON delay error
	if ((channel_num < 0) || ((channel_num > 15) && (channel_num != 255)))
				return  CHANNEL_NUM_ILLEGAL;	// channel no. error

	// I/p params are valid; so proceed with our calculations

	// Duty Cycle close to 100% leads to roll over error in led_off calculation below
	//  So, we must handle this differently by turning the pwm channel to full on
	if (duty_cycle > 99.9)	return	fullOn(channel_num, delay);

	// Duty Cycle close to 0% means that the pwm channel is full off
	if (duty_cycle < 0.1)	return	fullOff(channel_num, delay);

	led_on  = floorf((40.96*delay) + 0.5) - 1;		// see example 1 on page 16
	led_off = led_on + floorf((40.96*duty_cycle)+ 0.5);	//    of PCA9685 data sheet
	if (led_off > 4095)  led_off = led_off - 4096;	    	// see example 2 on page 16

	led_on_l  = led_on & 0xff;
	led_on_h  = led_on >> 8;
	led_off_l = led_off & 0xff;
	led_off_h = led_off >> 8;

	// Set to Auto Increment mode so that we can write to all 4 registers at once
	buf[0] = _MODE1_REG;
 	buf[1] = MODE1_REG_DEFAULT | 0x21;
	if(write(my_fd, buf, 2) != 2)  return  I2C_TRANSACTION_FAILED;
 
	if (channel_num == 255)			// Update ALL PWM registers
		buf[0] = _ALL_LED_ON_L;			// ALL_LED_ON base address
	else
		buf[0] = _LED0_ON_L + 4*channel_num;	// specific channel base address

	buf[1] = led_on_l;
	buf[2] = led_on_h;
	buf[3] = led_off_l;
	buf[4] = led_off_h;
	if(write(my_fd, buf, 5) != 5)  return  I2C_TRANSACTION_FAILED;

	return	0;		  	  // success!
}


// Set Width of each Pulse of the PWM signal
//    Pulse width is specified in milliseconds. For example, a pulse width of 0.02 sec is 20 here.
//    Delay is a %age; channel_num = 0xff means ALL outputs
//    NOTES: 1. If the chip is in sleep mode, this function will awaken it!!!
//	     2. For more precise timing, set a frequency between 25-250 Hz. In this frequency
//		range, please be aware that your pulse width may still be off by 5%. If you are
//		controlling a servo motor, you need to use a 5% safety factor to ensure that the
//		pulse width generated is within the min and max spec of the servo.

int PWM_Board::setPulseWidth(float pulse_width, unsigned int channel_num, float delay) {

	float		max_pulse_width, freq, dcycle;

	// Validate inputs first: first find the actual frequency that was set, then calculate
	//   the max pulse width corresponding to that frequency.
	freq = getFreq();
	if (freq < 0)	return	freq;			// i.e. error code returned by getFreq

	max_pulse_width = 1000.0 / freq;		// in milliseconds
	dcycle = 100*pulse_width/max_pulse_width;	// duty cycle (%)

	if(dcycle > 100)	return  PULSE_WIDTH_ILLEGAL;	   // pulse width error
	else	return setDutyCycle(dcycle, channel_num);
	// We reuse our setDutyCycle function above. Other i/p params are checked in that
	//   function. So, we don't need to check them for validity here. Also, this return
	//   call returns whatever 'setDutyCycle' returns!
}



// Set Channel to Full ON
//   Same as 100% duty cycle. There will be *no* momentary 1->0 level transitions at the end of
//   each pulse period - therefore, this function is safe to assert a logic 1 for use as a chip
//   select (enable). This is very useful since it frees up a GPIO pin on your controller board.
//   NOTE: Linux is not a real time OS, so no timing guarantees are possible.

int PWM_Board::fullOn(unsigned int channel_num, float delay) {

	if (!isInitialized())	return  FD_UNINITIALIZED; // failed! my_fd is uninitialized

	uint8_t		led_on_l, led_on_h, led_off_l, led_off_h, buf[MAXSIZE];
	uint16_t	led_on;

	led_on  = floorf((40.96*delay) + 0.5) - 1;	// see example 1 on pg. 16  AND
	led_on_l  = led_on & 0xff;			//  fig 9 (example 3) on pg. 17
	led_on_h  = (led_on >> 8)|0x10;			// bit 4: logic 1 = full ON
	led_off_l = 0x00;			// bits 0-7: don't care
	led_off_h = 0x00;			// bit 4: logic 0 = not OFF, bits 0-3: don't care

	// Set to Auto Increment mode so that we can write to all 4 registers at once
	buf[0] = _MODE1_REG;
 	buf[1] = MODE1_REG_DEFAULT | 0x21;
	if(write(my_fd, buf, 2) != 2)  return  I2C_TRANSACTION_FAILED;

	if (channel_num == 255)			// Update ALL PWM registers
		buf[0] = _ALL_LED_ON_L;			// ALL_LED_ON base address
	else
		buf[0] = _LED0_ON_L + 4*channel_num;	// specific channel base address

	buf[1] = led_on_l;
	buf[2] = led_on_h;
	buf[3] = led_off_l;
	buf[4] = led_off_h;
	if(write(my_fd, buf, 5) != 5)  return  I2C_TRANSACTION_FAILED;
	
	return	0;
}



// Set Channel to Full OFF
//   Same as 0% duty cycle There will be *no* momentary 0->1 level transitions at the end of
//   each pulse period - therefore, this function is safe to assert a logic 0 for use as a chip
//   select (enable). This is very useful since it frees up a GPIO pin on your controller board.
//   NOTE: Linux is not a real time OS, so no timing guarantees are possible.

int PWM_Board::fullOff(unsigned int channel_num, float delay) {

	if (!isInitialized())	return  FD_UNINITIALIZED; // failed! my_fd is uninitialized

	uint8_t		led_on_l, led_on_h, led_off_l, led_off_h, buf[MAXSIZE];

	// see example 1 on pg. 16  AND  fig 9 (example 3) on pg. 17
	led_on_l  = 0x00;		// bits 0-7: don't care, same as led_off_l
	led_on_h  = 0x00;		// bit 4: logic 0 = full OFF, bits 0-3: don't care
	led_off_l = 0x00;		// bits 0-7: don't care, same as led_on_l
	led_off_h = 0x00;		// bit 4: logic 0 = not OFF, bits 0-3: don't care

	// Set to Auto Increment mode so that we can write to all 4 registers at once
	buf[0] = _MODE1_REG;
 	buf[1] = MODE1_REG_DEFAULT | 0x21;
	if(write(my_fd, buf, 2) != 2)  return  I2C_TRANSACTION_FAILED;

	if (channel_num == 255)			// Update ALL PWM registers
		buf[0] = _ALL_LED_ON_L;			// ALL_LED_ON base address
	else
		buf[0] = _LED0_ON_L + 4*channel_num;	// specific channel base address

	buf[1] = led_on_l;
	buf[2] = led_on_h;
	buf[3] = led_off_l;
	buf[4] = led_off_h;
	if(write(my_fd, buf, 5) != 5)  return  I2C_TRANSACTION_FAILED;
	
	return	0;
}



// Put PCA9685 in sleep mode - low power, oscillator off. The wakeup function is
//   provided as well in this class.

int PWM_Board::sleep(void) {

	if (!isInitialized())	return FD_UNINITIALIZED; // failed! my_fd is uninitialized

	uint8_t buf[MAXSIZE];
	int	mode1_val;

	// Let's attempt to preserve the register's contents
	mode1_val = readRegister(_MODE1_REG);
	if(mode1_val < 0) {		// ERROR Reading Mode1 register
		buf[1] = 0x0;		// try to continue anyway
	}
	else	buf[1] = (uint8_t) mode1_val;

	buf[1] = buf[1] | 0x10;	// just set the sleep bit
	buf[0] = _MODE1_REG;
	if(write(my_fd, buf, 2) != 2)  return I2C_TRANSACTION_FAILED;

	return	0;		  	// success!
}



// Wake up PCA9685 from sleep
//   All PWM channels are restored to pre-sleep state. The functions to set  
//   the PWM frequency and duty cycle automatically awaken the chip; therefore
//   this function need not be called if you are going to set a new frequency and/or
//   duty cycle anyway.

int PWM_Board::wakeUp(void) {

	if (!isInitialized())	return FD_UNINITIALIZED; // failed! my_fd is uninitialized

	uint8_t	buf[MAXSIZE];
	int	mode1_val;
 
	// Save contents of MODE1 register
	mode1_val = readRegister(_MODE1_REG);
	if(mode1_val < 0) {		// ERROR Reading MODE1 register
		buf[1] = 0x0;		// try to continue anyway
	}
	else	buf[1] = (uint8_t) mode1_val;
	
	// There seems to be no point in checking to see if the 'Restart' bit is set;
	//   In any case, we will need to write a '1' to this bit to restart the chip.
	//   For more info, see table 4 (pg. 14) & section 7.3.1.1 (pg. 14) of PCA9685 datasheet.
	buf[1] = buf[1] & 0x6F;		// clear 'Sleep' bit
	buf[0] = _MODE1_REG;		
	if(write(my_fd, buf, 2) != 2)  return I2C_TRANSACTION_FAILED;

	usleep(500);		// 500us delay needed before we can write logic 1 to 'Restart' bit

	buf[1] = buf[1] | 0x80;		// write logic 1 to 'Restart' bit; keep 'Sleep' bit cleared
	if(write(my_fd, buf, 2) != 2)  return I2C_TRANSACTION_FAILED;

	return	0;		  	// success!
}



// Reset internal MODE1 and MODE2 registers to useful defaults; the prescale
//   value register of the PCA9685 is preset so as to yield a 1 KHz PWM signal.
//   You *do* have to set the duty cycle explicitly to actually activate the PWM o/ps.

int PWM_Board::softReset(void) {

	if (!isInitialized())	return FD_UNINITIALIZED; // failed! my_fd is uninitialized

	uint8_t	buf[MAXSIZE];

	// Set PWM freq to 50 Hz by loading the prescale register
	//   Strictly speaking, we need not set the frequency, but it is a nice known
	//   default to have after a soft reset.
	if(setFreq(50) == I2C_TRANSACTION_FAILED)	return I2C_TRANSACTION_FAILED;

	// Turn off all PWM channels: Duty cycle = 0 %, All channels selected, Delay = 0 %
	if(setDutyCycle(0,255,0) == I2C_TRANSACTION_FAILED)	return I2C_TRANSACTION_FAILED;

	// Set MODE1 register: Restart=0, ExtClk=0, AutoIncr=0, Sleep=0, Sub1-3=0, AllCall=1
	buf[0] = _MODE1_REG;
	buf[1] = MODE1_REG_DEFAULT;
	if(write(my_fd, buf, 2) != 2)  return I2C_TRANSACTION_FAILED;

	// Set MODE2 register: O/p uninverted, LED o/ps are Open drain, O/ps change on STOP
	buf[0] = _MODE2_REG;
	buf[1] = MODE2_REG_DEFAULT;
	if(write(my_fd, buf, 2) != 2)  return I2C_TRANSACTION_FAILED;

	return 0;		// success; PCA9685 now has default values preloaded in
				//   its MODE1, MODE2, PRE_SCALE and LEDn_* registers
}



// Hard Reset - be very careful! This fn will quite possibly reset all PCA9685
//   slaves on the specified I2C bus channel. It may even reset non PCA9685 slaves
//   if the PCA9685's control word for reset matches theirs.

int PWM_Board:: hardReset(void) {

	uint8_t	buf[MAXSIZE];
	int	retcode, file_desc;
	char	filename[20], adapter_no[4];

	// The minor I2C device number is hardcoded below, but it is
	//   better to dynamically determine this no.
	strcpy(adapter_no, "1");

	// Copy "/path/i2cdevicename" to filename
	strcpy(filename, "/dev/i2c-");
	strcat(filename, adapter_no);

	// Open i2c device file using above filename
	file_desc = open(filename, O_RDWR);

	if (file_desc < 0) {  // ERROR - could not open the I2C device file
	   return file_desc;
	}

	// If I2C device file opened OK, then link the GENERAL CALL ADDRESS
	// (to which every I2C slave responds) to the above file descriptor.
	retcode = ioctl(file_desc, I2C_SLAVE_FORCE, GENERAL_CALL_ADDR);
 	if (retcode < 0) {  // ERROR - ioctl call failed
	   return retcode;
	}
	// Else I2C bus devices are now ready for general access
	buf[0] = SW_RESET;		// command to reset device(s)
	if(write(file_desc, buf, 1) != 1)  return I2C_TRANSACTION_FAILED;

	return 0;			// success
}



// Class Destructor
//   Turns off all PCM channels and the oscillator
//   IMPORTANT NOTE: If you statically allocate your PWM_Board object, you *must*
//	 end your control program (e.g. main) or the function within which the
//	 PWM_Board object was created with a 'return'. If you do not have a
//	 'return' statement there, then, when the function/program terminates
//	 and the object goes out of scope, the destructor *may not* be called. There
//	 no earthly reason why this would happen - perhaps I just encountered a bug
//	 in the compiler (?). In such a case ...
//	 This will leave your PWM board with its oscillator on, and all channels
//	 active. You usually will not want that to happen!

PWM_Board::~PWM_Board(void) {

	if (!isInitialized())	return;	  // PCA9685 is inaccessible; can't do anything

	uint8_t	buf[MAXSIZE];
	
	// Turn off all PWM channels: Duty cycle = 0 %, All channels selected, Delay = 0 %
	if(setDutyCycle(10,255,0) == I2C_TRANSACTION_FAILED)
		cout << endl << "CAUTION: PWM channels could not be shut down!" << endl;

	// Blanking the PWM channels does not turn off the internal oscillator; so ..
	buf[0] = _MODE1_REG;		// set oscillator to sleep, but leave the PCA9685
	buf[1] = 0x11;			//  addressable via 'All Call' from subsequent programs		
	if(write(my_fd, buf, 2) != 2)
		cout << endl << "CAUTION: Failed to put PCA9685 to sleep!" << endl;
}