#include <Servo.h>

#define inputCLK 2
#define inputDT 3

#define baza_butt 4
#define trunchi_butt 5
#define rec_butt 6
#define run_butt 7

#define mem_poz_butt 11


Servo baza;
Servo trunchi;
//Servo brat;
//Servo gripp;


int counterBaza = 0;
int counterTrunchi = 0;
//int counterBrat = 0;
//int counterGrapp = 0;

int currentStateCLK;
int previousStateCLK;

int mem_var;

int baza_var;
int trunchi_var;

int rec_var;
int run_var;
int i=0;
int mem[100];

String encdir ="";

void setup() {

  pinMode(inputCLK,INPUT);
  pinMode(inputDT,INPUT);
  pinMode(baza_butt,INPUT);
  pinMode(trunchi_butt,INPUT);
  pinMode(run_butt,INPUT);

  pinMode(rec_butt,INPUT);
  pinMode(mem_poz_butt,INPUT);


  Serial.begin(9600);

  baza.attach(8);
  trunchi.attach(9);
  //brat.attach(10);
  //gripp.attach(11);

  previousStateCLK = digitalRead(inputCLK);

  
  }

void loop() {

    rec_var = digitalRead(rec_butt);
    run_var = digitalRead(run_butt);
    
    if(rec_var == 1)
    {i=0;
      record();
    }
    if(rec_var == 0 && run_var==1)
    {

      while(run_var == 1)
      {
        for(int a=0;a < i; a++)
        {
          if(1000<mem[a]<2000)
          {
            baza.write(mem[a]-1000);
            delay(100);
          }
          if(2000<mem[a]<3000)
          {
            trunchi.write(mem[a]-2000);
            delay(100);
          }

        }
          run_var = digitalRead(run_butt);
        }

    }

}


void record()
{
  
  previousStateCLK = digitalRead(inputCLK);
    while((rec_var == 1) && i<100 )
    {
      baza_var = digitalRead(baza_butt);
      
      if(baza_var == 1)
      {
         encoder(2);
         mem[i]=counterBaza+1000;

      
          i++;

             Serial.print("i===&&");
        Serial.println(i);
      }
      
      trunchi_var = digitalRead(trunchi_butt);
      
       if(trunchi_var == 1)
      {
         encoder(4);
         mem[i]=counterTrunchi+2000;
          i++;
      }
      rec_var = digitalRead(rec_butt);
        Serial.print("i====");
        Serial.println(i);
    }
    if(i==100)
    {
      while(rec_var == 1)
      {
          rec_var = digitalRead(rec_butt);
          delay(200);
      }
    }

}


int encoder(int ser)
{
  
     if(ser == 2){
       while(baza_var ==1)
       {
         baza_var = digitalRead(baza_butt);
     currentStateCLK = digitalRead(inputCLK);
        
   
        if (currentStateCLK != previousStateCLK)
          {
             
              if (digitalRead(inputDT) != currentStateCLK)
                    {
                      counterBaza --;
                        if (counterBaza<0)
                          {
                            counterBaza=0;
                          }
                    }
              else
                {
                  counterBaza ++;
                    if (counterBaza>180)
                      {
                        counterBaza=180;
                      }
                }
                
                 baza.write(counterBaza);
        Serial.print("Position baza: ");
        Serial.println(counterBaza);
          }
         

             previousStateCLK = currentStateCLK;
            }
            }
           

            if(ser==4){
               while(trunchi_var ==1)
       {
          trunchi_var = digitalRead(trunchi_butt);
              currentStateCLK = digitalRead(inputCLK);
        
        
        if (currentStateCLK != previousStateCLK)
          {
              if (digitalRead(inputDT) != currentStateCLK)
                    {
                      counterTrunchi --;
                        if (counterTrunchi<0)
                          {
                            counterTrunchi=0;
                          }
                    }
              else
                {
                  counterTrunchi ++;
                    if (counterTrunchi>180)
                      {
                        counterTrunchi=180;
                      }
                }
                 trunchi.write(counterTrunchi);
        Serial.print("Position trunchi: ");
        Serial.println(counterTrunchi);
          }
             previousStateCLK = currentStateCLK;
 
            }
            }


  

       
          
    
}