//Libraries
#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <AccelStepper.h>
#include <ResponsiveAnalogRead.h>

//Functions
void mode_1();       //Performing the Exercise
void mode_2();       //Setting the Speed
void mode_3();       //Setting the Reps
void mode_4();       //Setting the Angle
void update_angle(); //Reading the Angle Sensor and Outputting the Angle
void print_mainscreen(int editing_mode, int mode, int sub_mode); //Printing Screen
int up_button_press();   //Moving Motor during Mode_4
int down_button_press(); //Moving Motor during Mode_4
int up_button_toggle(); 
int down_button_toggle();
int enter_button_toggle();
void sound_buzzer();
void sound_begin_excercise_buzzer();
void sound_end_excercise_buzzer();

//Pins
#define dir_Pin 10          //Stepper Motor
#define step_Pin 11         //Stepper Motor
#define angle_Pin A0        //Angle Sensor
#define down_Button_Pin 9   //Down Button
#define up_Button_Pin 12    //Up Button
#define enter_Button_Pin 13 //Enter Button
#define buzzer_Pin 8        //Buzzer
#define led_Pin A2          //LED

LiquidCrystal_I2C lcd(0x27, 20, 4);                   //LCD Display
AccelStepper stepper(1, step_Pin, dir_Pin);           //Stepper Motor
ResponsiveAnalogRead accelerometer(angle_Pin, true);  //Angle Sensor

//LCD Stuff
String excercise_text = "";
String speed_text = "";
String reps_text = "";
String angle_text = "";

//Button Stuffs
int enter_btn_new_state;
int enter_btn_old_state = 0;
int down_btn_new_state;
int down_btn_old_state = 0;
int up_btn_new_state;
int up_btn_old_state = 0;

//Angle Stuff
int WINDOW_SIZE = 10;
int INDEX = 0;
int VALUE = 0;
int SUM = 0;
int READINGS[10];
int AVERAGED = 0;

//Exercise Stuffs
float first_angle = -1;
float second_angle = -1;
int reps_total = 10;  //Default Reps
int reps_current = 0;
int speed = 1;        //Default Speed
int current_arm_angle = 0;

//Motor Stuff
int direction_motor = 1;
int maxspeed = 1400;
int motor_speed = 100;
int forward_speed = -200;
int reverse_speed = 200;
int slow_reverse_speed = 400;

bool game = true;
int mode = 4;

void setup() 
{
  //Pins
  pinMode(buzzer_Pin, OUTPUT);
  pinMode(down_Button_Pin, INPUT);
  pinMode(up_Button_Pin, INPUT);
  pinMode(enter_Button_Pin, INPUT);
  pinMode(dir_Pin, OUTPUT);
  pinMode(step_Pin, OUTPUT);
  pinMode(angle_Pin, INPUT);
  pinMode(led_Pin, OUTPUT);

  //Stepper Motor
  stepper.setMaxSpeed(maxspeed);
  stepper.setAcceleration(100);

  //LCD Display
  lcd.init();
  lcd.backlight();

  //Debugging
  Serial.begin(9600); 
}

void loop() 
{
  while(game)
  {
    if (mode == 1)
    {
      print_mainscreen(0, 1, 1);
      if (enter_button_toggle()) //Perform Exercise
      {
        mode = 1;
        reps_current = 0;
        mode_1(); 
      }
      else if (up_button_toggle())
      {
        mode = 2;
        print_mainscreen(2, mode, 1);
      }
      else if (down_button_toggle())
      {
        mode = 3;
        print_mainscreen(2, mode, 1);
      }
    }
    else if (mode == 2) 
    {
      mode_2(); //Setting Speed
      mode = 1;
    }
    else if (mode == 3) 
    {
      mode_3(); //Setting Reps
      mode = 1;
    }
    else if (mode == 4) 
    {
      mode_4(); //Setting Angles
      mode = 1;
    }
  }
}

//Setting the bottom and top angle of the exercise
void mode_4() 
{
  int exit_flag = 1;
  while (exit_flag) 
  {
    while (1) 
    {
      print_mainscreen(1, 4, 1);
      update_angle();

      while (up_button_press())   //Move motor to move arm up
      {
        stepper.setSpeed(forward_speed);
        stepper.runSpeed();
        update_angle();
      }

      while (down_button_press()) //Move motor to move arm down
      {
        stepper.setSpeed(reverse_speed);
        stepper.runSpeed();
        update_angle();
      }

      if (enter_button_toggle())  //Set arm angle as bottom angle
      {
        first_angle = current_arm_angle;
        print_mainscreen(1, 4, 1);

        stepper.setCurrentPosition(0);
        break;
      }
    }

    while (1) 
    {
      print_mainscreen(1, 4, 2);
      update_angle();

      while (up_button_press())   //Move motor to move arm up
      {
        stepper.setSpeed(forward_speed);
        stepper.runSpeed();
        update_angle();
      }

      while (down_button_press()) //Move motor to move arm down
      {
        stepper.setSpeed(reverse_speed);
        stepper.runSpeed();
        update_angle();
      }

      if (enter_button_toggle())  //Set arm angle as top angle
      {
        second_angle = current_arm_angle;
        print_mainscreen(1, 4, 2);
        
        if (first_angle >= second_angle) 
        {
          print_mainscreen(1, 4, 3);
          delay(2000);
        } 
        else 
        {
          exit_flag = 0;
          break;
        } 
      }
    }
  }
}

//Setting the speed of the exercise
void mode_2() 
{
  while (1) 
  {
    print_mainscreen(1, 2, 1);
    if (up_button_toggle())         //Increase speed by 1
    {
      if (1 <= speed && speed < 5) 
      {
        speed += 1;
      }
    } 
    else if (down_button_toggle())  //Decrease speed by 1
    {
      if (1 < speed && speed <= 5) 
      {
        speed -= 1;
      }
    } 
    else if (enter_button_toggle()) //Set speed
    {
      motor_speed = map(speed, 1, 5, 100, 500);
      delay(500);
      break;
    }
  }
}

//Setting the reps of the exercise
void mode_3() 
{
  while (1) 
  {
    print_mainscreen(1, 3, 1);
    if (up_button_toggle())         //Increase reps by 1 
    {
      if (0 < reps_total && reps_total < 30) 
      {
        reps_total += 1;
      }
    } 
    else if (down_button_toggle())  //Decrease reps by 1 
    {
      if (1 < reps_total && reps_total <= 30) 
      {
        reps_total -= 1;
      }
    } 
    else if (enter_button_toggle()) //Set reps 
    {
      break;
    }
  }
}

//Performing the exercise
void mode_1() 
{
  direction_motor = 0;
  while (1) 
  {
    update_angle();

    // We need this at the start so that the lcd screen immediately updates when we start the excercise
    if (direction_motor == 0)
      print_mainscreen(0, 1, 5);
    else if (direction_motor == 1)
      print_mainscreen(0, 1, 4);

    if (reps_current == 0)
    {
      sound_begin_excercise_buzzer();
    }

    while (current_arm_angle < second_angle && direction_motor == 0) //run stepper motor forward (direction_motor: 0 is forward)
    {
      update_angle();
      stepper.setSpeed(-motor_speed);	//stepper.begin(60, 1);
      stepper.runSpeed(); //stepper.rotate(360);
      if (enter_button_toggle()) //press enter to stop function
      {
        direction_motor = 2;
        stepper.moveTo(0);
        stepper.setSpeed(slow_reverse_speed);
        while (stepper.distanceToGo() != 0) 
        {
           //returning to original position (reverse motor)
          stepper.runToPosition();
          Serial.println(String(stepper.speed()));
          break;
        }
        if (direction_motor == 2)
        {
          mode = 4;
          break;
        }     
      }
    }

    while (current_arm_angle > first_angle && direction_motor == 1) //run stepper motor backward (direction_motor: 1 is backwards)
    {
      update_angle();
      stepper.setSpeed(motor_speed);	//stepper.begin(60, 1);
      stepper.runSpeed(); //stepper.rotate(-360);
      if (enter_button_toggle()) //press enter to stop function
      {
        direction_motor = 2;
        stepper.moveTo(0);
        stepper.setSpeed(slow_reverse_speed);
        while (stepper.distanceToGo() != 0) 
        {
          //stepper.setSpeed(slow_reverse_speed); //returning to original position (reverse motor)
          stepper.runToPosition();
          Serial.println(String(stepper.speed()));
          break;
        }
        if (direction_motor == 2)
        {
          mode = 4;
          break;
        }
      }
    }

    // if reach top
    // one alternative for motor code is to use the return position code to return hand to bottom
    if (current_arm_angle < second_angle + 3 && current_arm_angle > second_angle - 3 && direction_motor == 0) 
    {
      reps_current += 1;
      sound_buzzer();
      direction_motor = 1;  //change direction
      print_mainscreen(0, 1, 4);
      Serial.println("Reached top, angle: " + String(current_arm_angle));
    }

    //if reach bottom
    if (current_arm_angle < first_angle + 3 && current_arm_angle > first_angle - 3 && direction_motor == 1) 
    {
      sound_buzzer();
      direction_motor = 0;  //change direction
      print_mainscreen(0, 1, 5);
      Serial.println("Reached bottom, angle: " + String(current_arm_angle));
    }

    if (direction_motor == 2) 
    {
      print_mainscreen(0, 1, 3);
      sound_end_excercise_buzzer();
      delay(3000);
      break;
    }

    if (reps_current == reps_total)
    {
      delay(100);
      print_mainscreen(0, 1, 3);
      sound_end_excercise_buzzer();
      reps_current = 0;
      delay(3000);
      break;
    }
  }
}

void update_angle() 
{
  accelerometer.update();
  SUM = SUM - READINGS[INDEX]; 
  VALUE = accelerometer.getValue();
  READINGS[INDEX] = VALUE;            // Add the newest reading to the window
  SUM = SUM + VALUE;                  // Add the newest reading to the sum
  INDEX = (INDEX + 1) % WINDOW_SIZE;  // Increment the index, and wrap to 0 if it exceeds the window size
  AVERAGED = SUM / WINDOW_SIZE;

  current_arm_angle = constrain(map(AVERAGED, 344, 508, 0, 180), 0, 180); //Calculates angle based on angle sensor value
  //current_arm_angle = AVERAGED; use to calibrate the angle sensor; 271, 330, 395; 283, 419; 344, 508
}

//Prints the different screens
void print_mainscreen(int editing_mode, int mode, int sub_mode) 
{
  if (editing_mode == 0)      //Exercise
  { 
    if (mode == 1)
    {
      if (sub_mode == 1)      //Haven't started excercise
      {
        excercise_text = "Press ENTER to start";
        speed_text = "Speed: " + String(speed);
        reps_text = "REPS: " + String(reps_total);
        angle_text = "Angle: " + String(second_angle);
      }
      else if (sub_mode == 3) //Finished excercise
      {
        excercise_text = " ";
        speed_text = " Exercise Complete!";
        reps_text = "     Great Job!";
        angle_text = " ";
      }
      else if (sub_mode == 4) //Arm moving down
      {
        excercise_text = "Press ENTER to stop";
        speed_text = "Speed: " + String(speed);
        reps_text = "REPS: " + String(reps_current) + '/' + String(reps_total);
        angle_text = "Angle: Moving Down";
      }
      else if (sub_mode == 5) //Arm moving up
      {
        excercise_text = "Press ENTER to stop";
        speed_text = "Speed: " + String(speed);
        reps_text = "REPS: " + String(reps_current) + '/' + String(reps_total);
        angle_text = "Angle: Moving Up";
      }
    }
  } 
  else if (editing_mode == 1) //Editing speed/reps 
  {
    if (mode == 2) 
    {
      if (sub_mode == 1)      //Editing speed screen
      {
        excercise_text = "Editing Speed";
        speed_text = "> Speed: " + String(speed);
        reps_text = "REPS: " + String(reps_total);
        angle_text = "Angle: " + String(second_angle);
      }
    } 
    else if (mode == 3) 
    {
      if (sub_mode == 1)      //Editing reps screen
      {
        excercise_text = "Editing REPS";
        speed_text = "Speed: " + String(speed);
        reps_text = "> REPS: " + String(reps_total);
        angle_text = "Angle: " + String(second_angle);
      }
    } 
    else if (mode == 4)       //Editing Angles
    {
      if (sub_mode == 1)      //Setting bottom angle screen
      {
        excercise_text = "   Use up/down to";
        speed_text = "  Change DOWN angle";
        reps_text = "    Enter to Save";
        angle_text = "     " + String((int)current_arm_angle) + "  ||  NIL";
      } 
      else if (sub_mode == 2) //Setting top angle screen
      {
        excercise_text = "   Use up/down to";
        speed_text = "  Change UP angle";
        reps_text = "   Enter to Save";
        angle_text = "     " + String((int)first_angle) + "  ||  " + String((int)current_arm_angle);
      } 
      else if (sub_mode == 3) //Invalid angle screen
      {
        excercise_text = " ";
        speed_text = "   Invalid angle!";
        reps_text = "     Try again!";
        angle_text = " ";
      }
    }
  }

  else if (editing_mode == 2) //Setting screen
  {
    if (mode == 2) 
    {
      if (sub_mode == 1)      //Setting speed screen
      {
        excercise_text = "Enter to set Speed";
        speed_text = "> Speed: " + String(speed);
        reps_text = "REPS: " + String(reps_total);
        angle_text = "Angle: " + String(second_angle);
      }
    } 
    else if (mode == 3) 
    {
      if (sub_mode == 1)      //Setting reps screen
      {
        excercise_text = "Enter to set REPS";
        speed_text = "Speed: " + String(speed);
        reps_text = "> REPS: " + String(reps_total);
        angle_text = "Angle: " + String(second_angle);
      }
    } 
  }  
  
  lcd.init();
  lcd.setCursor(0, 0);
  lcd.print(excercise_text);
  lcd.setCursor(0, 1);
  lcd.print(speed_text);
  lcd.setCursor(0, 2);
  lcd.print(reps_text);
  lcd.setCursor(0, 3);
  lcd.print(angle_text);
}

//Checks if enter button is released from being pressed
int enter_button_toggle() 
{
  enter_btn_new_state = digitalRead(enter_Button_Pin);

  if (enter_btn_old_state == 1 && enter_btn_new_state == 0) 
  {
    enter_btn_old_state = enter_btn_new_state;
    sound_buzzer();
    return 1;
  } 
  else 
  {
    enter_btn_old_state = enter_btn_new_state;
    return 0;
  }
}

//Checks if up button is released from being pressed
int up_button_toggle() 
{
  up_btn_new_state = digitalRead(up_Button_Pin);

  if (up_btn_old_state == 1 && up_btn_new_state == 0) 
  {
    up_btn_old_state = up_btn_new_state;
    sound_buzzer();
    return 1;
  } 
  else 
  {
    up_btn_old_state = up_btn_new_state;
    return 0;
  }
}

//Checks if down button is released from being pressed
int down_button_toggle() 
{
  down_btn_new_state = digitalRead(down_Button_Pin);

  if (down_btn_old_state == 1 && down_btn_new_state == 0) 
  {
    down_btn_old_state = down_btn_new_state;
    sound_buzzer();
    return 1;
  } 
  else 
  {
    down_btn_old_state = down_btn_new_state;
    return 0;
  }
}

//Checks if the up button is pressed and held
int up_button_press() 
{
  up_btn_new_state = digitalRead(up_Button_Pin);

  if (up_btn_old_state == 1 && up_btn_new_state == 1) 
  {
    up_btn_old_state = up_btn_new_state;
    return 1;
  } 
  else 
  {
    up_btn_old_state = up_btn_new_state;
    return 0;
  }
}

//Checks if the down button is pressed and held
int down_button_press() 
{
  down_btn_new_state = digitalRead(down_Button_Pin);

  if (down_btn_old_state == 1 && down_btn_new_state == 1) 
  {
    down_btn_old_state = down_btn_new_state;
    return 1;
  } 
  else 
  {
    down_btn_old_state = down_btn_new_state;
    return 0;
  }
}

//Sounds buzzer and light when button is pressed
void sound_buzzer() 
{
  digitalWrite(buzzer_Pin, 1);
  delay(100);
  digitalWrite(buzzer_Pin, 0);
  digitalWrite(led_Pin, HIGH);
  delay(500);
  digitalWrite(led_Pin, LOW);
}

//Sounds buzzer and flashes light at the beginning of the exercise
void sound_begin_excercise_buzzer()
{
  delay(1000);
  sound_buzzer();
  delay(500);
  sound_buzzer();
  delay(250);
  sound_buzzer();
  delay(250);
}

//Sounds buzzer and flashes light at the end of the exercise
void sound_end_excercise_buzzer() 
{
  delay(1000);
  sound_buzzer();
  delay(500);
  sound_buzzer();
  delay(250);
  sound_buzzer();
  delay(250);
  sound_buzzer();
  delay(500);
  sound_buzzer();
  delay(1000);
  sound_buzzer();
  delay(500);
  sound_buzzer();
  delay(500);
}