//red-record
//blue-write
#define VERT_PIN A0
#define HORZ_PIN A1
#define SEL_PIN  2
#define VERT_PIN2 A2
#define HORZ_PIN2 A3
#define SEL_PIN2  4
#define SW1 5
#define SW2 6
#include <Servo.h>
Servo one;
Servo two;
Servo three;
Servo four;
Servo five;
const byte size_mass = 200; //max = 273
int readh1[size_mass];
int readh2[size_mass];
int readv1[size_mass];
int readv2[size_mass];
bool recording = false;
bool playing = false;
unsigned int time; //long?
int horz1_post = 90;
int horz2_post = 90;
int vert1_post = 90;
int vert2_post = 90;
void setup() {
  one.attach(3);
  two.attach(9);
  three.attach(10);
  four.attach(7);
  five.attach(11);
  pinMode(SW1, INPUT);
  pinMode(SW2, INPUT);
  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);
  pinMode(VERT_PIN2, INPUT);
  pinMode(HORZ_PIN2, INPUT);
  pinMode(SEL_PIN2, INPUT_PULLUP);
  Serial.begin(9600);
  for (int i = 0; i < size_mass; i++) {
    readh1[i] = 90;
    readh2[i] = 90;
    readv1[i] = 90;
    readv2[i] = 90;
  }
  time = millis();
}
//управление без записи и воспр.//////////////////////////////////////////////
void Joys_horz2(int horz2) {
  if (horz2>512){
    horz2_post++;
    if(horz2_post>180){
      horz2_post=180;
    }
  }
  if (horz2<512){
    horz2_post--;
    if(horz2_post<0){
      horz2_post=0;
    }
  }
  one.write(horz2_post);
}
void Joys_horz1(int horz1) {
  if (horz1>512){
    horz1_post++;
    if(horz1_post>180){
      horz1_post=180;
    }
  }
  if (horz1<512){
    horz1_post--;
    if(horz1_post<0){
      horz1_post=0;
    }
  }
  five.write(horz1_post);
}
void Joys_vert2(int vert2) {
  if (vert2>512){
    vert2_post++;
    if(vert2_post>180){
      vert2_post=180;
    }
  }
  if (vert2<512){
    vert2_post--;
    if(vert2_post<0){
      vert2_post=0;
    }
  }
  two.write(vert2_post);
  three.write(vert2_post);
}
void Joys_vert1(int vert1) {
  if (vert1>512){
    vert1_post++;
    if(vert1_post>180){
      vert1_post=180;
    }
  }
  if (vert1<512){
    vert1_post--;
    if(vert1_post<0){
      vert1_post=0;
    }
  }
  four.write(vert1_post);
}
///запись////////////////////////////////////
byte r = 0;
byte loop_for_write = 0;
void record (int horz1_post, int horz2_post, int vert1_post, int vert2_post) {
  if (millis() - time > size_mass){
    time = millis();
        readh1[r] = horz1_post;
        readh2[r] = horz2_post;
        readv1[r] = vert1_post;
        readv2[r] = vert2_post;
        r++;
        loop_for_write = r;
        if (r >= size_mass) {
        r = 0;
        recording = false; 
    }
    Serial.print("r: ");
    Serial.println(r);
  }
}
//воспроизведение/////////////////////////////////////////
byte w = 0;
void play() {
  if (playing && millis() - time > size_mass) {
    time = millis();
    int horz1_post = readh1[w];
    int horz2_post = readh2[w];
    int vert1_post = readv1[w];
    int vert2_post = readv2[w];
    five.write(horz1_post);
    one.write(horz2_post);
    two.write(vert2_post);
    three.write(vert2_post);
    four.write(vert1_post);
    delay(200);
    w++;
    if (w >= size_mass) {
      w = 0;
      playing = false;
    }
    if (w > loop_for_write) {
      w = 0;
    }
  }
  Serial.print("w: ");
  Serial.println(w);
}
void loop() {
  int vert1 = analogRead(VERT_PIN);
  int horz1 = analogRead(HORZ_PIN);
  int vert2 = analogRead(VERT_PIN2);
  int horz2 = analogRead(HORZ_PIN2);
  int selPressed1 = digitalRead(SEL_PIN);
  int selPressed2 = digitalRead(SEL_PIN2);
  int sw1 = digitalRead(SW1);
  int sw2 = digitalRead(SW2);
  Joys_horz1(horz1);
  Joys_horz2(horz2);
  Joys_vert2(vert2);
  Joys_vert1(vert1);
  /*Serial.print("sw1: ");
  Serial.print(sw1);
  Serial.print("  sw2: ");
  Serial.print(sw2);
  Serial.print("  recording: ");
  Serial.print(recording);
  Serial.print("  playing: ");
  Serial.println(playing);
  /*Serial.print(horz1_post);
  Serial.print(" ");
  Serial.print(horz2_post);
  Serial.print(" ");
  Serial.print(vert1_post);
  Serial.print(" ");
  Serial.println(vert2_post);*/
  if (selPressed1 == 0) {
    Serial.println("readh1");
    for (int i = 0; i < size_mass; i++) {
      Serial.print(readh1[i]);
      Serial.print(" ");
    }
    Serial.println();
    Serial.println("readh2");
    for (int i = 0; i < size_mass; i++) {
      Serial.print(readh2[i]);
      Serial.print(" ");
    }
    Serial.println();
    Serial.println("readv1");
    for (int i = 0; i < size_mass; i++) {
      Serial.print(readv1[i]);
      Serial.print(" ");
    }
    Serial.println();
    Serial.println("readv2");
    for (int i = 0; i < size_mass; i++) {
      Serial.print(readv2[i]);
      Serial.print(" ");
    }
    Serial.println();
   // delay(500);
  }
  if(recording){
    record(horz1_post, horz2_post, vert1_post, vert2_post);
  }
   if(playing){
    play();
  }
//перключатель записи - вкл.////////////////////////////////////
  if (sw1 == 1){
    recording = true;
    //erial.println("Recording...");
  }
//перключатель записи - выкл.////////////////////////////////////
  if (sw1 == 0){
    recording = false;
    r = 0;
    //Serial.println("Recording sdopeses");
  }
//перключатель воспроизведения - вкл.////////////////////////////////////
  if (sw2 == 1){
    playing = true;
    //Serial.println("Playing...");
  }
//перключатель воспроизведения - выкл.////////////////////////////////////
  if (sw2 == 0){
    playing = false;
    w = 0;
    //Serial.println("Playing sdopeses");
  }
}