#include <Servo.h> // Use Servo library, included with IDE
#include <EEPROM.h>

#define apin A4
#define bpin A5
#define cpin A6
#define dpin A7

int oy = 0;
int oa = 0;
int va = 0;
int vy = 0;
int x = 100;

int oyo = 0;
int oao = 0;
int vao = 0;
int vyo = 0;

int i;
int j;
int k;
int l;
int a;
int b;
    
    Servo servo;
    Servo servo2; 
    




void setup() {
    Serial.begin(9600);

    pinMode(A1 , INPUT);

    
    Serial.println(servo2.read());
    
    servo.attach(2);
    servo2.attach(3); 

    Serial.println(servo2.read());
    
    j = EEPROM.read(a);
    i = EEPROM.read(b);

    servo2.write(j);
    servo.write(i);

    Serial.println(servo2.read());
    }

void loop() {

    va = analogRead(apin);
    oa = analogRead(bpin);
    oy = analogRead(cpin);
    vy = analogRead(dpin);
    

    oyo = map(oy, 0, 1000, 0, x);
    oao = map(oa, 0, 1000, 0, x);
    vao = map(va, 0, 1000, 0, x);
    vyo = map(vy, 0, 1000, 0, x);

    
    Serial.print(oyo);
    Serial.print(" ");
    Serial.print(oao);
    Serial.print(" ");
    Serial.print(vao);
    Serial.print(" ");
    Serial.println(vyo); 
    
    
    //horizontal
    if (vao + vyo <= oao + oyo && i <= 180){
      if(j<=57){i--;}else{i++;}
      //Serial.println("hor+");
    }else if (vao + vyo >= oao + oyo && i >= 0){
      if(j<=57){i++;}else{i--;} 
      
      //Serial.println("hor-");
    }

    //vertical
    if (oyo + vyo <= oao + vao && j <= 150){
      j++;
      //Serial.println("ver+");
    }else if (oyo + vyo >= oao + vao && j >= 0){
      j--;
      //Serial.println("ver-");
    }

    if(vao == oyo && vyo == oao){
      delay(10000);
    }

    if(i >= 180){
      i = 30;
      j = 150 - j;
      delay(10);
      k=1;
      //Serial.println("vasen");
    }

    if(i <= 0){
      i = 150;
      j = 150 - j;
      delay(10);
      k=1;
      //Serial.println("oikea");
    }
    
    servo2.write(j);
    servo.write(i);

    if(k == 1){
      delay(2000);
      k=0; 
    }
   
    delay(500);
/*
    Serial.print(servo2.read());
    Serial.print(" ");
    Serial.println(servo.read());
    */
    EEPROM.write(a, j);
    EEPROM.write(b, i);

    
    }