#define GREYSCALES 128
#define LATCH_PORT B00000001
#define CLOCK_PORT B00010000
#define DATA_PORT B00001000


//Pin connected to ST_CP of 74HC595
int latchPin = 8;
//Pin connected to SH_CP of 74HC595
int clockPin = 12;
////Pin connected to DS of 74HC595
int dataPin = 11;

//holder for infromation you're going to pass to shifting function
byte data = 0; 


inline void latchOn() {
  // Port 8
  PORTB |=  B00000001;
}

inline void latchOff() {
  // Port 8
  PORTB &= ~B00000001;
}

inline void dataOn() {
  // Port 11
  PORTB |=  B00001000;
}

inline void dataOff() {
  // Port 11
  PORTB &= ~B00001000;
}

inline void clockOn() {
  // Port 12
  PORTB |=  B00010000;
}

inline void clockOff() {
  // Port 12
  PORTB &= ~B00010000;
}




void setup() {
  //set pins to output because they are addressed in the main loop
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);

}


void loop() {


    lightPwm(100,
       random(GREYSCALES) / 10,
       random(GREYSCALES) / 10,
       random(GREYSCALES) / 10,
       random(GREYSCALES) / 10,
       random(GREYSCALES),
       random(GREYSCALES),
       random(GREYSCALES)
     );


}


void lightPwm(long duration, 
              int white, 
              int r1, int g1, int b1, 
              int r2, int g2, int b2) {
  
  byte data = 0;                

  // TODO: Gamma
  int r1tar = r1;
  int g1tar = g1;
  int b1tar = b1;
  int r2tar = r2;
  int g2tar = g2;
  int b2tar = b2;
  int whtar = white;
  
  int r1inv = GREYSCALES - r1tar;
  int g1inv = GREYSCALES - g1tar;
  int b1inv = GREYSCALES - b1tar;
  int r2inv = GREYSCALES - r2tar;
  int g2inv = GREYSCALES - g2tar;
  int b2inv = GREYSCALES - b2tar;
  int whinv = GREYSCALES - whtar;
                

  int r1err = 0;
  int g1err = 0;
  int b1err = 0;
  int r2err = 0;
  int g2err = 0;
  int b2err = 0;
  int wherr = 0;
                
  for (int t=duration; t >= 0; t--)  {    
    data = 0;
    for (int i=GREYSCALES-1; i >= 0; i--)  {

      if (r1err > 0) {data |= B00000001; r1err -= r1inv;} else {data &= ~B00000001; r1err += r1tar;}
      if (g1err > 0) {data |= B00000010; g1err -= g1inv;} else {data &= ~B00000010; g1err += g1tar;}
      if (b1err > 0) {data |= B00000100; b1err -= b1inv;} else {data &= ~B00000100; b1err += b1tar;}
      if (r2err > 0) {data |= B00001000; r2err -= r2inv;} else {data &= ~B00001000; r2err += r2tar;}
      if (g2err > 0) {data |= B00010000; g2err -= g2inv;} else {data &= ~B00010000; g2err += g2tar;}
      if (b2err > 0) {data |= B00100000; b2err -= b2inv;} else {data &= ~B00100000; b2err += b2tar;}
      if (wherr > 0) {data |= B01000000; wherr -= whinv;} else {data &= ~B01000000; wherr += whtar;}
      
      
/*      
      if (r1 == i)    data |= B00000001;
      if (g1 == i)    data |= B00000010;
      if (b1 == i)    data |= B00000100;
      if (r2 == i)    data |= B00001000;
      if (g2 == i)    data |= B00010000;
      if (b2 == i)    data |= B00100000;
      if (white == i) data |= B01000000;
*/

      shiftOut(data);       
      shiftOut(data);       
      shiftOut(data);       
      shiftOut(data);             
      shiftOut(data);       
    }

  }
} 


inline void shiftOut(byte data) {

  PORTB &= ~LATCH_PORT; // Latch off

  PORTB &= ~CLOCK_PORT; // Clock off
  if (data & B00000001) PORTB |= DATA_PORT; else PORTB &= ~DATA_PORT;
  PORTB |=  CLOCK_PORT; // Clock on

  PORTB &= ~CLOCK_PORT; // Clock off
  if (data & B00000010) PORTB |= DATA_PORT; else PORTB &= ~DATA_PORT;
  PORTB |=  CLOCK_PORT; // Clock on
 
  PORTB &= ~CLOCK_PORT; // Clock off
  if (data & B00000100) PORTB |= DATA_PORT; else PORTB &= ~DATA_PORT;
  PORTB |=  CLOCK_PORT; // Clock on
 
  PORTB &= ~CLOCK_PORT; // Clock off
  if (data & B00001000) PORTB |= DATA_PORT; else PORTB &= ~DATA_PORT;
  PORTB |=  CLOCK_PORT; // Clock on
 
  PORTB &= ~CLOCK_PORT; // Clock off
  if (data & B00010000) PORTB |= DATA_PORT; else PORTB &= ~DATA_PORT;
  PORTB |=  CLOCK_PORT; // Clock on
 
  PORTB &= ~CLOCK_PORT; // Clock off
  if (data & B00100000) PORTB |= DATA_PORT; else PORTB &= ~DATA_PORT;
  PORTB |=  CLOCK_PORT; // Clock on
 
  PORTB &= ~CLOCK_PORT; // Clock off
  if (data & B01000000) PORTB |= DATA_PORT; else PORTB &= ~DATA_PORT;
  PORTB |=  CLOCK_PORT; // Clock on
 
  PORTB &= ~CLOCK_PORT; // Clock off
  if (data & B10000000) PORTB |= DATA_PORT; else PORTB &= ~DATA_PORT;
  PORTB |=  CLOCK_PORT; // Clock on

  PORTB |=  LATCH_PORT; // Latch on

}