#define TRIG_PIN 13
#define ECHO_PIN 12
#define X_PIN A1
#define Y_PIN A0
#define SEL_PIN  2

#include <Servo.h>

Servo servo;
long duration, distance;
int x_val, y_val, sel_val;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  servo.attach(9);
  servo.write(90);
  pinMode(SEL_PIN, INPUT);
  digitalWrite(SEL_PIN, HIGH);
}

bool checkDistance() {
  //Signala izsutisana no Trig pin
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  //Atbildes impulsa salasisana no Sensora
  duration = pulseIn(ECHO_PIN, HIGH);

  //Attaluma noteiksana
  //Izmantojam skanas atrumu 343 metri sekunde
  distance = (duration / 2) * 0.0343;

  Serial.println(distance);
  return distance < 50;
}

void readJoystick() {
  sel_val = digitalRead(SEL_PIN);
  x_val = analogRead(X_PIN);
  y_val = analogRead(Y_PIN);
  Serial.print("Switch:  ");
  Serial.print(sel_val);
  Serial.print(" | ");
  Serial.print("X-axis: ");
  Serial.print(x_val);
  Serial.print(" | ");
  Serial.print("Y-axis: ");
  Serial.print(y_val);
  Serial.println(" | ");

}

void loop() {
  /*
  if (checkDistance()) {
    servo.write(180);
  } else {
    servo.write(0);
  }
  */
  
  readJoystick();
  if (x_val > 512) {
    servo.write(0);
  } else if (x_val < 512) {
    servo.write(180);
  }

  if (!sel_val) {
    servo.write(90);
  }
  delay(200);
}