// RGB Lock driver
//
// Version 2 - couple bugfixes, adds 30s timeout, press 3 buttons simultaneously to reset.

#define DATAOUT 11  // MOSI - master out
#define DATAIN  12  // MISO - slave out
#define SCK  13     // clock
#define CS 10       // slave select
#define CLKDELAY 25 // 25 microsecond delay for talking to SPI board
#define RELAYPIN 9  // controls lock relay
#define NUMCOLORS 8 // number of colors in the sequence
#define NO_KEYPRESS_TIMEOUT 30000 // timeout in ms to reset the board if no keys have been pressed

// Initial state of buttons.   1  2  3  4  5  6  7  8  9 10 11 12 13 14 15 16
int initialButtonColorMap[] = {0, 1, 2, 0, 3, 4, 5, 0, 6, 0, 1, 0, 0, 0, 0, 0};

// Victory condition.  Note that we are only using 3x3 grid out of 4x4 buttons, so
// buttons 4, 8, 12, 13, 14, 15, 16 are unused (and marked with -1).
//
// Solution: 0 off,     1 magenta,  2 cyan
//           4 white,   5 off,      6 blue
//           8 magenta, 9 yellow,  10 cyan
//
//  0     1     2     3        4     5       6      7
// Red, Green, Blue, Yellow, Cyan, Magenta, White, Off
//
// EDIT THIS TO CHANGE THE SOLUTION!
const int solutionButtonColorMap[] = {7, 5, 4, -1, 6, 7, 2, -1, 5, 3, 4, -1, -1, -1, -1, -1};

// Colors: Red, Green, Blue, Yellow, Cyan, Magenta, White, Off.
const unsigned long colorSequence[] = {0x00FF0000, 0x0000FF00, 0x000000FF, 
                                       0x00FFFF00, 0x0000FFFF, 0x00FF00FF, 
                                       0x00FFFFFF, 0x00000000};

// Variables.

byte redState[16];
byte greenState[16];
byte blueState[16];
byte buttonStateOld[16];

unsigned long lastKeyPress = 0;

boolean isSolved = false;
boolean needToResetButtons = false;
boolean needToShowVictory = false;

// Current state of buttons.
int buttonColorMap[] = {0, 1, 2, 0, 3, 4, 5, 0, 6, 0, 1, 0, 0, 0, 0, 0};

   
                        
void setup() 
{ 
  Serial.begin(9600); 
  
  // Config I/O pins
  pinMode(DATAOUT, OUTPUT);
  pinMode(DATAIN, INPUT);
  pinMode(SCK, OUTPUT);
  pinMode(CS, OUTPUT);
  pinMode(RELAYPIN, OUTPUT);

  Serial.println("Waiting 1 sec for board startup");
  delay(1000);
  Serial.println("Start board config...");
  ConfigBoard();
  Serial.println("Board config done");
  
  SetBoard(0, 255, 0);
  isSolved = true;
  Serial.println("Unlocking chest");
  digitalWrite(RELAYPIN, LOW);
} 

// Ensures the board is configured as a single board.
void ConfigBoard()
{
  digitalWrite(CS, LOW);
  digitalWrite(DATAOUT, LOW);
  digitalWrite(SCK, LOW);
  
  delayMicroseconds(CLKDELAY);
  
  //Enter CMD mode
  digitalWrite(DATAOUT, HIGH);
  ClockOne();
  
  //Write Command ID(0x01)
  digitalWrite(DATAOUT, HIGH);
  ClockOne();
  digitalWrite(DATAOUT, LOW);
  ClockOne();
  ClockOne();
  ClockOne();
  ClockOne();
  ClockOne();
  ClockOne();
  ClockOne();
  
  //Write Command Parameter(0x01) = # of boards
  digitalWrite(DATAOUT, HIGH);
  ClockOne();
  digitalWrite(DATAOUT, LOW);
  ClockOne();
  ClockOne();
  ClockOne();
  ClockOne();
  ClockOne();
  ClockOne();
  ClockOne();
}

void ClockOne()
{
  delayMicroseconds(CLKDELAY);
  digitalWrite(SCK, HIGH);
  delayMicroseconds(CLKDELAY);
  digitalWrite(SCK, LOW);
  delayMicroseconds(CLKDELAY);
}

void SendByte(byte b)
{
  byte mask = 0x01;
  
  for(int i = 0; i < 8; i++)
  {
    if((mask & b) == 0)
    {
      digitalWrite(DATAOUT, LOW);
    }
    else
    {
      digitalWrite(DATAOUT, HIGH);
    }
    
    ClockOne();
    mask <<= 1;
  }
}

// Used for clocking out key data
byte SendAndReadByte(byte b)
{
  byte retVal = 0x00;
  byte mask = 0x01;
  
  for(int i = 0; i < 8; i++)
  {
    if((mask & b) == 0)
    {
      digitalWrite(DATAOUT, LOW);
    }
    else
    {
      digitalWrite(DATAOUT, HIGH);
    }
    
    // Clock.
    delayMicroseconds(CLKDELAY);
    digitalWrite(SCK, HIGH);
    delayMicroseconds(CLKDELAY);
    
    // Read key data.
    if(digitalRead(DATAIN) == HIGH)
    {
      retVal |= 0x01;
    }
    
    digitalWrite(SCK, LOW);
    delayMicroseconds(CLKDELAY);
    
    if(i<7)
    {
      retVal <<= 1;
    }
    mask <<= 1;
  }
  
  return retVal;
}

// Sends all the board state to the board.
void SendData(boolean checkButtons)
{
  //Init
  digitalWrite(SCK, HIGH);
  delayMicroseconds(CLKDELAY);
  digitalWrite(CS, HIGH);
  delayMicroseconds(CLKDELAY);
  digitalWrite(SCK, LOW);
  
  // Send the data.
  
  // Red
  for (int i = 0; i < 16; i++)
  {
    SendByte(redState[i]);
  }
  
  // Green
  for (int i = 0; i < 16; i++)
  {
    SendByte(greenState[i]);
  }
  
  // Blue
  for (int i = 0; i < 16; i++)
  {
    SendByte(blueState[i]);
  }
  
  // Buttons - clock out dummy data, read in button state.
  int newButtonsPressed = 0;
  int totalButtonsPressed = 0;
  int whichNewButtonWasPressed = -1;
  
  for (int i = 0; i < 16; i++)
  {
    byte buttonState = SendAndReadByte(0x00);
    
    if(checkButtons == true)
    {
      // 0x00 means pressed, 0xFF means not.
      if(buttonState == 0x00)
      {
        totalButtonsPressed++;
        // Check if state changed from off -> on.
        if(buttonStateOld[i] != 0x00)
        {
          newButtonsPressed++;
          whichNewButtonWasPressed = i;
        }
      }
      buttonStateOld[i] = buttonState;
    }
  }
  if(totalButtonsPressed == 1 && newButtonsPressed == 1)
  {
    // Only change colors if exactly one button is pressed and it was newly pressed.
    Serial.print("One (new) button pressed: ");
    Serial.println(whichNewButtonWasPressed, DEC);
    HandleButtonPress(whichNewButtonWasPressed);
  }
  else if(totalButtonsPressed >= 3 && newButtonsPressed >= 1)
  {
    // If at least 3 buttons are pressed in total, and at least one of them new, reset the board.
    Serial.println("More than 3 buttons pressed, resetting...");
    needToResetButtons = true;
  }

  // Wrap up.
  digitalWrite(CS, LOW);
  delayMicroseconds(CLKDELAY);
}

// Increment button color state when pressed. Do not send data from here - directly or indirectly.
void HandleButtonPress(int buttonNumber)
{
  lastKeyPress = millis();
  
  if(isSolved)
  {
    isSolved = false;
    needToResetButtons = true;
    
    // Lock chest.
    Serial.println("Locking chest");
    digitalWrite(RELAYPIN, HIGH);
  }
  else
  {
    buttonColorMap[buttonNumber] = buttonColorMap[buttonNumber]++;
    if(buttonColorMap[buttonNumber] > 7)
    {
      buttonColorMap[buttonNumber] = 0;
    }
    
    SetRGBStates(buttonNumber);
    
    // Check for solution.
    bool correct = true;
    for(int i = 0; i < 16; i++)
    {
      // -1 means unused, don't compare those.    
      if((solutionButtonColorMap[i] != -1) &&
         (solutionButtonColorMap[i] != buttonColorMap[i]))
      {
        correct = false;
        break;
      }
    }
    
    if(correct)
    {
      isSolved = true;
      needToShowVictory = true;
      
      // Unlock chest.
      Serial.println("Unlocking chest");
      digitalWrite(RELAYPIN, LOW);
    }
  }
}

// Calculates RGB states for a button.
void SetRGBStates(int button)
{
  unsigned long color = colorSequence[buttonColorMap[button]];
  
  redState[button] = (color & 0x00FF0000) >> 16;
  greenState[button] = (color & 0x0000FF00) >> 8;
  blueState[button] = color & 0x000000FF;
}

// Sets the entire board state and then updates the board.
void SetBoard(byte red, byte green, byte blue)
{
  for(int i = 0; i < 16; i++)
  {
    redState[i] = red;
    greenState[i] = green;
    blueState[i] = blue;
  }
  
  SendData(false);
}

void InitButtons()
{
  Serial.println("InitButtons");
  for(int i = 0; i < 16; i++)
  {
    buttonColorMap[i] = initialButtonColorMap[i];
    SetRGBStates(i);
  }
}

void ShowVictory()
{
  Serial.println("Show victory");
  SetBoard(0, 0, 0);
  delay(600);
  
  SetBoard(0, 255, 0);
  delay(1000);
  SetBoard(0, 0, 0);
  delay(600);
  SetBoard(0, 255, 0);
  delay(1000);
  SetBoard(0, 0, 0);
  delay(600);
  SetBoard(0, 255, 0);
  delay(1000);
  SetBoard(0, 0, 0);
  delay(600);
  SetBoard(0, 255, 0);
}

// Main program loop, called in loop by the framework.
void loop() 
{ 
  if(!isSolved &&
     (millis() - lastKeyPress > NO_KEYPRESS_TIMEOUT))
  {
    Serial.println("timed out, resetting...");
    needToResetButtons = true;
    lastKeyPress = millis();
    Serial.println("Locking chest");
    digitalWrite(RELAYPIN, HIGH);
  }
  
  if(needToResetButtons)
  {
    needToResetButtons = false;
    InitButtons();
  }
  else if(needToShowVictory)
  {
    needToShowVictory = false;
    ShowVictory();
  }
  else
  {
    SendData(true);
  }
  
  delay(10);
}