#include <Wire.h>;
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define PAUSE 1000
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//#include <
#define Sense0_B  11 //(PB3) Sense Rows at (B)ottom or (T)op position
#define Sense0_T  12 //(PB4);
#define Sense1_B  5  //(PD5);
#define Sense1_T  6  //(PD6);
#define Sense2_B  7  //(PD7);
#define Sense2_T  8  //(PB0);
#define Sense3_B  9  //(PB1);
#define Sense3_T  10 //(PB2);

#define bobbin1 0
#define bobbin2 0
#define bobbin3 0
#define bobbin4 0

int location = 0;
int bobbin =  0;

// 1 = Cross
// 0 = Twist
int patterns[1][3] = { //update values as added rows, columns
        {1, 0, 1}, //wholestitch
//        {0, 1, 0, 1} //other stitch
};
int numberPatterns = 1;
String patternNames[]={"wholestich", ""};


int last[]={0,1,2,3};
int goal[]={0,1,2,3};
int current[]={0,1,2,3};
int patternIndex=0;
int stitchIndex=0;
int movetype=0;
  
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Wire.begin();
  display.begin(SSD1306_SWITCHCAPVCC, 0x3D);  
  display.fillScreen(0);
  display.display();
  //delay(2000);



  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,32);         
  display.print("BOBBIN");
display.print("HERO");
display.display();
delay(2000);
display.clearDisplay();


pinMode(Sense0_B, INPUT_PULLUP); // do for all 8
pinMode(Sense0_T, INPUT_PULLUP);
pinMode(Sense1_B, INPUT_PULLUP); 
pinMode(Sense1_T, INPUT_PULLUP);
pinMode(Sense2_B, INPUT_PULLUP);
pinMode(Sense2_T, INPUT_PULLUP);
pinMode(Sense3_B, INPUT_PULLUP);
pinMode(Sense3_T, INPUT_PULLUP);

}



int readBobbins(int location){ //break this out of the loop 
  int B=HIGH;
  int T=HIGH;
  if(location == 0){   // (4) if statements for where)
    B = digitalRead(Sense0_B);
    T = digitalRead(Sense0_T);
  }
  
  if(location == 1){
    B = digitalRead(Sense1_B);
    T = digitalRead(Sense1_T);
  }
  
  if(location == 2){
    B = digitalRead(Sense2_B);
    T = digitalRead(Sense2_T);
  }
  
  if(location == 3){
    B = digitalRead(Sense3_B);
    T = digitalRead(Sense3_T);
  }

    
  if(B == HIGH && T == HIGH){    //(4) if statements for what bobbin
    bobbin = 0;
//   Serial.println("flag0");
  }
  if(B == LOW && T == HIGH){
    bobbin = 1; 
//   Serial.println("flag1");
  }
  if(B == HIGH && T == LOW){
    bobbin = 2;
//   Serial.println("flag2");
  }
  if(B == LOW && T == LOW){
    bobbin = 3;
//   Serial.println("flag3");
  }
//  Serial.print("bobbin found    ");
//  Serial.println(bobbin);
  return bobbin;
}



void loop() {
//  Serial.print("pattern name is...   ");
//  Serial.println(patternNames[patternIndex]);
  // put your main code here, to run repeatedly:
//Serial.println("hello world");
 display.drawPixel(64, 32, 1);
 display.setCursor(32,15);
 display.setTextColor(2);
 display.setTextSize(2);
// display.print("BOBBINS ");
  
  display.display(); //dumps everything in screen buffer to screen
  display.clearDisplay();

//Serial.print("last [0] =     ");
//  Serial.println(last[0]);
//Serial.print("last [1] =     ");
//  Serial.println(last[1]);
//Serial.print("last [2] =     ");
//  Serial.println(last[2]);
//Serial.print("last [3] =     ");
//  Serial.println(last[3]);

delay(5000);

current[0] = readBobbins(0);
Serial.print("current[0] =   ");
Serial.println(current[0]);
current[1] = readBobbins(1);
Serial.print("current[1] =  ");
Serial.println(current[1]);
current[2] = readBobbins(2);
Serial.print("current[2] =  ");
Serial.println(current[2]);
current[3] = readBobbins(3);
Serial.print("current[3] =  ");
Serial.println(current[3]);

delay(2000);
// first pick a move and increment placeholder

movetype=patterns[patternIndex][stitchIndex];


//then evaluate what new should be in correct move
 if (movetype == 0) { //TWIST
  goal[0] = last[1];
  goal[1] = last[0];
  goal[2] = last[3];
  goal[3] = last[2];
  display.print("TWIST      ");
  display.display();
}
else{
  goal[0] = last[0];//last[0]; //CROSS    ANOTHER IF FOR CROSS??? A START VECTOR? 
  goal[1] = last[2];//last[2];
  goal[2] = last[1];//last[1];
  goal[3] = last[3];//last[3];
  display.print("CROSS      ");
  display.display();
}
//Serial.print("movetype =     ");
//  Serial.println(movetype);

//delay(2000);

Serial.print("goal [0] =     ");
  Serial.println(goal[0]);
Serial.print("goal [1] =     ");
  Serial.println(goal[1]);
Serial.print("goal [2] =     ");
  Serial.println(goal[2]);
Serial.print("goal [3] =     ");
  Serial.println(goal[3]);

delay(5000);


//give user 10 sec to move
//delay(10000);
Serial.println("time: 10");
delay(1000);
Serial.println("time: 9");
delay(1000);
Serial.println("time: 8");
delay(1000);
Serial.println("time: 7");
delay(1000);
Serial.println("time: 6");
delay(1000);
Serial.println("time: 5");
delay(1000);
Serial.println("time: 4");
delay(1000);
Serial.println("time: 3");
delay(1000);
Serial.println("time: 2");
delay(1000);
Serial.println("time: 1");
delay(1000);
//read current state



current[0] = readBobbins(0);
Serial.print("current[0] =   ");
Serial.println(current[0]);
current[1] = readBobbins(1);
Serial.print("current[1] =  ");
Serial.println(current[1]);
current[2] = readBobbins(2);
Serial.print("current[2] =  ");
Serial.println(current[2]);
current[3] = readBobbins(3);
Serial.print("current[3] =  ");
Serial.println(current[3]);

Serial.print("goal [0] =     ");
  Serial.println(goal[0]);
Serial.print("goal [1] =     ");
  Serial.println(goal[1]);
Serial.print("goal [2] =     ");
  Serial.println(goal[2]);
Serial.print("goal [3] =     ");
  Serial.println(goal[3]);

//Serial.print("bobbin # = ");
//Serial.println(bobbin);
       
//compare to goal
if ((current[0] == goal[0]) && (current[1] == goal[1]) 
&& (current[2] == goal[2]) && (current[3] == goal[3])){

  display.print("YES");
  Serial.println("YES"); //if correct display yes
  last[0] = goal[0];  //update last position
  last[1] = goal[1];
  last[2] = goal[2];
  last[3] = goal[3];
  stitchIndex ++; 
  if (stitchIndex >= sizeof(patterns[patternIndex])){ //occurs when end of stitch is reached
    Serial.println(patternNames[patternIndex]);
    stitchIndex = 0;
    patternIndex ++;
    if (patternIndex == numberPatterns){
      patternIndex=0;
      display.clearDisplay();
      display.print("wholestitch!");
      display.display();
      delay(2000);
      // you finished the last one roll credits
      
      //Serial.println("wholestitch");
    }
//    if (patternIndex >=  0) {//NUMBER OF PATTERNS TOTAL -1
//      patternIndex = 0;
//    }
  }
}
  else{Serial.println("FAIL"); // else display failed
  display.print("FAIL");
  last[0] = current[0];  //update last position
  last[1] = current[1];
  last[2] = current[2];
  last[3] = current[3];
  stitchIndex=0;
  // if we want to restart set set stitchIndex back to 0 
  // maybe display startover gameover etc whatever
  }




// at the very end of loop

// increment stitch index
//if stitchIndex > length of pattern-1 reset stitchIndex to 0 and choose new pattern

// 

}