/*
Written by cheese and fibbef for BitBuilt
Using information from:
https://web.archive.org/web/20120724014753/http://repairfaq.ece.drexel.edu/REPAIR/F_SNES.html#SNES_004
*/
#define p1_d1 2
#define p1_clock 3
#define latch_pin 4
#define p2_d1 5
#define p2_clock 6
#define p1_b 22
#define p1_y 23
#define p1_select 24
#define p1_start 25
#define p1_up 26
#define p1_down 27
#define p1_left 28
#define p1_right 29
#define p1_a 30
#define p1_x 31
#define p1_l 32
#define p1_r 33
#define p2_b 34
#define p2_y 35
#define p2_select 36
#define p2_start 37
#define p2_up 38
#define p2_down 39
#define p2_left 40
#define p2_right 41
#define p2_a 42
#define p2_x 43
#define p2_l 44
#define p2_r 45

int data1 = 0;
int data2 = 0;
int temp1_0 = 0;
int temp1_1 = 0;
int temp2_0 = 0;
int temp2_1 = 0;
int k = 0;
int m = 0;
int contType = 0;

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(2,INPUT); //p1 d1
  pinMode(3,OUTPUT); //p1 clock
  pinMode(4,OUTPUT); //latch
  pinMode(5,INPUT); //p2 d1
  pinMode(6,OUTPUT); //p2 clock
  digitalWrite(p1_clock, HIGH);
  digitalWrite(p2_clock, HIGH);
  digitalWrite(latch_pin, LOW);
  Serial.begin(9600);
        pinMode(p1_b,INPUT);
        pinMode(p1_y,INPUT);
        pinMode(p1_select,INPUT);
        pinMode(p1_start,INPUT);
        pinMode(p1_up,INPUT);
        pinMode(p1_down,INPUT);
        pinMode(p1_left,INPUT);
        pinMode(p1_right,INPUT);
        pinMode(p1_a,INPUT);
        pinMode(p1_x,INPUT);
        pinMode(p1_l,INPUT);
        pinMode(p1_r,INPUT);
        pinMode(p2_b,INPUT);
        pinMode(p2_y,INPUT);
        pinMode(p2_select,INPUT);
        pinMode(p2_start,INPUT);
        pinMode(p2_up,INPUT);
        pinMode(p2_down,INPUT);
        pinMode(p2_left,INPUT);
        pinMode(p2_right,INPUT);
        pinMode(p2_a,INPUT);
        pinMode(p2_x,INPUT);
        pinMode(p2_l,INPUT);
        pinMode(p2_r,INPUT);
}

void loop() {
  digitalWrite(latch_pin, HIGH);
  delayMicroseconds(10); //doesn't need to be exact, since digitalWrite is slow
  digitalWrite(latch_pin, LOW);

  data1 = 1<<12; //reset data
  data2 = 1<<12; //reset data

  for(int i = 0; i < 12; i++) {
    temp1_0 = 0;
    temp1_1 = 0;
    temp2_0 = 0;
    temp2_1 = 0;
    digitalWrite(p1_clock, LOW);
    digitalWrite(p2_clock, LOW);
    temp1_0 = !digitalRead(p1_d1); //read in data P1
    temp2_0 = !digitalRead(p2_d1); //read in data P2
    digitalWrite(p1_clock, HIGH); //bring clock high to give time for data to come
    digitalWrite(p2_clock, HIGH); //bring clock high to give time for data to come
    temp1_0 = temp1_0 << i; //shift data over
    temp2_0 = temp2_0 << i; //shift data over
    data1 |= temp1_0; //store the data
    data2 |= temp2_0; //store the data
  }

  temp1_1 = data1;
  temp2_1 = data2;
  if(data1 == 5156){ //5156 = L+Sel+Down
    contType = 1;
    Serial.print("Rosalina controls activated.");
    Serial.println();
  }
  
  if(data1 == 5140){ //5140 = L+Sel+Up
    contType = 0;
    Serial.print("Rosalina controls deactivated.");
    Serial.println();
  }
  
  for(int j = 0; j <= 12; j++){ //Calculate P1 button presses
    k = temp1_1 >> j;
    m = temp2_1 >> j;
    if(k & 1){
      if(j == 0){
        pinMode(p1_b,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_b,LOW);
        }else if(contType == 1){
          digitalWrite(p1_b,HIGH);
        }
      }else if(j == 1){
        pinMode(p1_y,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_y,LOW);
        }else if(contType == 1){
          digitalWrite(p1_y,HIGH);
        }
      }else if(j == 2){
        pinMode(p1_select,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_select,LOW);
        }else if(contType == 1){
          digitalWrite(p1_select,HIGH);
        }
      }else if(j == 3){
        pinMode(p1_start,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_start,LOW);
        }else if(contType == 1){
          digitalWrite(p1_start,HIGH);
        }
      }else if(j == 4){
        pinMode(p1_up,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_up,LOW);
    Serial.println("up");
        }else if(contType == 1){
          digitalWrite(p1_up,HIGH);
        }
      }else if(j == 5){
        pinMode(p1_down,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_down,LOW);
    Serial.println("down");
        }else if(contType == 1){
          digitalWrite(p1_down,HIGH);
        }
      }else if(j == 6){
        pinMode(p1_left,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_left,LOW);
    Serial.println("left");
        }else if(contType == 1){
          digitalWrite(p1_left,HIGH);
        }
      }else if(j == 7){
        pinMode(p1_right,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_right,LOW);
    Serial.println("right");
        }else if(contType == 1){
          digitalWrite(p1_right,HIGH);
        }
      }else if(j == 8){
        pinMode(p1_a,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_a,LOW);
        }else if(contType == 1){
          digitalWrite(p1_a,HIGH);
        }
      }else if(j == 9){
        pinMode(p1_x,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_x,LOW);
        }else if(contType == 1){
          digitalWrite(p1_x,HIGH);
        }
      }else if(j == 10){
        pinMode(p1_l,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_l,LOW);
        }else if(contType == 1){
          digitalWrite(p1_l,HIGH);
        }
      }else if(j == 11){
        pinMode(p1_r,OUTPUT);
        if(contType == 0){
          digitalWrite(p1_r,LOW);
        }else if(contType == 1){
          digitalWrite(p1_r,HIGH);
        }
      }
    }else{
      //Whichever bits are not 1, switch those respective pin modes to INPUT so they won't output false data
      if(j == 0){
        pinMode(p1_b,INPUT);
      }else if(j == 1){
        pinMode(p1_y,INPUT);
      }else if(j == 2){
        pinMode(p1_select,INPUT);
      }else if(j == 3){
        pinMode(p1_start,INPUT);
      }else if(j == 4){
        pinMode(p1_up,INPUT);
      }else if(j == 5){
        pinMode(p1_down,INPUT);
      }else if(j == 6){
        pinMode(p1_left,INPUT);
      }else if(j == 7){
        pinMode(p1_right,INPUT);
      }else if(j == 8){
        pinMode(p1_a,INPUT);
      }else if(j == 9){
        pinMode(p1_x,INPUT);
      }else if(j == 10){
        pinMode(p1_l,INPUT);
      }else if(j == 11){
        pinMode(p1_r,INPUT);
      }
    }
    if(m & 1){
      if(j == 0){
        pinMode(p2_b,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_b,LOW);
    Serial.println("b");
        }else if(contType == 1){
          digitalWrite(p2_b,HIGH);
        }
      }else if(j == 1){
        pinMode(p2_y,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_y,LOW);
    Serial.println("y");
        }else if(contType == 1){
          digitalWrite(p2_y,HIGH);
        }
      }else if(j == 2){
        pinMode(p2_select,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_select,LOW);
    Serial.println("select");
        }else if(contType == 1){
          digitalWrite(p2_select,HIGH);
        }
      }else if(j == 3){
        pinMode(p2_start,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_start,LOW);
    Serial.println("start");
        }else if(contType == 1){
          digitalWrite(p2_start,HIGH);
        }
      }else if(j == 4){
        pinMode(p2_up,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_up,LOW);
    Serial.println("up");
        }else if(contType == 1){
          digitalWrite(p2_up,HIGH);
        }
      }else if(j == 5){
        pinMode(p2_down,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_down,LOW);
    Serial.println("down");
        }else if(contType == 1){
          digitalWrite(p2_down,HIGH);
        }
      }else if(j == 6){
        pinMode(p2_left,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_left,LOW);
    Serial.println("left");
        }else if(contType == 1){
          digitalWrite(p2_left,HIGH);
        }
      }else if(j == 7){
        pinMode(p2_right,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_right,LOW);
    Serial.println("right");
        }else if(contType == 1){
          digitalWrite(p2_right,HIGH);
        }
      }else if(j == 8){
        pinMode(p2_a,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_a,LOW);
    Serial.println("a");
        }else if(contType == 1){
          digitalWrite(p2_a,HIGH);
        }
      }else if(j == 9){
        pinMode(p2_x,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_x,LOW);
    Serial.println("x");
        }else if(contType == 1){
          digitalWrite(p2_x,HIGH);
        }
      }else if(j == 10){
        pinMode(p2_l,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_l,LOW);
    Serial.println("l");
        }else if(contType == 1){
          digitalWrite(p2_l,HIGH);
        }
      }else if(j == 11){
        pinMode(p2_r,OUTPUT);
        if(contType == 0){
          digitalWrite(p2_r,LOW);
    Serial.println("r");
        }else if(contType == 1){
          digitalWrite(p2_r,HIGH);
        }
      }
    }else{
      //Whichever bits are not 1, switch those respective pin modes to INPUT so they won't output false data
      if(j == 0){
        pinMode(p2_b,INPUT);
      }else if(j == 1){
        pinMode(p2_y,INPUT);
      }else if(j == 2){
        pinMode(p2_select,INPUT);
      }else if(j == 3){
        pinMode(p2_start,INPUT);
      }else if(j == 4){
        pinMode(p2_up,INPUT);
      }else if(j == 5){
        pinMode(p2_down,INPUT);
      }else if(j == 6){
        pinMode(p2_left,INPUT);
      }else if(j == 7){
        pinMode(p2_right,INPUT);
      }else if(j == 8){
        pinMode(p2_a,INPUT);
      }else if(j == 9){
        pinMode(p2_x,INPUT);
      }else if(j == 10){
        pinMode(p2_l,INPUT);
      }else if(j == 11){
        pinMode(p2_r,INPUT);
      }
    }
  }
  delay(16);
}