// Libraries included
#include <Braccio.h>
#include <Servo.h>
#include <LiquidCrystal.h>

// Arduino Inputs
LiquidCrystal LCD(13, 12, A3, A2, A1, A0);             // Create LCD Object (Uses analog output as an digital input/output
Servo base;                                            // Braccio Base Servo                                                
Servo shoulder;                                        // Braccio Shoulder Servo
Servo elbow;                                           // Braccio Elbow Servo
Servo wrist_ver;                                       // Braccio Wrist Vertical Servo
Servo wrist_rot;                                       // Braccio Wrist Rotation Servo
Servo gripper;                                         // Braccio Gripper Servo
const int buttons[2] = {7, 8};                         // Buttons Array [Next Program, Run/Stop Program]

// Button bool variables
bool buttonRedClicked = false;                         // Is Red button (Next Program / STOP-UP Program) clicked?
bool buttonBlueClicked = false;                        // Is Blue button (Start/Stop Program) clicked?

// Program variables
int     programs = 1;                                  // Programs variables used for choosing of the programs
bool    programRun = false;                            // Wether a program is running
String  programName = "Pickup Sponge";                 // Program name
bool    stopUp = false;                                // Used to check if STOP-UP function is used
int programStep = 0;

// This function runs ones everytime Arduino starts/resets
void setup() {
  // Setup
  Serial.begin(9600);                                  // Start communication between Aduino and PC
  Serial.println("Program starting...");               // Print to console
  Braccio.begin();                                     // Setup Braccio Begin
  LCD.begin(16, 2);                                    // Start LCD
  pinMode(buttons[0], INPUT_PULLUP);                   // Set button to input_pullup
  pinMode(buttons[1], INPUT_PULLUP);                   // Set button to input_pullup
  
  // Start program
  LCDPrintProgramStart();                              // Update info on LCD
  Braccio.ServoMovement(30, 0, 95, 90, 90, 90, 50);    // Move all servos used with Braccio
  delay(3000);                                         // Wait 3s before main game loop
  Serial.println("Program running!");                  // Print to console
  LCDPrintProgramChooser();                            // Update info on LCD 
}

void loop() {
  // If RED button is pressed choose next program
  if (!digitalRead(buttons[0]) && !buttonRedClicked && !programRun){
    Serial.println("Clicked NEXT");     // Print info about button clicked

    programs++;                         // increase var programs
    if (programs >= 5) programs = 1;    // set var programs to 1

    // Change the program name according to variable programs
    switch(programs){
      case 1: programName = "Pickup Sponge";  break;
      case 2: programName = "Pickup Bottle";  break;
      case 3: programName = "Pet the bottle"; break;
      case 4: programName = "Dancing Arm";    break;
    }

    LCDPrintProgramChooser();           // Update info on LCD
    buttonRedClicked = true;            // Set button to clicked
  }

  // If Red button is pressed while it's running, do the STOP-UP function
  // This sets the Braccio arm back in a upright position
  if (!digitalRead(buttons[0]) && !buttonRedClicked && programRun){
    Serial.println("Clicked STOP-UP");  // Print info about button clicked
    programRun = false;                 // Set program running to false
    stopUp = true;                      // Set STOP-UP to true
    buttonRedClicked = true;            // Set button to clicked
  }

  // If Blue button is pressed while it's NOT running, start program
  if (!digitalRead(buttons[1]) && !buttonBlueClicked && !programRun){
    Serial.println("Clicked START");    // Print info about button clicked
    programRun = true;                  // Set program running to true
    LCDPrintProgramRun();               // Update info on LCD
    buttonBlueClicked = true;           // Set button to clicked
  }

  // If Blue button is pressed while it's running, do the STOP function
  // This stops the braccio at the current position
  if (!digitalRead(buttons[1]) && !buttonBlueClicked && programRun){
    Serial.println("Clicked STOP");     // Print info about button clicked
    programRun = false;                 // Set program running to false
    LCDPrintProgramChooser();           // Update info on LCD
    buttonBlueClicked = true;           // Set button to clicked
  }

  // Reset button clicked if button is released
  if (digitalRead(buttons[0]))  buttonRedClicked = false;
  if (digitalRead(buttons[1]))  buttonBlueClicked = false;

  // Run the braccio robot arm programs
  if (programRun) programChooser();

  // Set braccio robot arm to upright position
  if (stopUp){
    Braccio.ServoMovement(30, 0, 95, 90, 90, 90, 50);
    LCD.clear();                    // Clear the LCD
    LCD.setCursor(0, 0);            // Set new cursor for pos of write
    LCD.print("Please wait...");    // Write at cursor pos
    delay(4000);                    // Wait 4 seconds
    LCDPrintProgramChooser();       // Update info on LCD
    stopUp = false;                 // Set STOP-UP to false
  }
}

// Run a braccio arm program
void programChooser() {
  switch(programs){
    case 1: programOne();   break;    // Run program one
    case 2: programTwo();   break;    // Run program two
    case 3: programThree(); break;    // Run program three
    case 4: programFour();  break;    // Run program four
  }
}

// Program Pickup Sponge
void programOne(){
  // Må fiksa ein måte å gå ud av koden på...
  // Siden braccio bruke step-delay
  // Lag ein button funksjon mellom kvert ledd av braccio servo movement...
  // Oppdater LCD med at den e Running og hvilket STEP den e i?

  programStep++;
  LCDPrintProgramRunSteps();
  Braccio.ServoMovement(20,   0,    45,   180,  180,  90,   10);
  delay(1000);
  programStep++;
  LCDPrintProgramRunSteps();
  Braccio.ServoMovement(20,   0,    90,   180,  180,  90,   10);
  programStep++;
  LCDPrintProgramRunSteps();
  Braccio.ServoMovement(10,   0,    45,   180,  180,  90,   60);
  programStep++;
  LCDPrintProgramRunSteps();
  Braccio.ServoMovement(20,   0,    45,   180,  45,   0,    60);
  programStep++;
  LCDPrintProgramRunSteps();
  Braccio.ServoMovement(20,   180,  45,   180,  45,   0,    60);
  programStep++;
  LCDPrintProgramRunSteps();
  Braccio.ServoMovement(20,   0,    90,   180,  180,  90,   60);
  programStep++;
  LCDPrintProgramRunSteps();
  Braccio.ServoMovement(20,   0,    90,   180,  180,  90,   10);

  programStep = 0;
}

// Program Pickup bottle
void programTwo(){
  // Writes the moves
}

// Program Pet the bottle
void programThree(){
  // Writes the moves
}

// Program Dancing Arm
void programFour(){
  // Writes the moves
}

// Update the LCD text to start of program
void LCDPrintProgramStart(){
  LCD.clear();                    // Clear the LCD
  LCD.setCursor(3, 0);            // Set new cursor for pos of write
  LCD.print("ROBOT ARM");         // Write at cursor pos
  LCD.setCursor(1, 1);            // Set new cursor for pos of write
  LCD.print("Choose Program");    // Write at cursor pos 
}

// Update the LCD text according to program choose 
void LCDPrintProgramChooser(){
  LCD.clear();                    // Clear the LCD
  LCD.setCursor(0, 0);            // Set new cursor for pos of write
  LCD.print(programName);         // Write at cursor pos
  LCD.setCursor(0, 1);            // Set new cursor for pos of write
  LCD.print("START     NEXT->");  // Write at cursor pos  
}

// Update the LCD text according to program running
void LCDPrintProgramRun(){
  LCD.clear();                    // Clear the LCD
  LCD.setCursor(0, 0);            // Set new cursor for pos of write
  LCD.print(programName);         // Write at cursor pos
  LCD.setCursor(0, 1);            // Set new cursor for pos of write
  LCD.print("STOP     STOP-UP");  // Write at cursor pos
}

// Update the LCD text according to program running
void LCDPrintProgramRunSteps(){
  LCD.clear();                    // Clear the LCD
  LCD.setCursor(0, 0);            // Set new cursor for pos of write
  LCD.print("STEP " + String(programStep) + "/7");         // Write at cursor pos
  LCD.setCursor(0, 1);            // Set new cursor for pos of write
  LCD.print("STOP     STOP-UP");  // Write at cursor pos
}