#include <Servo.h> // include the Servo library

Servo shoulder ; // ไหล่
Servo arm ; // แขน
Servo wrist ; // ข้อมือ
Servo hand ; // มือ

//ขาservo
int servo0_pin = 10 ; //ไหล่
int servo1_pin = 11 ; //แขน
int servo2_pin = 12 ; //ข้อมือ
int servo3_pin = 13 ; //มือ

//ขาbotton
int botton0_pin = 0 ; // ปุ่มเลือกแขนเป้าหมาย
int botton1_pin = 1 ; // ปุ่มเพิ่มองศา
int botton2_pin = 2 ; // ปุ่มลดองศา
int botton3_pin = 3 ; // ปุ่มกางมือ
int botton4_pin = 4 ; // ปุ่มหุบมือ
int botton5_pin = 5 ; // ปุ่มเลียนแบบ

//ขาLED
int LED0_pin = 6 ; // servo0
int LED1_pin = 7 ; // servo1
int LED2_pin = 8 ; // servo2
int LED3_pin = 9 ; // Copy

int servoname = 0 ; // เลขประจำ servo เป้าหมาย

int pos0 = 90 ; //ไหล่
int pos1 = 90 ; //แขน
int pos2 = 90 ; //ข้อแขน
int pos3 = 90 ; //มือ

int pos0p = pos0 ; //ไหล่
int pos1p = pos1 ; //แขน
int pos2p = pos2 ; //ข้อแขน
int pos3p = pos3 ; //มือ

int cppos0 ; //ไหล่
int cppos1 ; //แขน
int cppos2 ; //ข้อแขน
int cppos3 ; //มือ

int cppos0p ; //ไหล่
int cppos1p ; //แขน
int cppos2p ; //ข้อแขน
int cppos3p ; //มือ

bool step1 ;
bool step2 ;

bool work ;

void setup() {
  // put your setup code here, to run once:

  Serial.begin(9600); // 

  //กำหนดสถนะ pin

  pinMode(botton0_pin, INPUT_PULLUP);
  pinMode(botton1_pin, INPUT_PULLUP);
  pinMode(botton2_pin, INPUT_PULLUP);
  pinMode(botton3_pin, INPUT_PULLUP);
  pinMode(botton4_pin, INPUT_PULLUP);
  pinMode(botton5_pin, INPUT_PULLUP);

  pinMode(LED0_pin, OUTPUT);
  pinMode(LED2_pin, OUTPUT);
  pinMode(LED1_pin, OUTPUT);
  pinMode(LED3_pin, OUTPUT);

  shoulder.attach(servo0_pin);
  arm.attach(servo1_pin);
  wrist.attach(servo2_pin);
  hand.attach(servo3_pin);

  step1 = true ;
  step2 = false ;
  work = false ;

  

}

void loop() {
  // put your main code here, to run repeatedly:
  Serial.print("servo : ");
  Serial.println(servoname);
  Serial.print("pos0 : ");
  Serial.println(pos0);
  Serial.print("pos1 : ");
  Serial.println(pos1);
  Serial.print("pos2 : ");
  Serial.println(pos2);
  
  //บังคับ Hand
  if(digitalRead(botton3_pin) == 0) {
      delay(200);
      pos3 = 120 ;
      if(pos3 >= 180){
      pos3 = 180 ;
      }
      step1 = false ;
      step2 = true ;
    }
    
    else if(digitalRead(botton4_pin) == 0) {
      delay(200);
      pos3 = pos3 - 1 ;
      if(pos3 <= 90){
      pos3 = 90 ;
      }
    }
  
if(step1 == true){
  shoulder.write(pos0);
  arm.write(pos1);
  wrist.write(pos2);
  hand.write(pos3);
  
  delay(200);

  //ปุ่มเลือกแขน
  if(digitalRead(botton0_pin) == 0){
    delay(200);
    while(digitalRead(botton0_pin) == 0){}
    servoname++ ;
    if( servoname > 2 ) {
      servoname = 0 ;
    }
  }

  //บังคับ shoulder
  if(servoname == 0 ){
    delay(100);

    digitalWrite(LED0_pin , 1 );
    digitalWrite(LED1_pin , 0 );
    digitalWrite(LED2_pin , 0 );
    digitalWrite(LED3_pin , 0 );

    if(digitalRead(botton1_pin) == 0) {
      delay(200);
      pos0 = pos0 + 1 ;
      if(pos0 >= 180){
      pos0 = 180 ;
      }
    }
    
    else if(digitalRead(botton2_pin) == 0) {
      delay(200);
      pos0 = pos0 - 1 ;
      if(pos0 <= 0){
      pos0 = 0 ;
      }
    }
  }

  //บังคับ arm
  if(servoname == 1 ){
    delay(100);

    digitalWrite(LED0_pin , 0 );
    digitalWrite(LED1_pin , 1 );
    digitalWrite(LED2_pin , 0 );
    digitalWrite(LED3_pin , 0 );

    if(digitalRead(botton1_pin) == 0) {
      delay(200);
      pos1 = pos1 + 1 ;
      if(pos1 >= 180){
      pos1 = 180 ;
      }
    }
    
    else if(digitalRead(botton2_pin) == 0) {
      delay(200);
      pos1 = pos1 - 1 ;
      if(pos1 <= 0){
      pos1 = 0 ;
      }
    }
  }

  //บังคับ wrist
  if(servoname == 2 ){
    delay(100);

    digitalWrite(LED0_pin , 0 );
    digitalWrite(LED1_pin , 0 );
    digitalWrite(LED2_pin , 1 );
    digitalWrite(LED3_pin , 0 );

    if(digitalRead(botton1_pin) == 0) {
      delay(200);
      pos2 = pos2 + 1 ;
      if(pos2 >= 180){
      pos2 = 180 ;
      }
    }
    
    else if(digitalRead(botton2_pin) == 0) {
      delay(200);
      pos2 = pos2 - 1 ;
      if(pos2 <= 0){
      pos2 = 0 ;
      }
    }
  }
  
}

if(step2 == true){
  delay(500);
  shoulder.write(pos0p) ;
  delay(500);
  arm.write(pos1p);
  delay(500);
  wrist.write(pos2p);
  delay(500);
  hand.write(pos3);
  delay(500);
  
  delay(200);

  //ปุ่มเลือกแขน
  if(digitalRead(botton0_pin) == 0){
    delay(200);
    while(digitalRead(botton0_pin) == 0){}
    servoname++ ;
    if( servoname > 2 ) {
      servoname = 0 ;
    }
  }

  //บังคับ shoulder
  if(servoname == 0 ){
    delay(100);

    digitalWrite(LED0_pin , 1 );
    digitalWrite(LED1_pin , 0 );
    digitalWrite(LED2_pin , 0 );
    digitalWrite(LED3_pin , 0 );

    if(digitalRead(botton1_pin) == 0) {
      delay(200);
      pos0p = pos0p + 1 ;
      if(pos0p >= 180){
      pos0p = 180 ;
      }
    }
    
    else if(digitalRead(botton2_pin) == 0) {
      delay(200);
      pos0p = pos0p - 1 ;
      if(pos0p <= 0){
      pos0p = 0 ;
      }
    }
  }

  //บังคับ arm
  if(servoname == 1 ){
    delay(100);

    digitalWrite(LED0_pin , 0 );
    digitalWrite(LED1_pin , 1 );
    digitalWrite(LED2_pin , 0 );
    digitalWrite(LED3_pin , 0 );

    if(digitalRead(botton1_pin) == 0) {
      delay(200);
      pos1p = pos1p + 1 ;
      if(pos1p >= 180){
      pos1p = 180 ;
      }
    }
    
    else if(digitalRead(botton2_pin) == 0) {
      delay(200);
      pos1p = pos1p - 1 ;
      if(pos1p <= 0){
      pos1p = 0 ;
      }
    }
  }

  //บังคับ wrist
  if(servoname == 2 ){
    delay(100);

    digitalWrite(LED0_pin , 0 );
    digitalWrite(LED1_pin , 0 );
    digitalWrite(LED2_pin , 1 );
    digitalWrite(LED3_pin , 0 );

    if(digitalRead(botton1_pin) == 0) {
      delay(200);
      pos2p = pos2p + 1 ;
      if(pos2p >= 180){
      pos2p = 180 ;
      }
    }
    
    else if(digitalRead(botton2_pin) == 0) {
      delay(200);
      pos2p = pos2p - 1 ;
      if(pos2p <= 0){
      pos2p = 0 ;
      }
    }
  }
  
}

  cppos0 = pos0 ;
  cppos1 = pos1 ;
  cppos2 = pos2 ;
  cppos3 = pos3 ;

  cppos0p = pos0p ;
  cppos1p = pos1p ;
  cppos2p = pos2p ;

  //ระบบออโต้
  if(digitalRead(botton5_pin) == 0){
    delay(200);
    while(digitalRead(botton5_pin) == 0){}
    work = true ;
    step1 = false ;
    step2 = false ;

  }

  if(work == true){

    digitalWrite(LED0_pin , 0 );
    digitalWrite(LED1_pin , 0 );
    digitalWrite(LED2_pin , 0 );
    digitalWrite(LED3_pin , 1 );

    shoulder.write(90);
    arm.write(90);
    wrist.write(90);
    hand.write(90);
    delay(500);
    shoulder.write(cppos0);
    delay(500);
    arm.write(cppos1);
    delay(500);
    wrist.write(cppos2);
    delay(500);
    hand.write(120);
    delay(500);
    hand.write(cppos3);
    delay(500);
    wrist.write(cppos2p);
    delay(500);
    arm.write(cppos1p);
    delay(500);
    shoulder.write(cppos0p);
    delay(500);

  }

}
//แก้ stpe1 step2 ทำงานร่วมกันให้ได้