// defining libraries
#include <Servo.h>
#include <LiquidCrystal_I2C.h>

// initializing components
  Servo f_servo;
  LiquidCrystal_I2C lcd(0x27, 20, 4);

// defining pins
const int 
  // joystick pins
  ver_pin = A1,
  hor_pin = A0,
  // motor pins
  p_step = 3,
  y_step = 5,
  p_dir = 2,
  y_dir = 4,
  // speed pot pin
  speed_pin = A2,
  // focus pot pin
  focus_pin = A3,
  // limit led pins
  y_led = 7,
  p_led = 6,
  // pitch limit switches
  pl_pin = 10,
  ph_pin = 12,
  // yaw limit switches
  yl_pin = 9,
  yr_pin = 8
;

// universal variable - millis()
unsigned long int prev_time = millis(); 

// universal variable - current position of pitch and yaw in degrees
float p_pos = 0,
y_pos = 0
;

void setup(){
  Serial.begin(9600);
  // defining pin modes (Inputs)
  pinMode(pl_pin, INPUT_PULLUP);
  pinMode(ph_pin, INPUT_PULLUP);
  pinMode(yl_pin, INPUT_PULLUP);
  pinMode(yr_pin, INPUT_PULLUP);
  pinMode(ver_pin, INPUT);
  pinMode(hor_pin, INPUT);
  pinMode(speed_pin, INPUT);
  // defining pin modes (Outputs)
  pinMode(p_step, OUTPUT);
  pinMode(y_step, OUTPUT);
  pinMode(p_dir, OUTPUT);
  pinMode(y_dir, OUTPUT);
  pinMode(y_led, OUTPUT);
  pinMode(p_led, OUTPUT);
  // Servo pin attachment
  f_servo.attach(11);
  // lcd setup
  lcd.init();
  lcd.backlight();
}

void loop(){
// input from speed pot
  int speed = map(analogRead(speed_pin), 0, 1023, 10, 100);

// focus motor motion and focus pot input
  int focus = map(analogRead(focus_pin), 0, 1023, 0, 180);
  f_servo.write(focus);

// input from joystick
  int ver = map(analogRead(ver_pin), 0, 1023, -1, 1);
  int hor = map(analogRead(hor_pin), 0, 1023, 1, -1);

// motor limit status
  //pitch
  if(!digitalRead(pl_pin) || !digitalRead(ph_pin)){
    digitalWrite(p_led, HIGH);
  }
  else{
    digitalWrite(p_led, LOW);
  }
    // yaw 
  if(!digitalRead(yl_pin) || !digitalRead(yr_pin)){
    digitalWrite(y_led, HIGH);
  }
  else{
    digitalWrite(y_led, LOW);
  }

// motion of pitch motor
  if(ver != 0){
    // clockwise 
    if(ver > 0 && digitalRead(ph_pin)){
      p_pos += 0.1125;
      digitalWrite(p_dir, HIGH);
      digitalWrite(p_step, HIGH);
      delay(speed);
      digitalWrite(p_step, LOW);
      delay(speed);
    }
    // counter clockwise 
    else if(ver < 0 && digitalRead(pl_pin)){
      p_pos -= 0.1125;
      digitalWrite(p_dir, LOW);
      digitalWrite(p_step, HIGH);
      delay(speed);
      digitalWrite(p_step, LOW);
      delay(speed);
    }
  }

// motion of yaw motor
  if(hor != 0){
    // clockwise 
    if(hor > 0 && digitalRead(yr_pin)){
      y_pos += 0.1125;
      digitalWrite(y_dir, HIGH);
      digitalWrite(y_step, HIGH);
      delay(speed);
      digitalWrite(y_step, LOW);
      delay(speed);
    }
    // counter clockwise 
    else if(hor < 0 && digitalRead(yl_pin)){
      y_pos -= 0.1125;
      digitalWrite(y_dir, LOW);
      digitalWrite(y_step, HIGH);
      delay(speed);
      digitalWrite(y_step, LOW);
      delay(speed);
    }
  }

// lcd display
  unsigned long int current_time = millis();
  if(current_time - prev_time >= 750){
    prev_time = current_time;
    char buffer[20];
    lcd.clear();
  // pitch data
    lcd.setCursor(0,0);
    lcd.print("Pitch = ");lcd.print(p_pos);
  // Yaw data
    lcd.setCursor(0,1);
    lcd.print("Yaw = ");lcd.print(y_pos);
  // Speed data
    lcd.setCursor(0,2);
    sprintf(buffer, "Delay = %d", speed);
    lcd.print(buffer);
  // Focus data
    lcd.setCursor(0,3);
    sprintf(buffer, "Focus = %d", focus);
    lcd.print(buffer);
  }
}