#include <Servo.h>
#define VERT_PIN A0
#define HORZ_PIN A1
#define SEL_PIN 12 // cant get working
#define light A2 // define input pin
#define L_PIN 6
#define R_PIN 7
Servo x_servo;  // base servo
Servo y_servo;  // second servo on top
Servo door;
int mode = 0; // mode 0 manual, mode 1 auto
int x_servo_pos = 90; // initial x position for servo
int move_left_pin;
int move_right_pin;

void setup() {
  x_servo.attach(9);  // attaches the servo on pin 9 to the servo object
  y_servo.attach(10);
  door.attach(11);
  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(L_PIN, INPUT); // gpio inputs from the jetson for controlling camera
  pinMode(R_PIN, INPUT); // gpio inputs from 
  pinMode(SEL_PIN, INPUT_PULLUP);
  Serial.begin(19200);
}


void loop() {
  int vert = map(analogRead(VERT_PIN), 0, 1023, 0, 180);
  int horz = map(analogRead(HORZ_PIN), 0, 1023, 0, 180);
  int Lvalue = analogRead(light);// read the light
  bool selPressed = digitalRead(SEL_PIN) == LOW;
  int select_button = digitalRead(SEL_PIN);
  if (select_button  == 0) {
    mode = 1; // switch to auto mode
  }

  // Serial.print(Lvalue);Serial.print(Lvalue);
  // Serial.println();

  if (mode == 0) {
    x_servo.write(horz);              // tell servo to go to position in variable 'pos'
    y_servo.write(vert);
    if (Lvalue <= 90) {
      door.write(0);
    } else {
      door.write(180);
    }
  }
  
  if (mode == 1) {
    move_left_pin = digitalRead(L_PIN);
    move_right_pin = digitalRead(R_PIN);
    Serial.print(move_left_pin);
    if (move_left_pin == HIGH) {
      x_servo_pos -= 1;
    } else if (move_right_pin == HIGH) {
      x_servo_pos += 1;}
  }
  delay(100);
}