#include <Servo.h>

#define VERT_PIN A2
#define HORZ_PIN A1
#define SEL_PIN  A0

// Пины для сервоприводов
const int servoPins[] = {8, 12, 10, 6, 7, 11, 9, 5};
// Серваки слева направо 
//[4 6]
//[0 2]
//[3 1]
//[7 5]    



// Количество сервоприводов
const int numServos = 8;

// Диапазон углов поворота сервопривода
const int servoMinAngle = 0;
const int servoMaxAngle = 180;

// Шаг поворота сервопривода
//const int servoStep = 20;

//Задержка между поворотом разных сервоприводов
//Мы не можем регулировать скорость вращения сервака, поэтому можем регулировать только задержку между их переключениями
//Это и будет наша условная скорость, чем больше, тем дольше ноги будут двигаться
const int s_delay = 50;

int c_angle_w = 0;
int speed = 10;
Servo servos[numServos];

void setup() {
  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);

  // Инициализация сервоприводов
  for (int i = 0; i < numServos; ++i) {
    servos[i].attach(servoPins[i]);
  }
}

void loop() {
int vert = analogRead(VERT_PIN);
int horz = analogRead(HORZ_PIN);
bool selPressed = digitalRead(SEL_PIN) == LOW;

int mappedVert = map(vert, 0, 1023, -100, 100);
int mappedHorz = map(horz, 0, 1023, -100, 100);

if (mappedVert > 50) {
// Стик вверх, идем вперед

  //servos[4].write(90);
  //servos[5].write(90);
  
  delay(s_delay);
  if(c_angle_w < 60){
  c_angle_w = c_angle_w + speed;
  servos[0].write(90+c_angle_w);
  servos[1].write(90-c_angle_w);
  delay(s_delay);
  servos[4].write(90-c_angle_w);
  servos[5].write(90+c_angle_w); 
  delay(s_delay);

  }

//delay(s_delay);
//servos[0].write(90);
//servos[1].write(90);
//servos[6].write(90);
//servos[7].write(90);
//delay(s_delay);
//servos[2].write(30);
//servos[3].write(150);
//delay(s_delay);
//servos[6].write(150);
//servos[7].write(30);
//delay(s_delay);
//servos[2].write(90);
//servos[3].write(90);

} else if (mappedVert < -50) {
// Стик вниз, идем назад
servos[4].write(90);
servos[5].write(90);
delay(s_delay);
servos[0].write(30);
servos[1].write(150);
delay(s_delay);
servos[4].write(30);
servos[5].write(150);
delay(s_delay);
servos[0].write(90);
servos[1].write(90);
servos[6].write(90);
servos[7].write(90);
delay(s_delay);
servos[2].write(150);
servos[3].write(30);
delay(s_delay);
servos[6].write(150);
servos[7].write(30);
delay(s_delay);
servos[2].write(90);
servos[3].write(90);
} else if (mappedHorz > 50) {
// Стик вправо, идем вправо
servos[4].write(90);
servos[5].write(90);
delay(s_delay);
servos[0].write(30);
servos[1].write(150);
delay(s_delay);
servos[4].write(30);
servos[5].write(150);
delay(s_delay);
servos[0].write(90 );
servos[1].write(90);
servos[6].write(90);
servos[7].write(90);
delay(s_delay);
servos[3].write(150);
servos[2].write(30);
delay(s_delay);
servos[6].write(150);
servos[7].write(30);
delay(s_delay);
servos[3].write(90);
servos[2].write(90);
} else if (mappedHorz < -50) {
// Стик вниз, идем вниз
servos[4].write(90);
servos[5].write(90);
delay(s_delay);
servos[0].write(150);
servos[1].write(30);
delay(s_delay);
servos[4].write(30);
servos[5].write(150);
delay(s_delay);
servos[0].write(90 );
servos[1].write(90);
servos[6].write(90);
servos[7].write(90);
delay(s_delay);
servos[3].write(30);
servos[2].write(150);
delay(s_delay);
servos[6].write(150);
servos[7].write(30);
delay(s_delay);
servos[3].write(90);
servos[2].write(90);
} 
else if (selPressed){
  c_angle_w = 0;
}
//else {
// Если стик по центру, мы все выпрямляем3
//for (int i = 0; i < numServos; ++i) {
//servos[i].write(90); // Assuming the center position is at 90 degrees
//}
//c_angle_w = 0;
//}

//delay(100); // Add delay to prevent rapid servo movements
}