#define VERT_PIN A0
#define HORZ_PIN A1
#define SEL_PIN  2
//#define JOINT_SERVO 10
//#define BASE_SERVO 9

#include <Servo.h>
#include <Keypad.h>

const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = { //keypad initialization
  { '1', '2', '3', 'J' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'E' },
  { 'M', '0', '=', 'S' }
};

uint8_t colPins[COLS] = { 4, 3, 7, 6 }; // Pins connected to C1, C2, C3, C4
uint8_t rowPins[ROWS] = { 13, 12, 11, 8 }; // Pins connected to R1, R2, R3, R4

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS); //create instance of keypad class


Servo JOINT_SERVO; //servo for arm joint
Servo BASE_SERVO; // servo for base rotation

int JOINT_POS = 0;    // variable to store the servo position
int BASE_POS = 0;    // variable to store the servo position
String ROTATION = " ";  //string for rotation value concatination in mode loop
int INSTANTROT = 0; // instant rotation value in mode loop
char MODE = ' '; //stores mode character B for base J for joint
int curr = 0; 

void setup() {
  Serial.begin(9600);
  JOINT_SERVO.attach(10); //attach servo to pin 10
  BASE_SERVO.attach(9); //attach servo to pin 9
  
  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);
}

void loop() {
  char key = keypad.getKey(); //character input from keypad
  int vert = analogRead(VERT_PIN); //vertical value for rotation
  int horz = analogRead(HORZ_PIN); //horizontal value for rotation
  bool selPressed = digitalRead(SEL_PIN) == LOW; 

  
    if((JOINT_POS > 0)&&(horz == 512)&&(vert == 0)){ //change statement numbers to be mapped to joystick values
      --JOINT_POS;//decrements joint position value when joystick is straight down
      delay(11); 
    }

    if((JOINT_POS < 180)&&(horz == 512)&&(vert == 1023)){
      ++JOINT_POS; //increments joint position value when stick is straight up
      delay(11);
    }

    if((BASE_POS > 0)&&(horz == 1023)&&(vert == 512)){ //change statement numbers to be mapped to joystick values
      --BASE_POS; //decrements base position value when stick is straight left
      delay(11); 
    }

    if((BASE_POS < 180)&&(horz == 0)&&(vert == 512)){
      ++BASE_POS; //increments base position value when stick is straight right
      delay(11);
    }

    if (key == 'M') { //enters mode select when m is pressed

    //create condition for combined mode where you can do both at once

      while( (key != 'B') && (key != 'J') ){ //selects J for joint rotate or B for base rotate
        key = keypad.getKey(); // get key value
        MODE = key; //set key to mode select J or B
        
      }

      while(key != '='){ //equals breaks condition and sends value to correct position loop

        key = keypad.getKey(); //get key value
        if(key != NO_KEY ){ //if key is pressed concatenate the number string for position value
        ROTATION += key; 
        }
      
      }

      INSTANTROT = ROTATION.toInt(); //convert number string to integer value

      if(MODE == 'J'){ //sets the current loop starter to current position value based on mode
        curr = JOINT_POS; 
      } else if(MODE == 'B'){
        curr = BASE_POS; 
      }

     if (curr < INSTANTROT) { // Check if going clockwise or counterclockwise
        for (int i = curr; i < INSTANTROT; ++i) {
            if (MODE == 'J') { // if mode is set to J 
                JOINT_SERVO.write(i); // write to joint servo in slow increments for speed purposes
                JOINT_POS = i; // change joint position value
            } else if (MODE == 'B') { // if mode is set to B
                BASE_SERVO.write(i); // write to base servo in slow increments for speed purposes
                BASE_POS = i; // change base position value
            } else {
                break; // if not in valid mode break into normal functioning joystick control
            }
            delay(11); 
        }
    } else { // Handle the condition where curr > INSTANTROT (going counterclockwise)
        for (int i = curr; i > INSTANTROT; --i) {
            if (MODE == 'J') { // if mode is set to J 
                JOINT_SERVO.write(i); // write to joint servo in slow decrements for speed purposes
                JOINT_POS = i; // change joint position value
            } else if (MODE == 'B') { // if mode is set to B
                BASE_SERVO.write(i); // write to base servo in slow decrements for speed purposes
                BASE_POS = i; // change base position value
            } else {
                break; // if not in valid mode break into normal functioning joystick control
            }
            delay(11); 
        }
    }
} else { // Handle the condition where key != 'M'
    JOINT_SERVO.write(JOINT_POS); //write current position to joint servo
    BASE_SERVO.write(BASE_POS); //write current position to base servo
}
    

    JOINT_SERVO.write(JOINT_POS); //write current position to joint servo
    BASE_SERVO.write(BASE_POS); //write current position to base servo

    ROTATION = " "; //reset ROTATION var
    INSTANTROT = 0;  //reset INTANTROT var
    MODE = ' ';  //reset mode to normal function
    curr = 0; 
  



 


  
}