/* Raspberry Pi PWM Hat
 * I2C controller library test
 * by Sinclair Gurny
 * Aug 17
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>     // for float
#include <unistd.h>   // for sleep
#include <wiringPi.h>
#include <wiringPiI2C.h>

// Register defines

#define MODE1    0x00
#define MODE2    0x01
#define SUBADR1  0x02
#define SUBADR2  0x03
#define SUBADR3  0x04
#define PRESCALE 0xFE
#define LED0_ON_L  0x06
#define LED0_ON_H  0x07
#define LED0_OFF_L 0x08
#define LED0_OFF_H 0x09
#define ALL_LED_ON_L  0xFA
#define ALL_LED_ON_H  0xFB
#define ALL_LED_OFF_L 0xFC
#define ALL_LED_OFF_H 0xFD

// Bit defines

#define RESTART 0x80
#define SLEEP   0x10
#define ALLCALL 0x01
#define INVRT   0x10
#define OUTDRV  0x04

// Setup for PWM board
int PWMBoardSetup( int address ) {
  int fd, ret;
  // Setup wiringPiLibrary
  fd = wiringPiI2CSetup( address );
  if ( fd == -1 ) { return -1; }
  // Reset all
  ret = PWMBoardSetPWMAll( fd, 0, 0 );
  if ( ret == -1 ) { return -1; }
  wiringPiI2CWriteReg8( fd, MODE2, OUTDRV );
  wiringPiI2CWriteReg8( fd, MODE1, ALLCALL );
  usleep( 5000 );        // wait for oscillator
  int mode = wiringPiI2CReadReg8( fd, MODE1 );
  mode = mode & ~SLEEP; // wakeup
  wiringPiI2CWriteReg8( fd, MODE1, mode );
  usleep( 5000 );       // wait for oscillator
  return fd;
}

// Software Reset ???
// Sends a software reset command to all servo drivers on the bus
int PWMBoardSoftReset( int fd ) {
  return wiringPiI2CWrite( 0x06 ); //SWRST
}


// Set PWM frequency
int PWMBoardSetFreq( int fd, int freq ) {
  float prescaleval = 25000000.0;
  prescaleval /= 4096.0;
  prescaleval /= 4096.0;
  prescaleval /= (float) freq;
  prescaleval -= 1.0;
  float prescale = floorf( prescaleval + 0.5 );
  int oldmode = wiringPiI2CReadReg8( fd, MODE1 );
  int newmode = ( oldmode & 0x7F ) | SLEEP;
  wiringPiI2CWriteReg8( fd, MODE1, newmode ); // go to sleep
  wiringPiI2CWriteReg8( fd, PRESCALE, (int) floorf( prescale) );
  wiringPiI2CWriteReg8( fd, MODE1, oldmode ); // wake up
  usleep( 5000 );
  wiringPiI2CWriteReg8( fd, MODE1, oldmode | RESTART ); // restart
  return 0;
}

// Sets a single PWM channel
int PWMBoardSetPWM( int fd, int channel, int on, int off ) {
  wiringPiI2CWriteReg8( fd, LED0_ON_L + 4*channel, on & 0xFF );
  wiringPiI2CWriteReg8( fd, LED0_ON_H + 4*channel, on >> 8 );
  wiringPiI2CWriteReg8( fd, LED0_OFF_L + 4*channel, off & 0xFF );
  wiringPiI2CWriteReg8( fd, LED0_OFF_H + 4*channel, off >> 8 );
  return 0;
}

// Set all PWM channels
int PWMBoardSetPWMAll( int fd, int on, int off ) {
  wiringPiI2CWriteReg8( fd, ALL_LED_ON_L, on & 0xFF );
  wiringPiI2CWriteReg8( fd, ALL_LED_ON_H, on >> 8 );
  wiringPiI2CWriteReg8( fd, ALL_LED_OFF_L, off & 0xFF );
  wiringPiI2CWriteReg8( fd, ALL_LED_OFF_H, off >> 8 );
  return 0;
}

int main() {
  int fd;
  fd = PWMBoardSetup( 0x40 );
  PWMBoardSetFreq( fd, 50 );
  return 0;
}