const int UpPin=47;
const int DownPin=53;
const int SelectPin=51;
const int BackPin=49;
const int ledRedPin=23;
const int ledGreenPin=25;
const int accelPinX=2;
const int accelPinY=1;
const int accelPinZ=0;
const int gyroPinZ=4;
const int gyroPinX=3;

int UpState=0;
int DownState=0;
int SelectState=0;
int BackState=0;
int lift=2;
int buttoncheck=0;
int setgoal=0; //Total number of sets to be performed
int setcount=0; //Current number of sets that have been performed
int repgoal=0; //Total number of reps to be performed, either for the specific set or every set
int repcount=0;// Current number of reps that have been performed for the specific set
int faultyrepcount=0;
int faultycheck=0;
int weight1=0;
int repcheck=0;
int weightcheck=0;
int spikecount=0;
char a=48;
char b=48;
char c=48;
char m=48;
char n=49;
char o=48;
char p=49;
int i=0;
int address1=0;


int gyroX = 0;
int gyroZ = 0;

int accelX = 0;
int accelY = 0;
int accelZ = 0;

int avgAccelXdummy = 0;
int avgAccelX = 0;
int avgAccelYdummy = 0;
int avgAccelY = 0;
int avgAccelZdummy = 0;
int avgAccelZ = 0;

int avgAccelXdeg = 0;
int avgAccelXdegdummy = 0;
int avgAccelYdeg = 0;
int avgAccelYdegdummy = 0;

int avgGyroXdummy = 0;
int avgGyroX = 0;
int avgGyroZdummy = 0;
int avgGyroZ = 0;

int avgAngleXdummy = 0;
int avgAngleX = 0;
int avgAngleYdummy = 0;
int avgAngleY = 0;

float acceleration;
float accelAngleXrad;
float accelAngleYrad;
float accelAngleZrad;
float accelAngleXdeg;
float accelAngleYdeg;
float accelAngleZdeg;

int gyroXrate;
int gyroZrate;
int prev_gyroXrate = 0;
int prev_gyroZrate = 0;
int prev_gyroXangle = 0;
int prev_gyroZangle = 0;

float accelerationX; 
float accelerationY;
float accelerationZ;

long gyroXangle = 0;
long gyroXangleInteg = 0;
long gyroZangle = 0;
long gyroZangleInteg = 0;

float accelXzeroVoltage = 1.532;   // Accelerometer X-axis reading for no movement     
float accelYzeroVoltage = 1.733;   // Accelerometer Y-axis reading for no movement
float accelZzeroVoltage = 1.65;   // Accelerometer Z-axis reading for no movement
float accelSensitivity = 0.8;     // in V/g -> (800mV/g)

float gyroXzeroVoltage = 1.2238;   // Gyro X-axis reading for 0deg/sec 
float gyroZzeroVoltage = 1.2238;    // Gyro Z-axis reading for 0deg/sec 
float gyroSensitivity = 0.0025;     // units -> V/deg/sec (2.5mV/deg/sec)
int rotationThreshold = 3;   // Minimum deg/sec to keep track of - helps with gyro drifting
float AnalogRef = 3.3175; // external reference

int address2;
int repcom;
int repdes;
int weight2;
int setnumber;
int totalsetnumber;
char buffer[5];

int startRead;


// LiquidCrystal lcd(rs, enable, d4, d5, d6, d7); 
#include <LiquidCrystal.h>
LiquidCrystal lcd(38, 36, 34, 32, 30, 28);
#include <EEPROM.h>

void setup()
{
  pinMode(ledRedPin, OUTPUT);
  pinMode(ledGreenPin, OUTPUT);
  pinMode(UpPin, INPUT);
  pinMode(DownPin, INPUT);
  pinMode(SelectPin, INPUT);
  pinMode(BackPin, INPUT);
  lcd.begin(16,2);
  digitalWrite(26, HIGH);
  Serial.begin(9600);
  
}


void loop()
{
  goto countdown;
 start: 

 
  
  lcd.clear();
  lcd.setCursor(3,0);
  lcd.print("Welcome to");
  lcd.setCursor(4,1);
  lcd.print("Team 31's");
  delay(2000);
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Electronic Rep");
  lcd.setCursor(4,1);
  lcd.print("Counter:");
  delay(2000);
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Fahim Kadhi,");
  lcd.setCursor(0,1);
  lcd.print("Andrew Mast, and");
  delay(2000);
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Ben Rosborough");
  delay(2000);
  
  
  
  
  
  
  choice1:
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Select option:");
  lcd.setCursor(0,1);
  lcd.print("Perform workout");
  choice1a:
  do
  {
    UpState=digitalRead(UpPin);
    DownState=digitalRead(DownPin);
    SelectState=digitalRead(SelectPin);
    BackState=digitalRead(BackPin);
    buttoncheck=UpState && DownState && SelectState && BackState;
  }
  while (buttoncheck==HIGH);
  delay(400);
  if (UpState==LOW)
  {
   goto choice3;
  }
  else if (DownState==LOW) 
  {
    goto choice2;
  }
  else if (SelectState==LOW)
  {
    goto squatdisplay1;
  } 
  else
  {
    goto choice1;
  } 
  
  choice2:
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Select option:");
  lcd.setCursor(0,1);
  lcd.print("Upload workout");
  choice2a:
  do
  {
    UpState=digitalRead(UpPin);
    DownState=digitalRead(DownPin);
    SelectState=digitalRead(SelectPin);
    BackState=digitalRead(BackPin);
    buttoncheck=UpState && DownState && SelectState && BackState;
  }
  while (buttoncheck==HIGH);
  delay(400);
  if (UpState==LOW)
  {
   goto choice1;
  }
  else if (DownState==LOW) 
  {
    goto choice3;
  } 
  else if (SelectState==LOW)
  {
    goto memread;
  } 
  else
  {
    goto choice2a;
  }
  
  
  choice3:
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Select option:");
  lcd.setCursor(1,1);
  lcd.print("Clear memory");
  choice3a:
  do
  {
    UpState=digitalRead(UpPin);
    DownState=digitalRead(DownPin);
    SelectState=digitalRead(SelectPin);
    BackState=digitalRead(BackPin);
    buttoncheck=UpState && DownState && SelectState && BackState;
  }
  while (buttoncheck==HIGH);
  delay(400);
  if (UpState==LOW)
  {
   goto choice2;
  }
  else if (DownState==LOW) 
  {
    goto choice1;
  }
  else if (SelectState==LOW)
  {
    goto memclear;
  }  
  else
  {
    goto choice3a;
  }
  
  
  
  
  
  
  
  /////////////////////////////////////////SQUAT/////////DISPLAY///////////////////////////////////////////////////////////////////////////////
  squatdisplay1:
  setgoal=0; //Total number of sets to be performed
  setcount=0; //Current number of sets that have been performed
  repgoal=0; //Total number of reps to be performed, either for the specific set or every set
  repcount=0;// Current number of reps that have been performed for the specific set
  faultyrepcount=0;
  weight1=0;
  repcheck=0;
  weightcheck=0;
  a=48;
  b=48;
  c=48;
  m=48;
  n=49;
  o=48;
  p=49;
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select lift:");
  lcd.setCursor(6,1) ;
  lcd.print ("Sqaut");
  squatdisplay2:
   do
  {
   UpState=digitalRead(UpPin);
   DownState=digitalRead(DownPin);
   SelectState=digitalRead(SelectPin);
   BackState=digitalRead(BackPin); 
   buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300); 
  if (UpState==LOW)
  {
    goto tricepsextensiondisplay1;
  } 
  else if (DownState==LOW)
  {
    goto benchpressdisplay1;
 
  }
  else if (SelectState==LOW)
  {
    lift=1;
    goto setselect;
    
  }
  else if (BackState==LOW)
  {
    goto choice1;
  } 
  else 
  {
    goto squatdisplay2;
  } 
  
/////////////////////////////////////////BENCH////PRESS/////DISPLAY///////////////////////////////////////////////////////////////////////////////  
  
  benchpressdisplay1:
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select lift:");
  lcd.setCursor(2,1) ;
  lcd.print ("Bench press"); 
  benchpressdisplay2:
  do
  {
   UpState=digitalRead(UpPin);
   DownState=digitalRead(DownPin);
   SelectState=digitalRead(SelectPin);
   BackState=digitalRead(BackPin); 
    buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)
  {
    goto squatdisplay1;
  }
  else if (DownState==LOW)
  {
   goto deadliftdisplay1;
  }
  else if (SelectState==LOW)
  {
   lift=2;
    goto setselect;
  }
  else
  {
    goto benchpressdisplay2;
  } 
  
  
 /////////////////////////////////////////DEAD////LIFT/////DISPLAY/////////////////////////////////////////////////////////////////////////////// 
  deadliftdisplay1:
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select lift:");
  lcd.setCursor(4,1) ;
  lcd.print ("Deadlift"); 
  deadliftdisplay2:
  do
  {
   UpState=digitalRead(UpPin);
   DownState=digitalRead(DownPin);
   SelectState=digitalRead(SelectPin);
   BackState=digitalRead(BackPin); 
    buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)
  {
    goto benchpressdisplay1;
  }
  else if (DownState==LOW)
  {
    goto bicepscurldisplay1;
  }
  else if (SelectState==LOW)
  {
    lift=3;
    goto setselect;
  }
  else
  {
    goto deadliftdisplay2;
  } 
  
 
 /////////////////////////////////////////BICEP///CURL//////DISPLAY/////////////////////////////////////////////////////////////////////////////// 
  bicepscurldisplay1:
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select lift:");
  lcd.setCursor(3,1) ;
  lcd.print ("Biceps curl");
  bicepscurldisplay2:
   do
  {
   UpState=digitalRead(UpPin);
   DownState=digitalRead(DownPin);
   SelectState=digitalRead(SelectPin);
   BackState=digitalRead(BackPin); 
    buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)
  {
    UpState=0;
    goto deadliftdisplay1;
  } 
  else if (DownState==LOW)
  {
    UpState=0;
    goto tricepsextensiondisplay1;
 
  }
  else if (SelectState==LOW)
  {
    SelectState=0;
    lift=4;
    goto setselect;
    
  }
  else 
  {
    goto bicepscurldisplay2;
  } 
  
 
 
/////////////////////////////////////////TRICEP////EXTENSION/////DISPLAY/////////////////////////////////////////////////////////////////////////////// 
  tricepsextensiondisplay1:
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select lift:");
  lcd.setCursor(2,1) ;
  lcd.print ("Triceps Ext.");
  tricepsextensiondisplay2:
   do
  {
   UpState=digitalRead(UpPin);
   DownState=digitalRead(DownPin);
   SelectState=digitalRead(SelectPin);
   BackState=digitalRead(BackPin); 
   buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)
  {
    goto bicepscurldisplay1;
  } 
  else if (DownState==LOW)
  {
    goto squatdisplay1;
 
  }
  else if (SelectState==LOW)
  {
    lift=5;
    goto setselect;
   }
  else 
  {
    goto tricepsextensiondisplay2;
  } 
  
  
  
  
 /////////////////////////////////////////SET/////////SELECT/////////////////////////////////////////////////////////////////////////////// 
  setselect: 
  a=48;
  b=49;
  c=48;
  m=48;
  n=49;
  o=48;
  p=49;
  setgoal=1;
  Serial.println(lift);
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Select number");
  lcd.setCursor(5,1);
  lcd.print("of sets");
  delay(2000);
  setfirstdigit:
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Maximum is 10");
  lcd.setCursor(5,1);
  lcd.print("sets=");
  lcd.write(a);
  lcd.write(b);
  Serial.println(setgoal);
  
  do
  {
    UpState=digitalRead(UpPin);
    DownState=digitalRead(DownPin);
    SelectState=digitalRead(SelectPin);
    BackState=digitalRead(BackPin); 
     buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)  
  {
      if (a==48 && b==57)
          {
              a=49;
              b=48;
              setgoal=10;
              goto setfirstdigit;
          }
      else if (a==49 && b==48)
          {
              a=48;
              b=49;
              setgoal=1;
              goto setfirstdigit;
          }
       else 
          {
              b++;
              setgoal++;
              goto setfirstdigit;
          }              
   }
  
  else if (DownState==LOW)
  {
    if (a==48 && b==49)
    {
        a=49;
        b=48;
        setgoal=10;
        goto setfirstdigit;
    }
    else if (a==49 && b==48)
    {
        a=48;
        b=57;
        setgoal=9;
        goto setfirstdigit;
    }
    else
    {
        b--;
        setgoal--;
        goto setfirstdigit;
    }   
  }
  else if (SelectState==LOW)
  { 
      lcd.clear();
      lcd.setCursor(4,0);
      lcd.print("sets=");
      lcd.write(a);
      lcd.write(b);
      Serial.println(setgoal);
      delay(2000);
      goto repselect1;
      
   }
  else if (BackState==LOW)
  {
    goto squatdisplay1;
  } 
  else 
  {
    goto setfirstdigit;
  } 
 
 
 
 
 
 
 
 
 



/////////////////////////////////////////REP/////////SELECT///////////////////////////////////////////////////////////////////////////////
 repselect1:
 a=48;
 b=48;
 c=48;
 m=48;
 n=49;
 o=48;
 p=49;
 repgoal=0;
 
 if (setgoal==1)
 {
   goto repselect2;
 } 
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Are reps equal");
  lcd.setCursor(2,1);
  lcd.print("for all sets?");
  delay(2000);
  temp1:
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Press up for yes");
  lcd.setCursor(3,1);
  lcd.print("Down for no");
  do
  {
   UpState=digitalRead(UpPin);
   DownState=digitalRead(DownPin); 
  } 
  while (UpState==HIGH && DownState==HIGH);
  delay(300);
  if (UpState==LOW)
  {
    repcheck=0;
    lcd.clear();
    lcd.setCursor(7,0);
    lcd.print("Yes");
    delay(2000);
    goto repselect2;
  } 
  else if (DownState==LOW)
  {
    repcheck=1;
    lcd.clear();
    lcd.setCursor(8,0);
    lcd.print("No");
    delay(2000);
    goto repselect3;
  }
     
  else
  {
    goto temp1; 
  } 

  repselect2:
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Select number");
  lcd.setCursor(5,1);
  lcd.print("of reps");
  delay(2000);
  goto repfirstdigit; 
  
  repselect3:
  repgoal=0;
  a=48;
  b=48;
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Select number of");
  lcd.setCursor(0,1);
  lcd.print("reps for set ");
  lcd.write(m);
  lcd.write(n);
  if (n==57)
  {
   n=48;
   m++;
  }
  else if(m==57 && n==57)
  {
    m=48;
    n=49;
  }  
  else
  {
    n++;
  } 
  delay(2000);
  goto repfirstdigit;
 
 
 
 
 
  repfirstdigit:
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select first");
  lcd.setCursor(1,1);
  lcd.print("digit: reps=");
  lcd.write(a);
  lcd.write(b);
  Serial.println(repgoal);
  do
  {
    UpState=digitalRead(UpPin);
    DownState=digitalRead(DownPin);
    SelectState=digitalRead(SelectPin);
    BackState=digitalRead(BackPin); 
    buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)  
  {
    if (b!=57)
    {
     b++;
     repgoal++;
     goto repfirstdigit;
    }
    else
    {
     b=48;
     repgoal=0;
     goto repfirstdigit;
    }      
  }
  
  else if (DownState==LOW)
  {
    if (b!=48)
    {
     b--;
     repgoal--;
     goto repfirstdigit;
    }
    else
    {
     b=57;
     repgoal=9;
     goto repfirstdigit;
    }
      
  }
  
  else if (SelectState==LOW)
  { 
    goto repseconddigit;
  }
  else if (BackState==LOW)
  {
    if (setcount==0)
    {
     goto setselect;
    }
    else if (setcount > 0)
    {
      goto squatdisplay1;
    } 
  } 
  else 
  {
    goto repfirstdigit;
  } 
 
  repseconddigit:
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select second");
  lcd.setCursor(1,1);
  lcd.print("digit: reps=");
  lcd.write(a);
  lcd.write(b);
  Serial.println(repgoal);
  do
  {
    UpState=digitalRead(UpPin);
    DownState=digitalRead(DownPin);
    SelectState=digitalRead(SelectPin);
    BackState=digitalRead(BackPin); 
    buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)  
  {
    if (a!=57)
    {
    a++;
    repgoal=repgoal+10;
    goto repseconddigit;
    }
    else
    {
     a=48;
     repgoal=repgoal-90;
     goto repseconddigit;
    }
      
  }
  
  else if (DownState==LOW)
  {
    if (a!=48)
    {
    a--;
    repgoal=repgoal-10;
    goto repseconddigit;
    }
    else
    {
     a=57;
     repgoal=repgoal+90;
     goto repseconddigit;
    }
      
  }
  
  else if (SelectState==LOW)
  { 
    lcd.clear();
    lcd.setCursor(4,0);
    lcd.print("reps=");
    lcd.write(a);
    lcd.write(b);
    delay(2000);
    
    if (setcount==0)
    {
      if (repcheck==0)
      {
                                            switch (lift)
                                            {
                                                  case 1:
                                                  for(address1=11; 10 < address1 < (11+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, repgoal);
                                                  }   
                                                  break;   
                                                  case 2:
                                                  for(address1=111; address1 < (111+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, repgoal);   
                                                  }
                                                  break;
                                                  case 3:
                                                  for(address1=211; address1 < (211+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, repgoal);   
                                                  }
                                                  break;
                                                  case 4:
                                                  for(address1=311; address1 < (311+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, repgoal);  
                                                  }
                                                  break;
                                                  case 5:
                                                  for(address1=411; address1 < (411+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, repgoal);   
                                                  }
                                                  break;
                                             }
                                             
              }
              else if (repcheck==1)
              {
                switch (lift)
                {
                  case 1:
                  EEPROM.write(11, repgoal);
                  break;
                  case 2:
                  EEPROM.write(111, repgoal);
                  break;
                  case 3:
                  EEPROM.write(211, repgoal);
                  break;
                  case 4:
                  EEPROM.write(311, repgoal);
                  break;
                  case 5:
                  EEPROM.write(411, repgoal);
                  break;
                }
              }
    
      goto weightselect1;
    } 
    
    else if (setcount > 0)
    {
      
        switch (lift)
        {
            case 1:
            EEPROM.write(11+setcount, repgoal);
            break;
            case 2:
            EEPROM.write(111+setcount, repgoal);
            break;
            case 3:
            EEPROM.write(211+setcount, repgoal);
            break;
            case 4:
            EEPROM.write(311+setcount, repgoal);
            break;
            case 5:
            EEPROM.write(411+setcount, repgoal); 
            break;     
        }
      
         if (weightcheck==0)
         {
           goto countdown;
         }
         else if (weightcheck==1)
         {
           goto weightselect3;
         } 
     
    }
    
    
    
    
     
  }
  else if (BackState==LOW)
  {
    if (setcount==0)
    {
     goto setselect;
    }
    else if (setcount > 0)
    {
      goto squatdisplay1;
    }
  } 
  else 
  {
    goto repseconddigit;
  }
  
///////////////////////////////////WEIGHT////////SELECT//////////////////////////////////////////////////////
  
 weightselect1:
 a=48;
 b=48;
 c=48;
 weight1=0;
 if (setgoal==1)
 {
   goto weightselect2; //select weight
 }

  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Is weight equal");
  lcd.setCursor(2,1);
  lcd.print("for all sets?");
  delay(2000);
  temp2:
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Press up for yes");
  lcd.setCursor(3,1);
  lcd.print("Down for no");
  do
  {
   UpState=digitalRead(UpPin);
   DownState=digitalRead(DownPin); 
  } 
  while (UpState==HIGH && DownState==HIGH);
  delay(300);
  if (UpState==LOW)
  {
    weightcheck=0;
    lcd.clear();
    lcd.setCursor(7,0);
    lcd.print("Yes");
    delay(2000);
    goto weightselect2;
  } 
  else if (DownState==LOW)
  {
    weightcheck=1;
    lcd.clear();
    lcd.setCursor(8,0);
    lcd.print("No");
    delay(2000);
    goto weightselect3;
  }
     
  else
  {
    goto temp2; 
  } 

 
  weightselect2:
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Select amount");
  lcd.setCursor(4,1);
  lcd.print("of weight");
  delay(2000);
  goto weightfirstdigit; 
 
 
  weightselect3:
  a=48;
  b=48;
  c=48;
  weight1=0;
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select weight");
  lcd.setCursor(3,1);
  lcd.print("for set ");
  lcd.write(o);
  lcd.write(p);
  if (p==57)
  {
   p=48;
   o++;
  }
  else if(o==57 && p==57)
  {
    o=48;
    p=48;
  }  
  else
  {
    p++;
  } 
  delay(2000);
  goto weightfirstdigit;
 
  weightfirstdigit:
  
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select first");
  lcd.setCursor(3,1);
  lcd.print("digit: ");
  lcd.write(a);
  lcd.write(b);
  lcd.write(c);
  Serial.println(weight1);
  do
  {
    UpState=digitalRead(UpPin);
    DownState=digitalRead(DownPin);
    SelectState=digitalRead(SelectPin);
    BackState=digitalRead(BackPin); 
    buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)  
  {
   if (a!=57)
   {
    a++;
    weight1=weight1+100;
    goto weightfirstdigit;
   }
   else
   {
     a=48;
     weight1=0;
     goto weightfirstdigit;
   }
  } 
  else if (DownState==LOW)
  {
   if (a!=48)
   {
    a--;
    weight1=weight1-100;
    goto weightfirstdigit; 
   } 
   else 
   {
     a=57;
     weight1=900;
     goto weightfirstdigit;
   }
  }
  else if (SelectState==LOW)
  {
    goto weightseconddigit;
  }
  else if (BackState==LOW)
  {
   if (setcount==0)
    {
     goto repselect1;
    }
    else if (setcount > 0)
    {
      goto squatdisplay1;
    }
  }
 
  weightseconddigit:
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select second");
  lcd.setCursor(3,1);
  lcd.print("digit: ");
  lcd.write(a);
  lcd.write(b);
  lcd.write(c);
  Serial.println(weight1);
  do
  {
    UpState=digitalRead(UpPin);
    DownState=digitalRead(DownPin);
    SelectState=digitalRead(SelectPin);
    BackState=digitalRead(BackPin); 
    buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)  
  {
    if (b!=57)
    {
    b++;
    weight1=weight1+10;
    goto weightseconddigit;
    }
    else
    {
     b=48;
     weight1=weight1-90;
     goto weightseconddigit;
    }
      
  }
  
  else if (DownState==LOW)
  {
    if (b!=48)
    {
    b--;
    weight1=weight1-10;
    goto weightseconddigit;
    }
    else
    {
     b=57;
     weight1=weight1+90;
     goto weightseconddigit;
    }   
  }
  
  else if (SelectState==LOW)
  { 
    goto weightthirddigit;
  }
  else if (BackState==LOW)
  {
    if (setcount==0)
    {
     goto repselect1;
    }
    else if (setcount > 0)
    {
      goto squatdisplay1;
    }
  } 
  else 
  {
    goto weightseconddigit;
  }
   
 
  weightthirddigit:
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Select third");
  lcd.setCursor(3,1);
  lcd.print("digit: ");
  lcd.write(a);
  lcd.write(b);
  lcd.write(c);
  Serial.println(weight1);
  do
  {
    UpState=digitalRead(UpPin);
    DownState=digitalRead(DownPin);
    SelectState=digitalRead(SelectPin);
    BackState=digitalRead(BackPin); 
    buttoncheck=UpState && DownState && SelectState && BackState; 
  } 
  while (buttoncheck==HIGH);
  delay(300);
  if (UpState==LOW)  
  {
    if (c!=57)
    {
    c++;
    weight1++;
    goto weightthirddigit;
    }
    else
    {
     c=48;
     weight1=weight1-9;
     goto weightthirddigit;
    }
      
  }
  
  else if (DownState==LOW)
  {
    if (c!=48)
    {
    c--;
    weight1--;
    goto weightthirddigit;
    }
    else
    {
     c=57;
     weight1=weight1+9;
     goto weightthirddigit;
    }
      
  }
  
  else if (SelectState==LOW)
  { 
    lcd.clear();
    lcd.setCursor(1,0);
    lcd.print("weight=");
    lcd.write(a);
    lcd.write(b);
    lcd.write(c);
    lcd.print(" lbs");
    delay(2000);
    
    if (setcount==0)
    {
      if (weightcheck==0)
      {
                                            switch (lift)
                                            {
                                                  case 1:
                                                  for(address1=21; address1 < (21+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, weight1);
                                                  }   
                                                  break;   
                                                  case 2:
                                                  for(address1=121; address1 < (121+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, weight1);
                                                  }
                                                  break;
                                                  case 3:
                                                  for(address1=221; address1 < (221+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, weight1);
                                                  }
                                                  break;
                                                  case 4:
                                                  for(address1=321; address1 < (321+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, weight1);    
                                                  }
                                                  break;
                                                  case 5:
                                                  for(address1=421; address1 < (421+setgoal); address1++)
                                                  {
                                                      EEPROM.write(address1, weight1);    
                                                  }
                                                  break;
                                            }
              }
              else if (weightcheck==1)
              {
                switch (lift)
                {
                  case 1:
                  EEPROM.write(21, weight1);
                  break;
                  case 2:
                  EEPROM.write(121, weight1);
                  break;
                  case 3:
                  EEPROM.write(221, weight1);
                  break;
                  case 4:
                  EEPROM.write(321, weight1);
                  break;
                  case 5:
                  EEPROM.write(421, weight1);
                  break;
              }
              }
    } 
    
    else if (setcount > 0)
    {
      
        switch (lift)
        {
            case 1:
            EEPROM.write(21+setcount, weight1);
            break;
            case 2:
            EEPROM.write(121+setcount, weight1);
            break;
            case 3:
            EEPROM.write(221+setcount, weight1);
            break;
            case 4:
            EEPROM.write(321+setcount, weight1);
            break;
            case 5:
            EEPROM.write(421+setcount, weight1);
            break;      
        }
       
     
    }
    
    
   goto countdown; 
    
     
  }
  else if (BackState==LOW)
  {
    if (setcount==0)
    {
     goto repselect1;
    }
    else if (setcount > 0)
    {
      goto squatdisplay1;
    }
  } 
  else 
  {
    goto weightthirddigit;
  } 
  
  
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////  
  
 countdown:
 lcd.clear();
 lcd.setCursor(4,0);
 lcd.print("Press up");
 lcd.setCursor(4,1);
 lcd.print("to begin");
 do
 {
   UpState=digitalRead(UpPin);
 }
 while (UpState==HIGH);
 lcd.clear();
 lcd.setCursor(8,0);
 lcd.print("5");
 delay(1000); 
 lcd.clear();
 lcd.setCursor(8,0);
 lcd.print("4");
 delay(1000);
 lcd.clear();
 lcd.setCursor(8,0);
 lcd.print("3");
 delay(1000);
 lcd.clear();
 lcd.setCursor(8,0);
 lcd.print("2");
 delay(1000);
 lcd.clear();
 lcd.setCursor(8,0);
 lcd.print("1");
 delay(1000);
 lcd.clear();
 lcd.setCursor(7,0);
 lcd.print("Go!");
 digitalWrite(ledRedPin, HIGH);
 digitalWrite(ledGreenPin, HIGH);
 delay(1000);
 digitalWrite(ledRedPin, LOW);
 digitalWrite(ledGreenPin, LOW);
 delay(500);
 
 switch (lift)
 {
   case 1:
    goto getdatasquat;
   case 2:
    goto getdatabenchpress;
   case 3:
    goto getdatadeadlift;
   case 4:
    goto getdatabiceps;
   case 5:
    goto getdatatriceps;
 }  
    
 
 
 
 
 
 
 
 
 
 
 
/////////////////////////////////////////SQUAT SUBROUTINE//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
 getdatasquat:
  do 
  {
    BackState=digitalRead(BackPin);
    //////////////////////////////////////////X-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelX = analogRead(accelPinX); // X-AXIS - ACCELEROMETER
    
    avgAccelXdummy = 0;
    for (int ia=0; ia<20; ia++) { avgAccelXdummy += accelX; }
    avgAccelX = avgAccelXdummy / 15;
    
    accelerationX =  ( ( (avgAccelX*AnalogRef) - (1023*accelXzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Y-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelY = analogRead(accelPinY); // Y-AXIS - ACCELEROMETER
    
    avgAccelYdummy = 0;
    for (int ib=0; ib<15; ib++) { avgAccelYdummy += accelY; }
    avgAccelY = avgAccelYdummy / 15;
    
    accelerationY =  ( ( (avgAccelY*AnalogRef) - (1023*accelYzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Z-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelZ = analogRead(accelPinZ); // Z-AXIS - ACCELEROMETER
    
    avgAccelZdummy = 0;
    for (int ic=0; ic<15; ic++) { avgAccelZdummy += accelZ; }
    avgAccelZ = avgAccelZdummy / 15;
    
    accelerationZ =  -( ( (avgAccelZ*AnalogRef) - (1023*accelZzeroVoltage)) / 1023) / accelSensitivity;




 //////////////////////////////////////////ACCELERATION MAGNITUDE/////////////////////////////////////////////////////////////////////////////////
    acceleration = sqrt((accelerationX*accelerationX) + (accelerationY*accelerationY) + (accelerationZ*accelerationZ));
    
//////////////////////////////////////////X-ANGLE/////////////////////////////////////////////////////////////////////////////////
      
      accelAngleXrad = asin(accelerationZ/acceleration); 
      accelAngleXdeg = accelAngleXrad * 57.29578; // 180/pi - to convert radians to degrees
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationX < 0) {accelAngleXdeg = 180 - accelAngleXdeg;}
    
      accelAngleXdeg -=85; // to offset the angle to make it align with desired scale
      if (gyroXrate > 27 || gyroXrate < -27)
      {
       accelAngleXdeg=0;
      } 
      // averaging out the angle for X-axis for the accelerometer
      avgAccelXdegdummy = 0;
      for (int j=0; j<15; j++) { avgAccelXdegdummy += accelAngleXdeg; }
      avgAccelXdeg = avgAccelXdegdummy / 15;
      
      if (avgAccelXdeg < 0)
          avgAccelXdeg += 360;
      else if (avgAccelXdeg > 359)
          avgAccelXdeg -= 360;
 

//////////////////////////////////////////Y-ANGLE/////////////////////////////////////////////////////////////////////////////////
      accelAngleYrad = asin(accelerationZ/acceleration); 
      accelAngleYdeg = (accelAngleYrad * 57.29578); // 180/pi
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationY < 0) {accelAngleYdeg = 180 - accelAngleYdeg;}
    
      accelAngleYdeg -= 90; // to offset the angle to make it align with desired scale
      
      avgAccelYdegdummy = 0;
      for (int k=0; k<15; k++) { avgAccelYdegdummy += accelAngleYdeg; }
      avgAccelYdeg = avgAccelYdegdummy / 15;
      
      if (avgAccelYdeg < 0)
          {avgAccelYdeg += 360;}
      else if (avgAccelYdeg > 359)
          avgAccelYdeg -= 360;    
//////////////////////////////////////////Z-ANGLE/////////////////////////////////////////////////////////////////////////////////    
     accelAngleZrad = atan(accelerationY/accelerationX);
      accelAngleZdeg = accelAngleZrad * 57.29578; // 180/pi 
      
//////////////////////////////////////////GYRO RATE/////////////////////////////////////////////////////////////////////////////////         

 analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroZ = analogRead(gyroPinZ); // Z-AXIS - GYROSCOPE
    
    avgGyroZdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroZdummy += gyroZ; }
    avgGyroZ = avgGyroZdummy / 15;
    
    gyroZrate =  ( ( (avgGyroZ*AnalogRef) - (1023*gyroZzeroVoltage)) / 1023) / gyroSensitivity;
    
    if ( ((avgAccelX >= 473)&&(avgAccelX <= 488)) && ((avgAccelY >= 535)&&(avgAccelY <= 555)) && ((avgAccelZ >= 230)&&(avgAccelZ <= 258)) )
    {
     gyroZrate = 0;
    } 
    
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroX = analogRead(gyroPinX); //X-AXIS - GYROSCOPE
    
    avgGyroXdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroXdummy += gyroX; }
    avgGyroX = avgGyroXdummy / 15;
    
    gyroXrate =  ( ( (avgGyroX*AnalogRef) - (1023*gyroXzeroVoltage)) / 1023) / gyroSensitivity;
    
//    if ( ((avgAccelX >= 473)&&(avgAccelX <= 487)) && ((avgAccelY >= 535)&&(avgAccelY <= 555)) && ((avgAccelZ >= 230)&&(avgAccelZ <= 258)) )
//    {
//     gyroXrate = 0;
//    } 
    
  }    
  
  while (acceleration < 1.74 && BackState==HIGH); //1.74
   delay(1000);
 if (BackState==LOW)
 {
   goto cutsetshort;
 } 
 
 else if (avgAccelYdeg < 290 || gyroXrate > 50 || gyroZrate > 50 || gyroXrate < -50 || gyroZrate < -50)
 {
   faultyrepcount++;
   digitalWrite(ledRedPin, HIGH);
   delay(500);
   digitalWrite(ledRedPin, LOW);
 }
 else
 { 
  repcount++;
  goto repcountcheck;
 } 
// if (accelerationY > 1.05)
// { 
//   faultyrepcount++;
//   Serial.println("faultyrepcount=");
//   Serial.println(faultyrepcount);
//   digitalWrite(ledRedPin, HIGH);
//   delay(500);
//   digitalWrite(ledRedPin, LOW);
//   delay(250);
// } 

 
 
 
 
 //Threshold acceleration is 1.74.
 
 
 
 
 
 
 
 
 
 
 
 
/////////////////////////////////////////BENCH PRESS SUBROUTINE//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
 getdatabenchpress:
  do 
  {
    BackState=digitalRead(BackPin);
    //////////////////////////////////////////X-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelX = analogRead(accelPinX); // X-AXIS - ACCELEROMETER
    
    avgAccelXdummy = 0;
    for (int ia=0; ia<20; ia++) { avgAccelXdummy += accelX; }
    avgAccelX = avgAccelXdummy / 15;
    
    accelerationX =  ( ( (avgAccelX*AnalogRef) - (1023*accelXzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Y-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelY = analogRead(accelPinY); // Y-AXIS - ACCELEROMETER
    
    avgAccelYdummy = 0;
    for (int ib=0; ib<15; ib++) { avgAccelYdummy += accelY; }
    avgAccelY = avgAccelYdummy / 15;
    
    accelerationY =  ( ( (avgAccelY*AnalogRef) - (1023*accelYzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Z-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelZ = analogRead(accelPinZ); // Z-AXIS - ACCELEROMETER
    
    avgAccelZdummy = 0;
    for (int ic=0; ic<15; ic++) { avgAccelZdummy += accelZ; }
    avgAccelZ = avgAccelZdummy / 15;
    
    accelerationZ =  -( ( (avgAccelZ*AnalogRef) - (1023*accelZzeroVoltage)) / 1023) / accelSensitivity;




 //////////////////////////////////////////ACCELERATION MAGNITUDE/////////////////////////////////////////////////////////////////////////////////
    acceleration = sqrt((accelerationX*accelerationX) + (accelerationY*accelerationY) + (accelerationZ*accelerationZ));
    
//////////////////////////////////////////X-ANGLE/////////////////////////////////////////////////////////////////////////////////
      
      accelAngleXrad = asin(accelerationZ/acceleration); 
      accelAngleXdeg = accelAngleXrad * 57.29578; // 180/pi - to convert radians to degrees
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationX < 0) {accelAngleXdeg = 180 - accelAngleXdeg;}
    
      accelAngleXdeg -=85; // to offset the angle to make it align with desired scale
      if (gyroXrate > 27 || gyroXrate < -27)
      {
       accelAngleXdeg=0;
      } 
      // averaging out the angle for X-axis for the accelerometer
      avgAccelXdegdummy = 0;
      for (int j=0; j<15; j++) { avgAccelXdegdummy += accelAngleXdeg; }
      avgAccelXdeg = avgAccelXdegdummy / 15;
      
      if (avgAccelXdeg < 0)
          avgAccelXdeg += 360;
      else if (avgAccelXdeg > 359)
          avgAccelXdeg -= 360;
 

//////////////////////////////////////////Y-ANGLE/////////////////////////////////////////////////////////////////////////////////
      accelAngleYrad = asin(accelerationZ/acceleration); 
      accelAngleYdeg = (accelAngleYrad * 57.29578); // 180/pi
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationY < 0) {accelAngleYdeg = 180 - accelAngleYdeg;}
    
      accelAngleYdeg -= 90; // to offset the angle to make it align with desired scale
      
      avgAccelYdegdummy = 0;
      for (int k=0; k<15; k++) { avgAccelYdegdummy += accelAngleYdeg; }
      avgAccelYdeg = avgAccelYdegdummy / 15;
      
      if (avgAccelYdeg < 0)
          {avgAccelYdeg += 360;}
      else if (avgAccelYdeg > 359)
          avgAccelYdeg -= 360;    
//////////////////////////////////////////Z-ANGLE/////////////////////////////////////////////////////////////////////////////////    
     accelAngleZrad = atan(accelerationY/accelerationX);
      accelAngleZdeg = accelAngleZrad * 57.29578; // 180/pi 
      
//////////////////////////////////////////GYRO RATE/////////////////////////////////////////////////////////////////////////////////         

 analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroZ = analogRead(gyroPinZ); // Z-AXIS - GYROSCOPE
    
    avgGyroZdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroZdummy += gyroZ; }
    avgGyroZ = avgGyroZdummy / 15;
    
    gyroZrate =  ( ( (avgGyroZ*AnalogRef) - (1023*gyroZzeroVoltage)) / 1023) / gyroSensitivity;
    
    if ( ((avgAccelX >= 473)&&(avgAccelX <= 488)) && ((avgAccelY >= 535)&&(avgAccelY <= 555)) && ((avgAccelZ >= 230)&&(avgAccelZ <= 258)) )
    {
     gyroZrate = 0;
    } 
   Serial.println(avgAccelY);  //850
  }
 while (avgAccelY < 850 && BackState==HIGH);
 if (BackState==LOW)
 {
   delay(400);
   goto cutsetshort;
 } 
 else 
  {
   delay(1000);
   repcount++;  
  }
 
 if (gyroZrate > 55 || gyroZrate < -35 || avgAccelYdeg > 290 || avgAccelYdeg < 252)
 { 
   faultyrepcount++;
   digitalWrite(ledRedPin, HIGH);
   delay(500);
   digitalWrite(ledRedPin, LOW);
 } 
 goto repcountcheck;
















/////////////////////////////////////////DEADLIFT SUBROUTINE///////////////////////////////////////////////////////////// 
 getdatadeadlift:
  do 
  {
    BackState=digitalRead(BackPin);
    //////////////////////////////////////////X-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelX = analogRead(accelPinX); // X-AXIS - ACCELEROMETER
    
    avgAccelXdummy = 0;
    for (int ia=0; ia<20; ia++) { avgAccelXdummy += accelX; }
    avgAccelX = avgAccelXdummy / 15;
    
    accelerationX =  ( ( (avgAccelX*AnalogRef) - (1023*accelXzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Y-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelY = analogRead(accelPinY); // Y-AXIS - ACCELEROMETER
    
    avgAccelYdummy = 0;
    for (int ib=0; ib<15; ib++) { avgAccelYdummy += accelY; }
    avgAccelY = avgAccelYdummy / 15;
    
    accelerationY =  ( ( (avgAccelY*AnalogRef) - (1023*accelYzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Z-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelZ = analogRead(accelPinZ); // Z-AXIS - ACCELEROMETER
    
    avgAccelZdummy = 0;
    for (int ic=0; ic<15; ic++) { avgAccelZdummy += accelZ; }
    avgAccelZ = avgAccelZdummy / 15;
    
    accelerationZ =  -( ( (avgAccelZ*AnalogRef) - (1023*accelZzeroVoltage)) / 1023) / accelSensitivity;




 //////////////////////////////////////////ACCELERATION MAGNITUDE/////////////////////////////////////////////////////////////////////////////////
    acceleration = sqrt((accelerationX*accelerationX) + (accelerationY*accelerationY) + (accelerationZ*accelerationZ));
    
//////////////////////////////////////////X-ANGLE/////////////////////////////////////////////////////////////////////////////////
      
      accelAngleXrad = asin(accelerationZ/acceleration); 
      accelAngleXdeg = accelAngleXrad * 57.29578; // 180/pi - to convert radians to degrees
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationX < 0) {accelAngleXdeg = 180 - accelAngleXdeg;}
    
      accelAngleXdeg -=85; // to offset the angle to make it align with desired scale
      if (gyroXrate > 27 || gyroXrate < -27)
      {
       accelAngleXdeg=0;
      } 
      // averaging out the angle for X-axis for the accelerometer
      avgAccelXdegdummy = 0;
      for (int j=0; j<15; j++) { avgAccelXdegdummy += accelAngleXdeg; }
      avgAccelXdeg = avgAccelXdegdummy / 15;
      
      if (avgAccelXdeg < 0)
          avgAccelXdeg += 360;
      else if (avgAccelXdeg > 359)
          avgAccelXdeg -= 360;
 

//////////////////////////////////////////Y-ANGLE/////////////////////////////////////////////////////////////////////////////////
      accelAngleYrad = asin(accelerationZ/acceleration); 
      accelAngleYdeg = (accelAngleYrad * 57.29578); // 180/pi
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationY < 0) {accelAngleYdeg = 180 - accelAngleYdeg;}
    
      accelAngleYdeg -= 90; // to offset the angle to make it align with desired scale
      
      avgAccelYdegdummy = 0;
      for (int k=0; k<15; k++) { avgAccelYdegdummy += accelAngleYdeg; }
      avgAccelYdeg = avgAccelYdegdummy / 15;
      
      if (avgAccelYdeg < 0)
          {avgAccelYdeg += 360;}
      else if (avgAccelYdeg > 359)
          avgAccelYdeg -= 360;    
//////////////////////////////////////////Z-ANGLE/////////////////////////////////////////////////////////////////////////////////    
     accelAngleZrad = atan(accelerationY/accelerationX);
      accelAngleZdeg = accelAngleZrad * 57.29578; // 180/pi 
      
//////////////////////////////////////////GYRO RATE/////////////////////////////////////////////////////////////////////////////////         

 analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroZ = analogRead(gyroPinZ); // Z-AXIS - GYROSCOPE
    
    avgGyroZdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroZdummy += gyroZ; }
    avgGyroZ = avgGyroZdummy / 15;
    
    gyroZrate =  ( ( (avgGyroZ*AnalogRef) - (1023*gyroZzeroVoltage)) / 1023) / gyroSensitivity;
    
    if ( ((avgAccelX >= 473)&&(avgAccelX <= 488)) && ((avgAccelY >= 535)&&(avgAccelY <= 555)) && ((avgAccelZ >= 230)&&(avgAccelZ <= 258)) )
    {
     gyroZrate = 0;
    } 
  Serial.println(avgAccelY);  
  }
 while (avgAccelY < .8 && BackState==HIGH);
 if (BackState==LOW)
 {
   goto cutsetshort;
 } 
 else 
 {
    repcount++;
    delay(2500);
 }
 goto repcountcheck;











/////////////////////////////////////////BICEPS CURLS SUBROUTINE///////////////////////////////////////////////////////////// 
 getdatabiceps:
  do 
  {
    BackState=digitalRead(BackPin);
    //////////////////////////////////////////X-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelX = analogRead(accelPinX); // X-AXIS - ACCELEROMETER
    
    avgAccelXdummy = 0;
    for (int ia=0; ia<20; ia++) { avgAccelXdummy += accelX; }
    avgAccelX = avgAccelXdummy / 15;
    
    accelerationX =  ( ( (avgAccelX*AnalogRef) - (1023*accelXzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Y-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelY = analogRead(accelPinY); // Y-AXIS - ACCELEROMETER
    
    avgAccelYdummy = 0;
    for (int ib=0; ib<15; ib++) { avgAccelYdummy += accelY; }
    avgAccelY = avgAccelYdummy / 15;
    
    accelerationY =  ( ( (avgAccelY*AnalogRef) - (1023*accelYzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Z-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelZ = analogRead(accelPinZ); // Z-AXIS - ACCELEROMETER
    
    avgAccelZdummy = 0;
    for (int ic=0; ic<15; ic++) { avgAccelZdummy += accelZ; }
    avgAccelZ = avgAccelZdummy / 15;
    
    accelerationZ =  -( ( (avgAccelZ*AnalogRef) - (1023*accelZzeroVoltage)) / 1023) / accelSensitivity;




 //////////////////////////////////////////ACCELERATION MAGNITUDE/////////////////////////////////////////////////////////////////////////////////
    acceleration = sqrt((accelerationX*accelerationX) + (accelerationY*accelerationY) + (accelerationZ*accelerationZ));
    
//////////////////////////////////////////X-ANGLE/////////////////////////////////////////////////////////////////////////////////
      
      accelAngleXrad = asin(accelerationZ/acceleration); 
      accelAngleXdeg = accelAngleXrad * 57.29578; // 180/pi - to convert radians to degrees
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationX < 0) {accelAngleXdeg = 180 - accelAngleXdeg;}
    
      accelAngleXdeg -=85; // to offset the angle to make it align with desired scale
//      if (gyroXrate > 27 || gyroXrate < -27)
//      {
//       accelAngleXdeg=0;
//      } 
      // averaging out the angle for X-axis for the accelerometer
      avgAccelXdegdummy = 0;
      for (int j=0; j<15; j++) { avgAccelXdegdummy += accelAngleXdeg; }
      avgAccelXdeg = avgAccelXdegdummy / 15;
      
      if (avgAccelXdeg < 0)
          avgAccelXdeg += 360;
      else if (avgAccelXdeg > 359)
          avgAccelXdeg -= 360;
 

//////////////////////////////////////////Y-ANGLE/////////////////////////////////////////////////////////////////////////////////
      accelAngleYrad = asin(accelerationZ/acceleration); 
      accelAngleYdeg = (accelAngleYrad * 57.29578); // 180/pi
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationY < 0) {accelAngleYdeg = 180 - accelAngleYdeg;}
    
      accelAngleYdeg -= 90; // to offset the angle to make it align with desired scale
      
      avgAccelYdegdummy = 0;
      for (int k=0; k<15; k++) { avgAccelYdegdummy += accelAngleYdeg; }
      avgAccelYdeg = avgAccelYdegdummy / 15;
      
      if (avgAccelYdeg < 0)
          {avgAccelYdeg += 360;}
      else if (avgAccelYdeg > 359)
          avgAccelYdeg -= 360;    
//////////////////////////////////////////Z-ANGLE/////////////////////////////////////////////////////////////////////////////////    
     accelAngleZrad = atan(accelerationY/accelerationX);
      accelAngleZdeg = accelAngleZrad * 57.29578; // 180/pi 
      
//////////////////////////////////////////GYRO RATE/////////////////////////////////////////////////////////////////////////////////         

 analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroZ = analogRead(gyroPinZ); // Z-AXIS - GYROSCOPE
    
    avgGyroZdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroZdummy += gyroZ; }
    avgGyroZ = avgGyroZdummy / 15;
    
    gyroZrate =  ( ( (avgGyroZ*AnalogRef) - (1023*gyroZzeroVoltage)) / 1023) / gyroSensitivity;
    
    if ( ((avgAccelX >= 473)&&(avgAccelX <= 488)) && ((avgAccelY >= 535)&&(avgAccelY <= 555)) && ((avgAccelZ >= 230)&&(avgAccelZ <= 258)) )
    {
     gyroZrate = 0;
    } 
    
    
     analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroX = analogRead(gyroPinX); //X-AXIS - GYROSCOPE
    
    avgGyroXdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroXdummy += gyroX; }
    avgGyroX = avgGyroXdummy / 15;
    
    gyroXrate =  ( ( (avgGyroX*AnalogRef) - (1023*gyroXzeroVoltage)) / 1023) / gyroSensitivity;  
  
  
    
 //Serial.println(avgAccelYdeg);  
  }
 while (avgAccelYdeg < 225 && BackState==HIGH);
 delay(500);
 if (BackState==LOW)
 {
   goto cutsetshort;
 }
 else
 {
   if (gyroZrate > 200 || gyroZrate < -200 )
   {
     faultycheck++;
   } 
   do 
  {
    BackState=digitalRead(BackPin);
    //////////////////////////////////////////X-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelX = analogRead(accelPinX); // X-AXIS - ACCELEROMETER
    
    avgAccelXdummy = 0;
    for (int ia=0; ia<20; ia++) { avgAccelXdummy += accelX; }
    avgAccelX = avgAccelXdummy / 15;
    
    accelerationX =  ( ( (avgAccelX*AnalogRef) - (1023*accelXzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Y-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelY = analogRead(accelPinY); // Y-AXIS - ACCELEROMETER
    
    avgAccelYdummy = 0;
    for (int ib=0; ib<15; ib++) { avgAccelYdummy += accelY; }
    avgAccelY = avgAccelYdummy / 15;
    
    accelerationY =  ( ( (avgAccelY*AnalogRef) - (1023*accelYzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Z-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelZ = analogRead(accelPinZ); // Z-AXIS - ACCELEROMETER
    
    avgAccelZdummy = 0;
    for (int ic=0; ic<15; ic++) { avgAccelZdummy += accelZ; }
    avgAccelZ = avgAccelZdummy / 15;
    
    accelerationZ =  -( ( (avgAccelZ*AnalogRef) - (1023*accelZzeroVoltage)) / 1023) / accelSensitivity;




 //////////////////////////////////////////ACCELERATION MAGNITUDE/////////////////////////////////////////////////////////////////////////////////
    acceleration = sqrt((accelerationX*accelerationX) + (accelerationY*accelerationY) + (accelerationZ*accelerationZ));
    
//////////////////////////////////////////X-ANGLE/////////////////////////////////////////////////////////////////////////////////
      
      accelAngleXrad = asin(accelerationZ/acceleration); 
      accelAngleXdeg = accelAngleXrad * 57.29578; // 180/pi - to convert radians to degrees
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationX < 0) {accelAngleXdeg = 180 - accelAngleXdeg;}
    
      accelAngleXdeg -=85; // to offset the angle to make it align with desired scale
//      if (gyroXrate > 27 || gyroXrate < -27)
//      {
//       accelAngleXdeg=0;
//      } 
      // averaging out the angle for X-axis for the accelerometer
      avgAccelXdegdummy = 0;
      for (int j=0; j<15; j++) { avgAccelXdegdummy += accelAngleXdeg; }
      avgAccelXdeg = avgAccelXdegdummy / 15;
      
      if (avgAccelXdeg < 0)
          avgAccelXdeg += 360;
      else if (avgAccelXdeg > 359)
          avgAccelXdeg -= 360;
 

//////////////////////////////////////////Y-ANGLE/////////////////////////////////////////////////////////////////////////////////
      accelAngleYrad = asin(accelerationZ/acceleration); 
      accelAngleYdeg = (accelAngleYrad * 57.29578); // 180/pi
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationY < 0) {accelAngleYdeg = 180 - accelAngleYdeg;}
    
      accelAngleYdeg -= 90; // to offset the angle to make it align with desired scale
      
      avgAccelYdegdummy = 0;
      for (int k=0; k<15; k++) { avgAccelYdegdummy += accelAngleYdeg; }
      avgAccelYdeg = avgAccelYdegdummy / 15;
      
      if (avgAccelYdeg < 0)
          {avgAccelYdeg += 360;}
      else if (avgAccelYdeg > 359)
          avgAccelYdeg -= 360;    
//////////////////////////////////////////Z-ANGLE/////////////////////////////////////////////////////////////////////////////////    
     accelAngleZrad = atan(accelerationY/accelerationX);
      accelAngleZdeg = accelAngleZrad * 57.29578; // 180/pi 
      
//////////////////////////////////////////GYRO RATE/////////////////////////////////////////////////////////////////////////////////         

 analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroZ = analogRead(gyroPinZ); // Z-AXIS - GYROSCOPE
    
    avgGyroZdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroZdummy += gyroZ; }
    avgGyroZ = avgGyroZdummy / 15;
    
    gyroZrate =  ( ( (avgGyroZ*AnalogRef) - (1023*gyroZzeroVoltage)) / 1023) / gyroSensitivity;
    
    if ( ((avgAccelX >= 473)&&(avgAccelX <= 488)) && ((avgAccelY >= 535)&&(avgAccelY <= 555)) && ((avgAccelZ >= 230)&&(avgAccelZ <= 258)) )
    {
     gyroZrate = 0;
    } 
    
    
     analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroX = analogRead(gyroPinX); //X-AXIS - GYROSCOPE
    
    avgGyroXdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroXdummy += gyroX; }
    avgGyroX = avgGyroXdummy / 15;
    
    gyroXrate =  ( ( (avgGyroX*AnalogRef) - (1023*gyroXzeroVoltage)) / 1023) / gyroSensitivity;    
  }
 while (avgAccelYdeg > 125 && BackState==HIGH);
  if (BackState==LOW)
  { 
    goto cutsetshort;
  } 
   
   delay(500);
   repcount++;
   
 if (gyroZrate > 200 || gyroZrate < -200 )
 { 
   faultycheck++;
 }
 
 if (faultycheck > 0)
 {
   faultycheck=0;
   faultyrepcount++;
   digitalWrite(ledRedPin, HIGH);
   delay(500);
   digitalWrite(ledRedPin, LOW);
 } 
  
 }
 
 goto repcountcheck;




















/////////////////////////////////////////TRICEPS EXTENSION SUBROUTINE///////////////////////////////////////////////////////////// 
 getdatatriceps:
  do 
  {
    BackState=digitalRead(BackPin);
    //////////////////////////////////////////X-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelX = analogRead(accelPinX); // X-AXIS - ACCELEROMETER
    
    avgAccelXdummy = 0;
    for (int ia=0; ia<20; ia++) { avgAccelXdummy += accelX; }
    avgAccelX = avgAccelXdummy / 15;
    
    accelerationX =  ( ( (avgAccelX*AnalogRef) - (1023*accelXzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Y-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelY = analogRead(accelPinY); // Y-AXIS - ACCELEROMETER
    
    avgAccelYdummy = 0;
    for (int ib=0; ib<15; ib++) { avgAccelYdummy += accelY; }
    avgAccelY = avgAccelYdummy / 15;
    
    accelerationY =  ( ( (avgAccelY*AnalogRef) - (1023*accelYzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Z-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelZ = analogRead(accelPinZ); // Z-AXIS - ACCELEROMETER
    
    avgAccelZdummy = 0;
    for (int ic=0; ic<15; ic++) { avgAccelZdummy += accelZ; }
    avgAccelZ = avgAccelZdummy / 15;
    
    accelerationZ =  -( ( (avgAccelZ*AnalogRef) - (1023*accelZzeroVoltage)) / 1023) / accelSensitivity;




 //////////////////////////////////////////ACCELERATION MAGNITUDE/////////////////////////////////////////////////////////////////////////////////
    acceleration = sqrt((accelerationX*accelerationX) + (accelerationY*accelerationY) + (accelerationZ*accelerationZ));
    
//////////////////////////////////////////X-ANGLE/////////////////////////////////////////////////////////////////////////////////
      
      accelAngleXrad = asin(accelerationZ/acceleration); 
      accelAngleXdeg = accelAngleXrad * 57.29578; // 180/pi - to convert radians to degrees
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationX < 0) {accelAngleXdeg = 180 - accelAngleXdeg;}
    
      accelAngleXdeg -=85; // to offset the angle to make it align with desired scale
//      if (gyroXrate > 27 || gyroXrate < -27)
//      {
//       accelAngleXdeg=0;
//      } 
      // averaging out the angle for X-axis for the accelerometer
      avgAccelXdegdummy = 0;
      for (int j=0; j<15; j++) { avgAccelXdegdummy += accelAngleXdeg; }
      avgAccelXdeg = avgAccelXdegdummy / 15;
      
      if (avgAccelXdeg < 0)
          avgAccelXdeg += 360;
      else if (avgAccelXdeg > 359)
          avgAccelXdeg -= 360;
 

//////////////////////////////////////////Y-ANGLE/////////////////////////////////////////////////////////////////////////////////
      accelAngleYrad = asin(accelerationZ/acceleration); 
      accelAngleYdeg = (accelAngleYrad * 57.29578); // 180/pi
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationY < 0) {accelAngleYdeg = 180 - accelAngleYdeg;}
    
      accelAngleYdeg -= 90; // to offset the angle to make it align with desired scale
      
      avgAccelYdegdummy = 0;
      for (int k=0; k<15; k++) { avgAccelYdegdummy += accelAngleYdeg; }
      avgAccelYdeg = avgAccelYdegdummy / 15;
      
      if (avgAccelYdeg < 0)
          {avgAccelYdeg += 360;}
      else if (avgAccelYdeg > 359)
          avgAccelYdeg -= 360;    
//////////////////////////////////////////Z-ANGLE/////////////////////////////////////////////////////////////////////////////////    
     accelAngleZrad = atan(accelerationY/accelerationX);
      accelAngleZdeg = accelAngleZrad * 57.29578; // 180/pi 
      
//////////////////////////////////////////GYRO RATE/////////////////////////////////////////////////////////////////////////////////         

 analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroZ = analogRead(gyroPinZ); // Z-AXIS - GYROSCOPE
    
    avgGyroZdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroZdummy += gyroZ; }
    avgGyroZ = avgGyroZdummy / 15;
    
    gyroZrate =  ( ( (avgGyroZ*AnalogRef) - (1023*gyroZzeroVoltage)) / 1023) / gyroSensitivity;
    
    if ( ((avgAccelX >= 473)&&(avgAccelX <= 488)) && ((avgAccelY >= 535)&&(avgAccelY <= 555)) && ((avgAccelZ >= 230)&&(avgAccelZ <= 258)) )
    {
     gyroZrate = 0;
    } 
    
    
     analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroX = analogRead(gyroPinX); //X-AXIS - GYROSCOPE
    
    avgGyroXdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroXdummy += gyroX; }
    avgGyroX = avgGyroXdummy / 15;
    
    gyroXrate =  ( ( (avgGyroX*AnalogRef) - (1023*gyroXzeroVoltage)) / 1023) / gyroSensitivity;  
    
  }
 while (avgAccelYdeg > 125 && BackState==HIGH);
 

 delay(500);
 if (BackState==LOW)
 {
   goto cutsetshort;
 }
 else
 {
   if (gyroXrate > 160 || gyroXrate < -160)
   {
     faultycheck++;
   } 
   do 
  {
    BackState=digitalRead(BackPin);
    //////////////////////////////////////////X-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelX = analogRead(accelPinX); // X-AXIS - ACCELEROMETER
    
    avgAccelXdummy = 0;
    for (int ia=0; ia<20; ia++) { avgAccelXdummy += accelX; }
    avgAccelX = avgAccelXdummy / 15;
    
    accelerationX =  ( ( (avgAccelX*AnalogRef) - (1023*accelXzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Y-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelY = analogRead(accelPinY); // Y-AXIS - ACCELEROMETER
    
    avgAccelYdummy = 0;
    for (int ib=0; ib<15; ib++) { avgAccelYdummy += accelY; }
    avgAccelY = avgAccelYdummy / 15;
    
    accelerationY =  ( ( (avgAccelY*AnalogRef) - (1023*accelYzeroVoltage)) / 1023) / accelSensitivity;

    //////////////////////////////////////////Z-ACCEL/////////////////////////////////////////////////////////////////////////////////
    analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    accelZ = analogRead(accelPinZ); // Z-AXIS - ACCELEROMETER
    
    avgAccelZdummy = 0;
    for (int ic=0; ic<15; ic++) { avgAccelZdummy += accelZ; }
    avgAccelZ = avgAccelZdummy / 15;
    
    accelerationZ =  -( ( (avgAccelZ*AnalogRef) - (1023*accelZzeroVoltage)) / 1023) / accelSensitivity;




 //////////////////////////////////////////ACCELERATION MAGNITUDE/////////////////////////////////////////////////////////////////////////////////
    acceleration = sqrt((accelerationX*accelerationX) + (accelerationY*accelerationY) + (accelerationZ*accelerationZ));
    
//////////////////////////////////////////X-ANGLE/////////////////////////////////////////////////////////////////////////////////
      
      accelAngleXrad = asin(accelerationZ/acceleration); 
      accelAngleXdeg = accelAngleXrad * 57.29578; // 180/pi - to convert radians to degrees
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationX < 0) {accelAngleXdeg = 180 - accelAngleXdeg;}
    
      accelAngleXdeg -=85; // to offset the angle to make it align with desired scale
//      if (gyroXrate > 27 || gyroXrate < -27)
//      {
//       accelAngleXdeg=0;
//      } 
      // averaging out the angle for X-axis for the accelerometer
      avgAccelXdegdummy = 0;
      for (int j=0; j<15; j++) { avgAccelXdegdummy += accelAngleXdeg; }
      avgAccelXdeg = avgAccelXdegdummy / 15;
      
      if (avgAccelXdeg < 0)
          avgAccelXdeg += 360;
      else if (avgAccelXdeg > 359)
          avgAccelXdeg -= 360;
 

//////////////////////////////////////////Y-ANGLE/////////////////////////////////////////////////////////////////////////////////
      accelAngleYrad = asin(accelerationZ/acceleration); 
      accelAngleYdeg = (accelAngleYrad * 57.29578); // 180/pi
      
      
      // to correct the scale for the pitch angle from the accelerometer  
      if (accelerationY < 0) {accelAngleYdeg = 180 - accelAngleYdeg;}
    
      accelAngleYdeg -= 90; // to offset the angle to make it align with desired scale
      
      avgAccelYdegdummy = 0;
      for (int k=0; k<15; k++) { avgAccelYdegdummy += accelAngleYdeg; }
      avgAccelYdeg = avgAccelYdegdummy / 15;
      
      if (avgAccelYdeg < 0)
          {avgAccelYdeg += 360;}
      else if (avgAccelYdeg > 359)
          avgAccelYdeg -= 360;    
//////////////////////////////////////////Z-ANGLE/////////////////////////////////////////////////////////////////////////////////    
     accelAngleZrad = atan(accelerationY/accelerationX);
      accelAngleZdeg = accelAngleZrad * 57.29578; // 180/pi 
      
//////////////////////////////////////////GYRO RATE/////////////////////////////////////////////////////////////////////////////////         

 analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroZ = analogRead(gyroPinZ); // Z-AXIS - GYROSCOPE
    
    avgGyroZdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroZdummy += gyroZ; }
    avgGyroZ = avgGyroZdummy / 15;
    
    gyroZrate =  ( ( (avgGyroZ*AnalogRef) - (1023*gyroZzeroVoltage)) / 1023) / gyroSensitivity;
    
    if ( ((avgAccelX >= 473)&&(avgAccelX <= 488)) && ((avgAccelY >= 535)&&(avgAccelY <= 555)) && ((avgAccelZ >= 230)&&(avgAccelZ <= 258)) )
    {
     gyroZrate = 0;
    } 
    
    
     analogReference(EXTERNAL); // sets the reference voltage to 3.3V (externally connected to the Aref pin)
    gyroX = analogRead(gyroPinX); //X-AXIS - GYROSCOPE
    
    avgGyroXdummy = 0;
    for (int ie=0; ie<15; ie++) { avgGyroXdummy += gyroX; }
    avgGyroX = avgGyroXdummy / 15;
    
    gyroXrate =  ( ( (avgGyroX*AnalogRef) - (1023*gyroXzeroVoltage)) / 1023) / gyroSensitivity;  
  
  
    
  
  }
 while (avgAccelYdeg < 225 && BackState==HIGH);
  if (BackState==LOW)
  { 
    goto cutsetshort;
  } 
   delay(500);
   repcount++;

 
 if (gyroXrate > 160 || gyroXrate < -160)
 { 
   faultycheck++;
 }
 
 if (faultycheck > 0)
 {
   faultycheck=0;
   faultyrepcount++;
   digitalWrite(ledRedPin, HIGH);
   delay(500);
   digitalWrite(ledRedPin, LOW);
 } 
  
 }
 
 goto repcountcheck;




/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
 repcountcheck:
 if (repcount==repgoal)
 {
       cutsetshort:
       setcount++;
       
       switch (lift)
       {
             case 1:
             EEPROM.write(setcount, (repcount-faultyrepcount));
             break;
             case 2:
             EEPROM.write(100+setcount, (repcount-faultyrepcount));
             break;
             case 3:
             EEPROM.write(200+setcount, (repcount-faultyrepcount));
             break;
             case 4:
             EEPROM.write(300+setcount, (repcount-faultyrepcount));
             break;
             case 5:
             EEPROM.write(400+setcount, (repcount-faultyrepcount));
             break;
       }
       
       repcount=0;
       faultyrepcount=0;
       lcd.clear();
       lcd.setCursor(2,0);
       lcd.print("Set completed");
       digitalWrite(ledGreenPin, HIGH);
       delay(500);
       digitalWrite(ledGreenPin, LOW);
       delay(500);
       digitalWrite(ledGreenPin, HIGH);
       delay(500);
       digitalWrite(ledGreenPin, LOW);
       delay(500);
       digitalWrite(ledGreenPin, HIGH);
       delay(500);
       digitalWrite(ledGreenPin, LOW);
       delay(500);
       
       if (setcount==setgoal)
       {
                 lcd.clear();
                 lcd.setCursor(4,0);
                 lcd.print("Exercise");
                 lcd.setCursor(4,1);
                 lcd.print("completed");
                 delay(1000);
                 goto choice1;
       }
       else if (repcheck==0 && weightcheck==0) 
           {
                 goto countdown;
           }
       else if (repcheck==1)
           {
                 goto repselect3;
           } 
       else if (repcheck==0 && weightcheck==1)
           {
                 goto weightselect3;
           }
       else
           {
                 goto start;
           } 
 }
 else 
 {
       switch (lift)
     {
       case 1:
        goto getdatasquat;
       case 2:
        goto getdatabenchpress;
       case 3:
        goto getdatadeadlift;
       case 4:
        goto getdatabiceps;
       case 5:
        goto getdatatriceps; 
     }  
 } 
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 memread:
 
  delay(400);
  
  Serial.print("#S|LOGTEST|[");
  Serial.print("      Liftname  Setnumber  Completed Reps/Desired Reps  weight");
  Serial.println("]#");
  
  Serial.print("#S|LOGTEST|[");
  Serial.print("      --------------------------------------------------------");
  Serial.println("]#");
 
  //now i need to read the lifts and print them to the layout
  
  //now we need to find out how many sets were done
       
    ////////////////////////////////////////SQUAT PRINT////////////////////////////////////////////////
    squat:
    totalsetnumber = 1;
    setnumber = 1;
    for(address2 = 1; address2 < 11; address2++)
    {
      totalsetnumber++;                         
                      // this gives us the number of sets we did
    } 
    address2 = 1;
    for(setnumber = 1; setnumber < totalsetnumber; setnumber++)
    { 
      
      
      Serial.print("#S|LOGTEST|[");
      Serial.print("      ");       //I need to find a way to store the date the lift was done at NO YOU DON'T
      Serial.print("Squat         "); // prints squat and spacing
      
      //Here we print out appropriate spacing and the completed number of reps
      address2=address2+10;
      repdes=EEPROM.read(address2);
      address2=address2-10;
      repcom = EEPROM.read(address2);
      
      Serial.print(itoa((setnumber), buffer, 10));
      Serial.print("                ");
      Serial.print(itoa((repcom), buffer, 10));
      Serial.print("/");     
            
      address2 = address2 + 10 ;
      repdes = EEPROM.read(address2);
      if (repdes == 0) {goto bench;}
      
      Serial.print(itoa((repdes), buffer, 10));
      Serial.print("                ");
      
      address2 = address2 + 10 ;
      weight2 = EEPROM.read(address2);
      Serial.print(itoa((weight2), buffer, 10));
      Serial.println("]#");
      
      address2 = address2 - 19; //sets the address2 pointer to 
      
      
    }

    /////////////////////////////////////////BENCH////////////////////////////////////////////
    bench:
    
      Serial.print("#S|LOGTEST|[");
      Serial.print("      --------------------------------------------------------");
      Serial.println("]#");
    
    
    totalsetnumber = 1;
    setnumber = 1;
    for(address2 = 101; address2 < 111; address2++)
    {
      totalsetnumber++;                         
                              // this gives us the number of sets we did
    }
    address2 = 101;
    for(setnumber = 1; setnumber < totalsetnumber; setnumber++)
    {
      
      
      
      Serial.print("#S|LOGTEST|[");
      Serial.print("      ");       //I need to find a way to store the date the lift was done at
      Serial.print("Bench         "); // prints squat and spacing
      
      //Here we print out appropriate spacing and the completed number of reps
      address2=address2+10;
      repdes=EEPROM.read(address2);
      address2=address2-10;
      repcom = EEPROM.read(address2);
      
      Serial.print(itoa((setnumber), buffer, 10));
      Serial.print("                ");
      Serial.print(itoa((repcom), buffer, 10));
      Serial.print("/");     
            
      address2 = address2 + 10 ;
      repdes = EEPROM.read(address2);
      if (repdes == 0) {goto deadlift;}
      Serial.print(itoa((repdes), buffer, 10));
      Serial.print("                ");
      
      address2 = address2 + 10 ;
      weight2 = EEPROM.read(address2);
      Serial.print(itoa((weight2), buffer, 10));
      Serial.println("]#");

      address2 = address2 - 19;
      
      
    }
    
///////////////////////////////////DEAD LIFT//////////////////////////////////////    
    
    deadlift:
    
      Serial.print("#S|LOGTEST|[");
      Serial.print("      --------------------------------------------------------");
      Serial.println("]#");
    
    totalsetnumber = 1;
    setnumber = 1;
    for(address2 = 201; address2 < 211; address2++)
    {
      totalsetnumber++;                         
                              // this gives us the number of sets we did
    }
    address2 = 201;
    for(setnumber = 1; setnumber < totalsetnumber; setnumber++)
    {
     
      
      
      Serial.print("#S|LOGTEST|[");
      Serial.print("      ");       //I need to find a way to store the date the lift was done at
      Serial.print("Dead Lift     "); // prints squat and spacing
      
      //Here we print out appropriate spacing and the completed number of reps
       address2=address2+10;
      repdes=EEPROM.read(address2);
      address2=address2-10;
      repcom = EEPROM.read(address2);
      
      Serial.print(itoa((setnumber), buffer, 10));
      Serial.print("                ");
      Serial.print(itoa((repcom), buffer, 10));
      Serial.print("/");     
            
      address2 = address2 + 10 ;
      repdes = EEPROM.read(address2);
      if (repdes == 0) {goto bicepcurl;}
      Serial.print(itoa((repdes), buffer, 10));
      Serial.print("                ");
      
      address2 = address2 + 10 ;
      weight2 = EEPROM.read(address2);
      Serial.print(itoa((weight2), buffer, 10));
      Serial.println("]#");

      address2 = address2 - 19;
      
      
    }
    
////////////////////////////////BICEP CURL////////////////////////////////////////////////////    
    
     bicepcurl:
    
      Serial.print("#S|LOGTEST|[");
      Serial.print("      --------------------------------------------------------");
      Serial.println("]#");
      
    totalsetnumber = 1;
    setnumber = 1;
    for(address2 = 301; address2 < 311; address2++)
    {
      totalsetnumber++;                         
                              // this gives us the number of sets we did
    }
    address2 = 301;
    for(setnumber = 1; setnumber < totalsetnumber; setnumber++)
    {
      
      
      
      Serial.print("#S|LOGTEST|[");
      Serial.print("      ");       //I need to find a way to store the date the lift was done at
      Serial.print("Bicep Curl    "); // prints squat and spacing
      
      //Here we print out appropriate spacing and the completed number of reps
      address2=address2+10;
      repdes=EEPROM.read(address2);
      address2=address2-10;
      repcom = EEPROM.read(address2);
      
      Serial.print(itoa((setnumber), buffer, 10));
      Serial.print("                ");
      Serial.print(itoa((repcom), buffer, 10));
      Serial.print("/");     
            
      address2 = address2 + 10 ;
      repdes = EEPROM.read(address2);
      if (repdes == 0) {goto tricepextension;}
      Serial.print(itoa((repdes), buffer, 10));
      Serial.print("                ");
      
      address2 = address2 + 10 ;
      weight2 = EEPROM.read(address2);
      Serial.print(itoa((weight2), buffer, 10));
      Serial.println("]#");

      address2 = address2 - 19;
      
      
    }
    
    
////////////////////////////////////TRICEP EXTENSION///////////////////////////////////////
    
     tricepextension:
    
      Serial.print("#S|LOGTEST|[");
      Serial.print("      --------------------------------------------------------");
      Serial.println("]#");    
    
    totalsetnumber = 1;
    setnumber = 1;
    for(address2 = 401; address2 < 411; address2++)
    {
      totalsetnumber++;                         
                              // this gives us the number of sets we did
    }
    address2 = 401;
    for(setnumber = 1; setnumber < totalsetnumber; setnumber++)
    {
      
      
      
      Serial.print("#S|LOGTEST|[");
      Serial.print("      ");       //I need to find a way to store the date the lift was done at
      Serial.print("Tricep Ext.   "); // prints squat and spacing
      
      //Here we print out appropriate spacing and the completed number of reps
      address2=address2+10;
      repdes=EEPROM.read(address2);
      address2=address2-10;
      repcom = EEPROM.read(address2);
      
      Serial.print(itoa((setnumber), buffer, 10));
      Serial.print("                ");
      Serial.print(itoa((repcom), buffer, 10));
      Serial.print("/");     
            
      address2 = address2 + 10 ;
      repdes = EEPROM.read(address2);
      if (repdes == 0) {goto EndofWorkout;}
      Serial.print(itoa((repdes), buffer, 10));
      Serial.print("                ");
      
      address2 = address2 + 10 ;
      weight2 = EEPROM.read(address2);
      Serial.print(itoa((weight2), buffer, 10));
      Serial.println("]#");

      address2 = address2 - 19;
      
      
    }

   EndofWorkout:
   Serial.print("#S|LOGTEST|[");
   Serial.print("      --------------------------------------------------------");
   Serial.println("]#");
   
   Serial.print("#S|LOGTEST|[");
   Serial.print("      --------------------END OF WORKOUT----------------------");
   Serial.println("]#");
   
   Serial.print("#S|LOGTEST|[");
   Serial.print("      --------------------HIT THE SAUNA!----------------------");
   Serial.println("]#");
   
 goto choice1;
 
 memclear:
 for (int i = 0; i < 432; i++)
 EEPROM.write(i, 0);
 Serial.println("memory cleared");
 digitalWrite(ledGreenPin, HIGH);
 delay(1000);
 digitalWrite(ledGreenPin, LOW);
 goto choice1;

 
}