#include <SevSeg.h>
SevSeg sevseg;
extern volatile unsigned long timer0_millis;
byte D1 = 2;
byte D2 = 5;
byte D3 = 6;
byte D4 = 13;

byte A = 3;
byte B = 7;
byte C = 11;
byte D = 9;
byte E = 8;
byte F = 4;
byte G = 12;
byte DP = 10;

//segmenter2

SevSeg sevseg2;
byte D1A = 14;
byte D2A = 17;
byte D3A = 18;
byte D4A = 25;

byte AA = 15;
byte BA = 19;
byte CA = 23;
byte DA = 21;
byte EA = 20;
byte FA = 16;
byte GA = 24;
byte DPA = 22;

//segmenter3

SevSeg sevseg3;
byte D1B = 33;
byte D2B = 34;
//byte D3B = 41;
//byte D4B = 42;

byte AB = 31;
byte BB = 32;
byte CB = 36;
byte DB = 39;
byte EB = 38;
byte FB = 35;
byte GB = 40;
byte DPB = 37;
int numLaps = 0;

//int buttonPinReset = 44;
//int buttonStateReset = 0;
const int buttonPin = 44;
int buttonState;            // the current reading from the input pin
int lastButtonState = LOW;  // the previous reading from the input pin
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50;


const int buttonPin2 = 45;
int buttonState2;
int lastButtonState2 = LOW;
unsigned long lastDebounceTime2 = 0;

bool isbuttonPinResetPressed = false;

unsigned long time;
int milis;
int segs;
int segsDec;
int mins;

bool isStopped = false;
unsigned long savedMillis = 0;

void setup() {
  //pinMode(buttonPinReset, INPUT);
  pinMode(buttonPin, INPUT);

  byte numDigits = 4;
  byte numDigitsshort = 2;
  byte digitPins[] = {2, 5, 6, 13};
  byte segmentPins[] = {3, 4, 7, 8, 9, 10, 11, 12};
  bool resistorsOnSegments = 0;

  sevseg.begin(COMMON_CATHODE, numDigits, digitPins, segmentPins, resistorsOnSegments);
  sevseg.setBrightness(60);

  byte digitPinsA[] = {14, 17, 18, 25};
  byte segmentPinsA[] = {15, 16, 19, 20, 21, 22, 23, 24};

  sevseg2.begin(COMMON_CATHODE, numDigits, digitPinsA, segmentPinsA, resistorsOnSegments);
  sevseg2.setBrightness(60);

  byte digitPinsB[] = {33, 34};
  byte segmentPinsB[] = {31, 32, 35, 36, 37, 38, 39, 40};

  sevseg3.begin(COMMON_CATHODE, numDigitsshort, digitPinsB, segmentPinsB, resistorsOnSegments);
  sevseg3.setBrightness(60);





  Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly:
  time = millis();
  milis = time / 100 % 10;
  segs = time / 1000 % 10;
  segsDec = time / 10000 % 6;
  mins = time / 60000 % 10;

  int num = mins * 1000 + segsDec * 100 + segs * 10 + milis;
  int numStop = (savedMillis / 60000 % 10) * 1000 + (savedMillis / 10000 % 6) * 100 + (savedMillis / 1000 % 10) * 10 + (savedMillis / 100 % 10);
  //Serial.println(mins * 1000 + segsDec * 100 + segs * 10 + milis);

  char cstr[7];
  char cstr2[7];
  char cstrA[2];


  if (!isStopped) {
    //sprintf(cstr, "%04d", num);

    cstr[0] = '0' + mins;
    cstr[1] = '.';
    cstr[2] = '0' + segsDec;
    cstr[3] = '0' + segs;
    cstr[4] = '.';
    cstr[5] = '0' + milis;

    sevseg.setChars(cstr);
    sevseg.refreshDisplay();
  } else {
    //sprintf(cstr, "%04d", numStop);
    cstr[0] = '0' + numStop/1000%10;
    cstr[1] = '.';
    cstr[2] = '0' + numStop/100%10;
    cstr[3] = '0' + numStop/10%10;
    cstr[4] = '.';
    cstr[5] = '0' + numStop%10;
    sevseg.setChars(cstr);
    sevseg.refreshDisplay();
  }

  int reading = digitalRead(buttonPin);

  // If the switch changed, due to noise or pressing:
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    // whatever the reading is at, it's been there for longer than the debounce
    // delay, so take it as the actual current state:

    // if the button state has changed:
    if (reading != buttonState) {
      buttonState = reading;

      // only toggle the LED if the new button state is HIGH
      if (buttonState == HIGH) {
        if (!isStopped) {
          numLaps = numLaps + 1;
          memcpy(cstr2, cstr, sizeof(cstr2));
          Serial.println(cstr2[0]);
          Serial.println(cstr2[1]);
          Serial.println(cstr2[2]);
          Serial.println(cstr2[3]);
          setMillis(0);
        }
        else {
          numLaps = 0;
          memcpy(cstr2, "0000", 4);
          memcpy(cstr, "0000", 4);
          Serial.println("reset");
          setMillis(0);
          savedMillis = 0;
        }
      }
    }
  }

  lastButtonState = reading;

  ////BUTTON2

  int reading2 = digitalRead(buttonPin2);

  // If the switch changed, due to noise or pressing:
  if (reading2 != lastButtonState2) {
    // reset the debouncing timer
    lastDebounceTime2 = millis();
  }

  if ((millis() - lastDebounceTime2) > debounceDelay) {
    // whatever the reading is at, it's been there for longer than the debounce
    // delay, so take it as the actual current state:

    // if the button state has changed:
    if (reading2 != buttonState2) {
      buttonState2 = reading2;

      // only toggle the LED if the new button state is HIGH
      if (buttonState2 == HIGH) {
        if (!isStopped)
        {
          savedMillis = millis();
          numStop = (savedMillis / 60000 % 10) * 1000 + (savedMillis / 10000 % 6) * 100 + (savedMillis / 1000 % 10) * 10 + (savedMillis / 100 % 10);
          Serial.println(numStop);
        } else {
          setMillis(savedMillis);
          Serial.println("resume");
        }


        isStopped = !isStopped;
      }
    }
  }

  lastButtonState2 = reading2;

  //char fixer[7]={cstr2[0],'.',cstr2[1],cstr2[2],'.',cstr2[3]};
  //strncpy(cstr2, fixer, 7);

/*
  cstr2[0] = '0' + mins;
  cstr2[1] = '.';
  cstr2[2] = '0' + segsDec;
  cstr2[3] = '0' + segs;
  cstr2[4] = '.';
  cstr2[5] = '0' + milis;
*/
  sevseg2.setChars(cstr2); // set the characters/numbers to display.
  sevseg2.refreshDisplay();


  sprintf(cstrA, "%02d", numLaps);
  sevseg3.setChars(cstrA); // set the characters/numbers to display.
  sevseg3.refreshDisplay();
}


void setMillis(unsigned long new_millis) {
  uint8_t oldSREG = SREG;
  cli();
  timer0_millis = new_millis;
  SREG = oldSREG;
}