#include <Servo.h>
Servo myservo1, myservo2;
#define up 4
#define down 5

void servo_control_0_180_fd(int time,Servo myservo1, Servo myservo2);
void servo_control_180_0_bd( int time, Servo myservo1, Servo myservo2);
void servo_control_pos( int button, int time, Servo myservo1, Servo myservo2);
void servo_latching_push_button(int button, Servo myservo);



void setup() 
{
  myservo1.attach(2);
  myservo2.attach(3);
  pinMode(up, INPUT);
  pinMode(down, INPUT);

}

void loop()
{

//question
//latching push button, it will change the state of the motor when presses, one motor control
// servo_latching_push_button(up, myservo1);

servo_control_pos( up, 25,myservo1, myservo2);

  //continous press button, servo move, when button leave servo stop at that position (up button)
  // int button = digitalRead(up);
  // servo_control_pos(up, 100, myservo1, myservo2);

  //Question 
  // turn 0to180 via up button by pressing it ones, if continous push->continous 0to180
  //and vice versa in down button, 180to0,
  // if(digitalRead(up)==LOW)
  // {
  //   servo_control_0_180_fd( 15, myservo1, myservo2);
  // }
  
  // Question 
  // turn 180to0 via down button by pressing it ones, if continous push->continous 180to0
  // if(digitalRead(down)==LOW)
  //  {
  //   servo_control_180_0_bd(15, myservo1, myservo2);
    
  // }
  

}


void servo_latching_push_button(int button, Servo myservo)
{
  int prev_stat_motor = 0;
while(1)
{
  if(digitalRead(button)==LOW)
  {
    prev_stat_motor = ~prev_stat_motor;
    if(prev_stat_motor)
    {
    for(int pos = 0; pos<=180;pos++)
    {
    myservo.write(pos);
    delay(20);
  }
  for(int pos = 180;pos>=0;pos--)
  {
    myservo.write(pos);
    delay(20);
  }
}
}
}
}

void  servo_control_pos( int button, int time, Servo myservo1, Servo myservo2)
{
  int  pos = 0 ; 

for(pos; pos<=180; pos++)
		{
			if(digitalRead(button)==LOW)
			 {
        break;
        // fmid_pos = pos;
      // myservo1.write(pos);
				// myservo2.write(pos);
				delay(time);
        }
        else{
				myservo1.write(pos);myservo2.write(pos);
        delay(time);
			// 	// myservo2.write(pos);
			// 	// fmid_pos = pos;
			}
      // }
			// else
      //  {
				// myservo1.write(pos);
				// // myservo2.write(pos);
				// delay(time);
      }
		
    }
  

    // int bmid_pos = 180;
		// for(int pos = bmid_pos; pos>=0; pos--)
		// {
		// 	if(digitalRead(button)==LOW)
		// 	{
		// 		myservo1.write(pos);
		// 		myservo2.write(pos);
		// 		bmid_pos = pos;
		// 		break;

		// 	}
		// 	else {
		// 		myservo1.write(pos);
		// 		myservo2.write(pos);
		// 		delay(time);

		// 	}
		// }
	// }


void servo_control_0_180_fd(int time, Servo myservo1, Servo myservo2)
{
  for(int pos = 0 ; pos<=180; pos++)
  {
    myservo1.write(pos);
    myservo2.write(pos);
    delay(time);
  }
}

void servo_control_180_0_bd(int time, Servo myservo1, Servo myservo2)
{
  for(int pos = 180; pos>=0; pos--)
  {
    myservo1.write(pos);
    myservo2.write(pos);
    delay(time);
  }
}