#include <Adafruit_NeoPixel.h>

#define LED_PIN 9

#define LED_COUNT 16

Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);
// 74HC595 Pins
#define dataPin 2   // DS pin of 74HC595
#define clockPin 3  // SHCP pin of 74HC595
#define latchPin 4  // STCP pin of 74HC595

// 74HC165 Pins
#define loadPin 5   // PL pin of 74HC165
#define clockEnablePin 6  // CE pin of 74HC165
#define dataPinIn 7  // QH pin of 74HC165
#define clockPinIn 8  // SHCP pin of 74HC165

#define n_rows 8 // must be even to decode the board
#define n_cols 8

typedef byte Board[n_rows]; // global variable updated by read_board()
typedef byte Decoded_Board[n_rows / 2][n_cols/2];

Board board;           // global variable updated by read_board()
Decoded_Board decoded_board; // global variable updated from board 

Decoded_Board test_board = {
  1, 2, 3, 4,
  5, 6, 7, 8,
  0, 0, 0, 0,
  0, 0, 0, 0
};
void setup() {
  //Neopixel Setup
  strip.begin();
  strip.show();

  // 74HC595 Setup
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);

  // 74HC165 Setup
  pinMode(loadPin, OUTPUT);
  pinMode(clockEnablePin, OUTPUT);
  pinMode(clockPinIn, OUTPUT);
  pinMode(dataPinIn, INPUT);

  Serial.begin(9600);
  Serial.println("Program starting.\n");
}

void loop() {
  // read_board(&board);   // Pass the board pointer to the function
  // //print_board(&board);  // Print out the board for debugging
  // decode_board(&board, &decoded_board);
  // print_decoded_board(&decoded_board);
  for(byte i = 0; i < 16; i++) {
    color_pixels(i);
    delay(500);
  }
}

void show_progress(Decoded_Board* d) {
  byte correct_pieces = 0;
  for(byte i = 0; i < n_rows/2; i++) {
    for(byte j = 0; j < n_cols/2; j++) {
      if(4*i+j + 1 == (*d)[i][j]) {
        correct_pieces += 1;
      }
    }
  }
  strip.setPixelColor(0, round(255*(15-correct_pieces)/15), round(255*correct_pieces/15), 0); //(pixel, r, g, b)
  strip.show();
}

void color_pixels(byte correct) {
  float r = (255*pow(15-correct, 2))/(15*15);
  float g = (255*pow(correct, 2))/(15*15);
  float b = (255*2*(correct)*(15-correct))/15;
  strip.setPixelColor(0, round(r), round(g), round(b)); //(pixel, r, g, b)
  strip.show();
}

void print_decoded_board(Decoded_Board* d) {
  for(byte i = 0; i < n_rows/2; i++) {
    for(byte j = 0; j < n_cols/2; j++) {
      Serial.print((*d)[i][j]); Serial.print("  ");
    }
    Serial.println();
  }
  Serial.println("\n");
}

void decode_board(Board* b, Decoded_Board* d) {
  for(byte i = 0; i < n_rows/2; i++) {
    for(byte j = 0; j < n_cols/2; j++) {
      byte raw =  (1&(*b)[2*i]>>(2*j))<<0 |
                    (1&(*b)[2*i]>>(2*j+1))<<1 |
                    (1&(*b)[2*i+1]>>(2*j))<<2 |
                    (1&(*b)[2*i+1]>>(2*j+1))<<3;
      (*d)[i][j] = 15 - raw;
    }
  }
}

void read_board(Board* b) {
  for (byte i = 0; i < n_rows; i++) {
    writeShiftRegister_595(1 << i);
    (*b)[i] = readShiftRegister_165();
  }
}

void print_board(Board* b) {
  for (byte i = 0; i < n_rows; i++) {
    //Serial.print("Row "); Serial.print(i); Serial.print(": ");
    print_byte((*b)[i]);
  }
  Serial.println("\n");
}

void print_byte(byte b) {
  for (byte j = 0; j < 8; j++) {
    Serial.print(b >> j & 1);
    Serial.print("  ");
  }
  Serial.println();
}

byte readShiftRegister_165() {
  byte inputBits = 0;

  // Load the parallel inputs into the shift register
  digitalWrite(loadPin, LOW);
  delayMicroseconds(5);
  digitalWrite(loadPin, HIGH);

  // Read the serial data
  for (int i = 0; i < 8; i++) {
    digitalWrite(clockPinIn, LOW);
    delayMicroseconds(5);
    inputBits |= digitalRead(dataPinIn) << (7 - i);
    digitalWrite(clockPinIn, HIGH);
    delayMicroseconds(5);
  }

  return inputBits;
}

void writeShiftRegister_595(byte data) {
  // Prepare to shift data
  digitalWrite(latchPin, LOW);

  // Shift out the data
  shiftOut(dataPin, clockPin, MSBFIRST, data);

  // Lock the data into the output
  digitalWrite(latchPin, HIGH);
}