#include <Arduino.h>
//#include <SPI.h>
//#include <Wire.h>
//#include <Adafruit_GFX.h>
//#include <Adafruit_SSD1306.h>

#define outputpins 16
#if (outputpins % 8) > 0
  #define outputbytes (outputpins / 8) + 1
#else
  #define outputbytes (outputpins / 8)
#endif

#define SDA 21
#define SCL 22
#define SRCLR 19  // Clear Serial registers (Active low)
#define GOUT 5    // Enable drivers (Active low)
#define RCK 17    // Clock Shift register into driver register
#define SRCK 18   // Shift register clock
#define MOSI 23   // Serial data into shift register

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32

//Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
uint8_t outputstate[outputbytes];

void sendOutput();
void setBit(uint8_t outputpin, boolean level);
boolean getBit(uint8_t outputpin);

void setup() {
  pinMode(SRCLR, OUTPUT);
  pinMode(GOUT, OUTPUT);
  pinMode(RCK, OUTPUT);
  pinMode(SRCK, OUTPUT);
  pinMode(MOSI, OUTPUT);

  Serial.begin(9600);

  digitalWrite(GOUT, HIGH);
  digitalWrite(SRCLR, LOW); // reset all registers
  digitalWrite(RCK, LOW);   // 
  digitalWrite(SRCK, LOW);
  digitalWrite(SRCLR, HIGH);
  digitalWrite(GOUT, LOW);

  for (uint8_t i = 0; i < outputbytes; i++) {
    outputstate[i] = 00;
  }
}

void loop() {
  Serial.println("Start 1");
  for (uint8_t i = 0; i < outputpins; i++) {
    Serial.print(i);
    setBit(i, true);
    sendOutput(); 
  }
  Serial.println(".");
  for (uint8_t i = 0; i < outputpins; i++) {
    Serial.print(i);
    setBit(i, false);
    sendOutput(); 
  }
  Serial.println("-");
}

void sendOutput() {
  // Output bits to shiftregister, starting with MSByte and MSBit to LSB
  uint8_t tempbyte;
  for (uint8_t i = outputbytes; i > 0; i--) {
    tempbyte = outputstate[i];
    for (uint8_t j = 0; j < 8; j++) {
      if ((i && 0x80) > 0) {
        digitalWrite(MOSI, HIGH);
      } else {
        digitalWrite(MOSI, LOW);
      }
      digitalWrite(SRCK, HIGH);
      digitalWrite(SRCK, LOW);
      tempbyte << 1;
    }
  }
  digitalWrite(RCK, HIGH);
  digitalWrite(RCK, LOW);
}

void setBit(uint8_t outputpin, boolean level) {
  // set a specific bit in the range 1-(outputpins)
  uint8_t tempbyte;
  uint8_t targetbyte;
  if (outputpin > outputpins) return;
  targetbyte = outputpin / 8;
  if ((outputpin % 8) > 0) targetbyte++;
  tempbyte = 0x01;
  if ((outputpin % 8) > 0) tempbyte << (outputpin % 8);
  if (level) {
    outputstate[targetbyte] |= tempbyte;
  } else {
    tempbyte = 255 - tempbyte;
    outputstate[targetbyte] &= tempbyte;
  }
}

boolean getBit(uint8_t outputpin) {
  // return the state of a specific bit in the range 0-(outputpins-1)
  uint8_t tempbyte;
  uint8_t targetbyte;
  if (outputpin > outputpins) return false;
  targetbyte = outputpin / 8;
  if ((outputpin % 8) > 0) targetbyte++;
  tempbyte = 0x01;
  if ((outputpin % 8) > 0) tempbyte << (outputpin % 8);
  if ((outputstate[targetbyte] && tempbyte) > 0) {
    return true;
  } else {
    return false;
  }
}