#include <SPI.h>
#include <Wire.h>
#include <Adafruit_MCP23008.h>

Adafruit_MCP23008 mcp;

#define BUTTON1 0
#define BUTTON2 1
#define BUTTON3 2
#define S_BUTTON 3

#define LED1 4
#define LED2 5
#define LED3 6
#define S_LED 7

int potValues[5];
int potPositions[5];

int presets[3][5];

bool storeMode = false;
int storePreset = -1;

void setup() {
  Serial.begin(9600);
  mcp.begin();

  mcp.pinMode(LED1, OUTPUT);
  mcp.pinMode(LED2, OUTPUT);
  mcp.pinMode(LED3, OUTPUT);
  mcp.pinMode(S_LED, OUTPUT);

  mcp.pinMode(BUTTON1, INPUT);
  mcp.pullUp(BUTTON1, HIGH);
  mcp.pinMode(BUTTON2, INPUT);
  mcp.pullUp(BUTTON2, HIGH);
  mcp.pinMode(BUTTON3, INPUT);
  mcp.pullUp(BUTTON3, HIGH);
  mcp.pinMode(S_BUTTON, INPUT);
  mcp.pullUp(S_BUTTON, HIGH);

  for(int i=0; i<5; i++){
    potValues[i] = 0;
    potPositions[i] = 0;
  }

  for(int i=0; i<3; i++){
    for(int j=0; j<5; j++){
      presets[i][j] = 0;
    }
  }  
}

void loop() {
  if(storeMode){
    if(mcp.digitalRead(S_BUTTON) == LOW){
      if(storePreset == 0){
        storeDataToPreset(0);
      }else if(storePreset == 1){
        storeDataToPreset(1);
      }else if(storePreset == 2){
        storeDataToPreset(2);
      }

      for(int i=0; i<3; i++){
        mcp.digitalWrite(LED1, LOW);
        mcp.digitalWrite(LED2, LOW);
        mcp.digitalWrite(LED3, LOW);
      }

      mcp.digitalWrite(S_LED, HIGH);
      delay(500);
      mcp.digitalWrite(S_LED, LOW);

      storeMode = false;
      storePreset = -1;
    }
  }
  else{
    handleButton(BUTTON1, 0);
    handleButton(BUTTON2, 1);
    handleButton(BUTTON3, 2);
  }

  delay(100);
}

void storeDataToPreset(int preset){
  for(int i=0; i<5; i++){
    presets[preset][i] = potValues[i];
  }
}

void handleButton(int buttonPin, int preset){
  if(mcp.digitalRead(buttonPin) == LOW){
    for(int i=0; i<5; i++){
      potValues[i] = presets[preset][i];
      potPositions[i] = presets[preset][i];
    }

    mcp.digitalWrite(LED1, LOW);
    mcp.digitalWrite(LED2, LOW);
    mcp.digitalWrite(LED3, LOW);
    mcp.digitalWrite(buttonPin, HIGH);

    delay(500);
  }
}