// https://forum.arduino.cc/t/using-sipo-shift-registers-to-reduce-pin-usage-on-an-arduino/1397478


const unsigned int16_t l_a1 = 11;
const unsigned int16_t l_a2 = 8;
const unsigned int16_t l_a3 = 9;
const unsigned int16_t l_g = 10;
const unsigned int16_t l_r = 14;
const unsigned int16_t r_a1 = 3;
const unsigned int16_t r_a2 = 0;
const unsigned int16_t r_a3 = 1;
const unsigned int16_t r_g = 2;
const unsigned int16_t r_r = 6;
const unsigned int16_t sw_l = 3;
const unsigned int16_t sw_r = 2;
const unsigned int16_t lst = 12;
const unsigned int16_t lps = 13;
const unsigned int16_t rst = 4;
const unsigned int16_t rps = 5;

unsigned int16_t reg_val = 0;
unsigned int16_t rndnum;
const int16_t sr_latch = 11;
const int16_t sr_clock = 10;
const int16_t sr_data = 12;

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display(15);

int16_t count = 0;
int16_t finish = 0;
int16_t loffset = 0;
int16_t roffset = 0;
int16_t lelap;
int16_t relap;
int16_t lfinish;
int16_t rfinish;
int16_t swl;
int16_t swr;
int16_t repeat;
int16_t lron = 0;
int16_t rron = 0;

uint32_t msec, msecstart, ltime = 0, rtime = 0, lgtime, rgtime;
String stageing = String("no");

int16_t l_staged = 0;
int16_t r_staged = 0;
enum : uint8_t {full, pro, greens} tree_type = full;
int16_t pro4 = 1;

void setup() {

  pinMode(sr_data, OUTPUT);
  pinMode(sr_clock, OUTPUT);
  pinMode(sr_latch, OUTPUT);

  Serial.begin(115200);

  pinMode(sw_l, INPUT_PULLUP);
  pinMode(sw_r, INPUT_PULLUP);
  pinMode(A0, OUTPUT); // ????? needed???

  writereg(65535);
  delay(3000);

  // get stageeing value
  writereg(0);
  bitWrite(reg_val, lps, HIGH);
  writereg(reg_val);
  msec = millis();
  msecstart = msec;
  dispint(count, 0x3d);
  dispint(count, 0x3c);
  while (msec - msecstart < 3000 ) { // 3 sec to stop stageing
    if ( digitalRead(sw_l) == 0 ) {
      count++;
      // Serial.println(count);
      dispint(count, 0x3c);
      dispint(count, 0x3d);
      delay(300);
      msecstart = msec;
    }
    else {
      msec = millis();
    }

  }

  if (count == 0) {
    stageing = "yes";
  }
  else {
    stageing = "no";
    bitWrite(reg_val, rps, HIGH);
  }

  writereg(reg_val);
  delay(2000); // could put random delay here
  count = 0;
  reg_val = 0;
  writereg(reg_val);

  // get tree type
  msec = millis();
  msecstart = msec;
  dispint(count, 0x3d);
  dispint(count, 0x3c);
  while (msec - msecstart < 3000 ) { // 3 sec to switch select tree
    rndnum = random(65535);
    writereg(rndnum);
    if ( digitalRead(sw_l) == 0 ) {
      count++;
      // Serial.println(count);
      dispint(count, 0x3c);
      dispint(count, 0x3d);
      delay(300);
      msecstart = msec;
    }
    else {
      msec = millis();
    }

  }

  if (count == 0) {
    // tree_type = "full";
    tree_type = full;
  }
  if (count == 1) {
    // tree_type = "pro";
    tree_type = pro;
    pro4 = 1;
  }
  if (count == 2) {
    // tree_type = "pro";
    tree_type = pro;
    pro4 = 0;
  }
  if (count > 2 ) {
    // tree_type = "greens";
    tree_type = greens;
  }
  writereg(0);
  delay(2000); // could put random delay here
}

void(* resetFunc) (void) = 0;//declare reset function at address 0

void loop() {
  lgtime = 30000;
  rgtime = 30000;
  lfinish = 0;
  rfinish = 0;

  dispint(0, 0x3c);
  dispint(0, 0x3d);
  if (stageing == "yes" ) {
    bitWrite(reg_val, lps, HIGH);
    bitWrite(reg_val, rps, HIGH);
    writereg(reg_val);
  }
  //stage players here
  if (stageing == "yes" ) {
    l_staged = 0;
    r_staged = 0;
    while ( (l_staged == 0 ) || ( r_staged == 0 ) ) {
      swl = digitalRead(sw_l);
      if (swl == 0) {
        l_staged = 1;
        bitWrite(reg_val, lst, HIGH);
        writereg(reg_val);
      }
      swr = digitalRead(sw_r);
      if (swr == 0) {
        r_staged = 1;
        bitWrite(reg_val, rst, HIGH);
        writereg(reg_val);
      }
    }
  }

  rndnum = random(3000);

  if (tree_type == greens) {
    lgtime = millis() + rndnum + 1000 ;
    rgtime = lgtime;
  }
  if (tree_type == full) {
    lgtime = millis() + rndnum + 1500 + loffset;
    rgtime = millis() + rndnum + 1500 + roffset;
  }
  if (tree_type == pro) {
    lgtime = millis() + rndnum + 400 + loffset;
    rgtime = millis() + rndnum + 400 + roffset;
    if (pro4 == 0) {
      lgtime += 100;
      rgtime += 100;
    }
  }

  delay(rndnum);
  msecstart = millis();
  // Serial.print("starting ");
  // Serial.println(count);
  Serial.println();
  Serial.println();
  Serial.println();

  while ( (lfinish == 0 ) || ( rfinish == 0 ) ) {
    ltime = millis() - msecstart - loffset;
    rtime = millis() - msecstart - roffset;
    swl = digitalRead(sw_l);

    if (swl == 0 && lfinish == 0 ) {
      lfinish = 1;
      lelap = millis() - lgtime;
      // dispint(lelap, 0x3d);
      bitWrite(reg_val, l_a1, LOW);
      bitWrite(reg_val, l_a2, LOW);
      bitWrite(reg_val, l_a3, LOW);

      if (lelap < 0) {
        bitWrite(reg_val, l_r, HIGH);
        bitWrite(reg_val, lst, LOW);
        bitWrite(reg_val, lps, LOW);
        writereg(reg_val);
        lron = 1;
        // Serial.print("Left redlighted ");
        // Serial.println(lelap);
      }
      else {
        // Serial.print("Left time is ");
        // Serial.println(lelap);
        bitWrite(reg_val, l_g, HIGH);
        bitWrite(reg_val, lst, LOW);
        bitWrite(reg_val, lps, LOW);
      }
    }
    swr = digitalRead(sw_r);

    if (swr == 0 && rfinish == 0 ) {
      rfinish = 1;
      relap = millis() - rgtime;
      // dispint(relap, 0x3c);
      bitWrite(reg_val, r_a1, LOW);
      bitWrite(reg_val, r_a2, LOW);
      bitWrite(reg_val, r_a3, LOW);

      if (relap < 0) {
        bitWrite(reg_val, r_r, HIGH);
        bitWrite(reg_val, rst, LOW);
        bitWrite(reg_val, rps, LOW);
        writereg(reg_val);
        rron = 1;
        // Serial.print("Right redlighted ");
        // Serial.println(relap);
      }
      else {
        // Serial.print("Right time is ");
        // Serial.println(relap);
        bitWrite(reg_val, r_g, HIGH);
        bitWrite(reg_val, rst, LOW);
        bitWrite(reg_val, rps, LOW);
      }
    }
    writereg(reg_val);

    if (tree_type == full) {
      if ( lfinish == 0 ) {
        if ( ltime > 1500 ) {
          if (lron == 0 ) {
            bitWrite(reg_val, l_g, HIGH);
          }
          if (lron == 0 ) {
            bitWrite(reg_val, l_a3, LOW);
          }
        }
        else {
          if (ltime > 1000) {
            if (lron == 0 && lfinish == 0) {
              bitWrite(reg_val, l_a3, HIGH);
            }
            if (lron == 0) {
              bitWrite(reg_val, l_a2, LOW);
            }
          }
          else {
            if (ltime > 500) {
              if (lron == 0) {
                bitWrite(reg_val, l_a2, HIGH);
              }
              if (lron == 0) {
                bitWrite(reg_val, l_a1, LOW);
              }
            }
            else {
              if (ltime >= 0 ) {
                if (lron == 0) {
                  bitWrite(reg_val, l_a1, HIGH);
                }
              }
            }
          }
        }
        writereg(reg_val);
      }
      if (rfinish == 0 ) {
        if ( rtime > 1500 ) {
          if (rron == 0 ) {
            bitWrite(reg_val, r_g, HIGH);
          }
          if (rron == 0) {
            bitWrite(reg_val, r_a3, LOW);
          }
        }
        else {
          if (rtime > 1000) {
            if (rron == 0 && rfinish == 0) {
              bitWrite(reg_val, r_a3, HIGH);
            }
            if (rron == 0) {
              bitWrite(reg_val, r_a2, LOW);
            }
          }
          else {
            if (rtime > 500) {
              if (rron == 0) {
                bitWrite(reg_val, r_a2, HIGH);
              }
              if (rron == 0) {
                bitWrite(reg_val, r_a1, LOW);
              }
            }
            else {
              if (rtime >= 0 ) {
                if (rron == 0) {
                  bitWrite(reg_val, r_a1, HIGH);
                }
              }
            }
          }
        }
        writereg(reg_val);
      }
    }

    if (tree_type == greens) {

      if ( millis() >= lgtime) {
        if (lron == 0) {
          bitWrite(reg_val, l_g, HIGH);
        }
        if (rron == 0) {
          bitWrite(reg_val, r_g, HIGH);
        }
        writereg(reg_val);
      }
    }

    if (tree_type == pro) {
      if ( lfinish == 0 ) {
        if (ltime + (pro4 * 100) > 500 ) {
          if (lron == 0) {
            bitWrite(reg_val, l_a1, LOW);
          }
          if (lron == 0) {
            bitWrite(reg_val, l_a2, LOW);
          }
          if (lron == 0) {
            bitWrite(reg_val, l_a3, LOW);
          }
          if (lron == 0 && lfinish == 0) {
            bitWrite(reg_val, l_g, HIGH);
          }
        }

        else {
          if (ltime > 0 ) {
            if (lron == 0) {
              bitWrite(reg_val, l_a1, HIGH);
            }
            if (lron == 0) {
              bitWrite(reg_val, l_a2, HIGH);
            }
            if (lron == 0) {
              bitWrite(reg_val, l_a3, HIGH);
            }
          }

        }
        writereg(reg_val);
      }
      if (rfinish == 0 ) {
        if (rtime + (pro4 * 100) > 500 ) {
          if (rron == 0) {
            bitWrite(reg_val, r_a1, LOW);
          }
          if (rron == 0) {
            bitWrite(reg_val, r_a2, LOW);
          }
          if (rron == 0) {
            bitWrite(reg_val, r_a3, LOW);
          }
          if (rron == 0 && rfinish == 0) {
            bitWrite(reg_val, r_g, HIGH);
          }
        }

        else {
          if (rtime > 0 ) {
            if (rron == 0) {
              bitWrite(reg_val, r_a1, HIGH);
            }
            if (rron == 0) {
              bitWrite(reg_val, r_a2, HIGH);
            }
            if (rron == 0) {
              bitWrite(reg_val, r_a3, HIGH);
            }
          }

        }
        writereg(reg_val);
      }
    }

  }
  bitWrite(reg_val, lps, LOW);
  bitWrite(reg_val, rps, LOW);
  writereg(reg_val);
  dispint(lelap, 0x3d);
  dispint(relap, 0x3c);
  // light up winning lane

  if (lelap > 0 ) {
    if (relap < 0 || lelap <= relap) {
      bitWrite(reg_val, lps, HIGH);
    }
  }

  if (relap > 0 ) {
    if (lelap < 0 || relap <= lelap) {
      bitWrite(reg_val, rps, HIGH);
    }
  }
  writereg(reg_val);

  if (lelap < 0 && relap < 0) {

    for (int16_t i = 0; i < 10; i++) {
      bitWrite(reg_val, lps, HIGH);
      bitWrite(reg_val, rps, HIGH);
      writereg(reg_val);
      delay(300);
      bitWrite(reg_val, lps, LOW);
      bitWrite(reg_val, rps, LOW);
      writereg(reg_val);
      delay(300);
    }
  }

  delay(2000);

  //Serial.println("1111111 ");
  repeat = 0;
  while (repeat == 0) {
    swl = digitalRead(sw_l);
    if (swl == 0) {
      repeat = 1;
    }
    swr = digitalRead(sw_r);
    if (swr == 0) {
      resetFunc();  //call reset
    }
  }
  //Serial.println("222222 ");
  repeat = 0 ;
  lfinish = 0;
  rfinish = 0;
  swl = 1;
  swr = 1;
  lron = 0;
  rron = 0;
  reg_val = 0;
  writereg(reg_val);

  delay(2000);
}
void dispint(int16_t dint, char i2caddr) {
  display.setTextSize(3);
  display.setTextColor(WHITE);
  display.begin(SSD1306_SWITCHCAPVCC, i2caddr);
  display.clearDisplay();
  display.drawRoundRect(0, 0, 127, 32, 8, WHITE);
  display.setCursor(20, 4);
  display.print(dint);
  display.display();
}

void writereg(unsigned int16_t val) {
  digitalWrite(sr_latch, LOW);
  for (int16_t i = 0; i < 16; i++) {
    boolean j = val % 2;
    val = val / 2;
    // Serial.print(i);
    // Serial.print(" ");
    // Serial.println(j);
    digitalWrite(sr_clock, LOW);
    digitalWrite(sr_data, j);
    digitalWrite(sr_clock, HIGH);
  }
  digitalWrite(sr_latch, LOW);
  digitalWrite(sr_latch, HIGH);
}