// Remember ~ 1.5k pullups for I2C data and clock lines!

#include <Wire.h>
#define BASE_I2C_ADDRESS B1000000
#define PRESCALE_REG 0xFE
int MAX_VALUE = 4096;
int i2cAddress = 0;
int i2cAddress2 = 0;
int ledMapping[] = {
   6, 5, 7,
   3, 2, 4,
   1, 14, 0,
   12, 13, 11,
   9, 10, 8,
   15};
void setup()
{
  // Connect debug line
  Serial.begin(9600);
  Serial.println("Led blinker starting");
  Wire.begin();
  
  int chipAddress1 = 1;
  int chipAddress2 = 2;
  i2cAddress = (chipAddress1 & B00111111) | BASE_I2C_ADDRESS;
  i2cAddress2 = (chipAddress2 & B00111111) | BASE_I2C_ADDRESS;
  
  resetDriver();
  clearLeds(i2cAddress);
  clearLeds(i2cAddress2);
}

void loop()
{
  
  int top = MAX_VALUE - 1;  
  
  /*
  // Clear
  for (int i = 0; i < 16; i++) {
    setLedValue(i2cAddress, i, 0);
  }
  */
 /*
 // Walk
  for (int i = 0; i < 16; i++) {
    setLedValue(i2cAddress, i, top);
    delay(2500);
    setLedValue(i2cAddress, i, 0);
    delay(500);
  }
  */
  for (int v = 0; v < top; v += 10) {
    for (int led = 0; led < 5; led++) {
      setColor(i2cAddress, led, v, v/(5+4*led), v/40-led*10);
      setColor(i2cAddress2, led, top-v, v/(5+4*led), v/40-led*10);
    }
    setLedValue(i2cAddress, 15, sin(v*0.01) * 4096);
    setLedValue(i2cAddress2, 15, sin(v*0.01) * 4096);
    
    delay(3);
  }
  delay(1000);
  for (int v = top-1; v > 0; v -= 10) {
    for (int led = 0; led < 5; led++) {
      setColor(i2cAddress, led, v, v/(5+4*led), v/40-led*10);
      setColor(i2cAddress2, led, top - v, v/(5+4*led), v/40-led*10);
    }
    
    setLedValue(i2cAddress, 15, sin(v*0.01) * 4096);
    setLedValue(i2cAddress2, 15, sin(v*0.01) * 4096);
    delay(4);
  }
  clearLeds(i2cAddress);
  delay(1000);
}


void setColor(int address, int rgbled, int red, int green, int blue) {
  setLedValue(address, rgbled * 3, red);
  setLedValue(address, rgbled * 3 + 1, green);
  setLedValue(address, rgbled * 3 + 2, blue);
}


void resetDriver() {
  
  readDriverData(i2cAddress);
  
  delay(1);
  
  byte mode1Reg = 0;
  byte mode2Reg = 1;
  
  delay(2);
  // Reset
  writeI2cRegister(i2cAddress, mode1Reg, (byte)0x01);  
  delay(2);
  // Set prescale
  writeI2cRegister(i2cAddress, mode1Reg, (byte)0x110000);  // Sleep on
  delay(2);
  writeI2cRegister(i2cAddress, 254, 3);
  delay(2);
  // Set auto increment on
  writeI2cRegister(i2cAddress, mode1Reg, (byte)B00100001);
  
  writeI2cRegister(i2cAddress, mode2Reg, (byte)B00100);
  delay(1);
  
  
  
  readDriverData(i2cAddress);
  
}
void readDriverData(int address) {
  delay(500);
  // Read mode 
  Serial.print("Reading data from led driver with address ");
  Serial.println(address);
  delay(10);
  int numToRead = 32;
  Wire.requestFrom(address, numToRead);
  int reg = 0;
  while(Wire.available() > 0) {
    int data = Wire.read();
    Serial.print(reg); 
    Serial.print(": "); 
    Serial.println(data); 
    delay(1);
    reg++;
  }
  delay(500);
}
void writeI2cRegister(int address, byte reg, byte data) {
  Serial.print("Writing ");
  Serial.print(data);
  Serial.print(" to register ");
  Serial.print(reg);
  Serial.print(" on device ");
  Serial.println(address);
  
  Wire.beginTransmission(address);
  Wire.write(reg);
  Wire.write(data);
  int result = Wire.endTransmission();
  if (result == 0) Serial.println("Sent successfully");
  else {
    Serial.print("Write error: ");
    Serial.println(result);
  }
}
void setLedValue(int address, int led, int value) {
  value = constrain(value, 0, MAX_VALUE);
  //int phaseShift = address * 13 + led * 17;
  int phaseShift = 0;
  setLedLightPeriod(address, ledMapping[led], phaseShift, (value + phaseShift) & (MAX_VALUE - 1));
/*
  Serial.print("Setting LED ");
  Serial.print(led);
  Serial.print(" to ");
  Serial.println(value);
  */
}
void clearLeds(int address) {
  for (int i = 0; i < 16; i++) {
    setLedLightPeriod(address, i, 0, MAX_VALUE);
  }
}
void setLedLightPeriod(int address, int led, word onPoint, word offPoint) {
  int ledReg = 6;
  
  if (led >=0 && led < 16)    {
    
    Wire.beginTransmission(address);
    Wire.write(ledReg + 4 * led);
    
    Wire.write(lowByte(onPoint));
    Wire.write(highByte(onPoint));
    Wire.write(lowByte(offPoint));
    Wire.write(highByte(offPoint));
    
    int result = Wire.endTransmission();
    if (result != 0) {
      Serial.print("Led data write error: ");
      Serial.println(result);
    }
  }
}