#include <Servo.h>
#include <Wire.h>
// #include <QMC5883L.h>

// QMC5883L compass;
Servo x_servo;
Servo y_servo;

int x_key = A3;
int y_key = A4;
int x_pos;
int y_pos;
int servo1_pin = 9;
int servoy_pin = 8;
int initial_position = 90;
int sw_key = 34;
int sw;
int speed;

void setup() {
  Wire.begin();
  // compass.init();

  Serial.begin(9600);
  x_servo.attach(servo1_pin);
  y_servo.attach(servoy_pin);
  x_servo.write(initial_position);
  y_servo.write(initial_position);
  pinMode(x_key, INPUT);
  pinMode(y_key, INPUT); 
}

void loop() {

  // int x,y,z;
  // compass.read(&x,&y,&z);

 // Calculate heading when the magnetometer is level, then correct for signs of axis.
  // Atan2() automatically check the correct formula taking care of the quadrant you are in
  // float heading = atan2(y, x);

  // float declinationAngle = 0.0404;
  // heading += declinationAngle;
  // Find yours here: http://www.magnetic-declination.com/

   // Correct for when signs are reversed.
  // if(heading < 0)
    // heading += 2*PI;

  // Check for wrap due to addition of declination.
  // if(heading > 2*PI)
  //   heading -= 2*PI;

  // // Convert radians to degrees for readability.
  // float headingDegrees = heading * 180/M_PI; 

  speed = 100;
  // sw = analogRead(sw_key);
  // if (sw == 0) {
  //   Serial.println("Hey hey, I am going to set servo to 90 pos");
  //   x_servo.write(90);
  // }

  x_pos = analogRead(x_key);
  y_pos = analogRead(y_key);
  if (x_pos < 450) {
    if (initial_position < 1) {
    } else {
      initial_position = initial_position - 1;
      x_servo.write(initial_position);
      speed = map(x_pos, 0, 449, 10, 100);
      // Serial.print("Am - ");
      // Serial.println(headingDegrees);
      delay(speed);
    }
  }
  if (x_pos > 550) {
    if (initial_position > 179) {

    } else {
      initial_position = initial_position + 1;
      speed = map(x_pos, 551, 1023, 100, 10);
      x_servo.write(initial_position);
      // Serial.print("Am - ");
      // Serial.println(headingDegrees);
      delay(speed);
    }
  }
    if (y_pos < 450) {
    if (initial_position < 1) {
    } else {
      initial_position = initial_position - 1;
      y_servo.write(initial_position);
      speed = map(y_pos, 0, 449, 10, 100);
      // Serial.print("Am - ");
      // Serial.println(headingDegrees);
      delay(speed);
    }
  }
  if (y_pos > 550) {
    if (initial_position > 179) {

    } else {
      initial_position = initial_position + 1;
      speed = map(y_pos, 551, 1023, 100, 10);
      y_servo.write(initial_position);
      // Serial.print("Am - ");
      // Serial.println(headingDegrees);
      delay(speed);
    }
  }
}