#include <Servo.h>

Servo servo1;  // create servo object to control a servo
Servo servo2;
// twelve servo objects can be created on most boards

int sensorValue;
float voltage;
float voltageCheck;
int buttonState;
int buttonTimer;
int buttonCount;
int massageState;
bool buttonUp;

// Pin 13 has a LED connected on most Arduino boards.
int led = 13;

// digital pin 2 has a pushbutton attached to it. Give it a name:
int pushButtonDPin = 2;
bool PowerOn = false;

int pos1 = 0;    // variable to store the servo position
int pos2 = 180;

int rotationSpeed = 6;
int rotationSpeedMultiplier = 2;

//Reset Servo position bool
bool doOnce = true;

bool servo1Rotate0 = true;
bool servo1Rotate180 = false;
bool servo2Rotate0 = false;
bool servo2Rotate180 = true;

void setup() {
  massageState = 0;
  buttonCount = 0;
  buttonTimer = 100;
  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);
  // initialze the digital pin as an output.
  pinMode(led, OUTPUT);
  
  // make the pushbutton's pin an input:
  pinMode(pushButtonDPin, INPUT);

  servo1.attach(9);  // attaches the servo on pin 9 to the servo object
  servo2.attach(10);  // attaches the servo on pin 9 to the servo object
}

void loop() {
delay(6);
Power();
ButtonPressed();
  // read the input on analog pin 0:
  sensorValue = analogRead(A0);
  // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V):
  voltageCheck = (sensorValue * (5.0 / 1023.0)) /1.5;
  if(voltageCheck <= .55){
    voltage = .55;
  }
  else{
     voltage = voltageCheck; 
  }
rotationSpeed = voltage*rotationSpeedMultiplier;
if(PowerOn == true){
  if(massageState == 1){
            RotateCC();              
  }
  else if(massageState == 2){
       RotateOmAndOm();                
  }
  else{
      RotateInLoop(); 
}
  if(servo1Rotate0 == true){
      pos1 += rotationSpeed;  
  }
  else if(servo1Rotate180 == true){
     pos1 -= rotationSpeed;
  }
  if(servo2Rotate180 == true){
      pos2 -= rotationSpeed;
  }
  else if(servo2Rotate0 == true){
     pos2 += rotationSpeed;   
  }
  }
  servo1.write(pos1);
  servo2.write(pos2);
}

// FUNCTIONS
//--------------------------------------------------------------------------------------------------------
void ButtonPressed(){
  // read the input pin:
  buttonState = digitalRead(pushButtonDPin);
  //delay(1);        // delay in between reads for stability
}

void RotateInLoop(){
  if(doOnce == true){
    if(pos1 != 90 && pos2 != 90){
      pos1 = 90;
      pos2 = 90;
    }
    if(pos1 == 90 && pos2 == 90){
      doOnce = false;
    }
  }
  else{
  if(pos1 <= 0){
    servo1Rotate0 = true;
    servo1Rotate180 = false;
  }
  if(pos1 >= 180){
    servo1Rotate0 = false;
    servo1Rotate180 = true;
  }
    if(pos2 <= 0){
    servo2Rotate0 = true;
    servo2Rotate180 = false;
  }
    if(pos2 >= 180){
    servo2Rotate0 = false;
    servo2Rotate180 = true;
  }    
  }
}

void RotateCC(){
  if(doOnce == true){
    if(pos1 != 90 && pos2 != 90){
      pos1 = 90;
      pos2 = 90;
    }
    if(pos1 == 90 && pos2 == 90){
      doOnce = false;
    }
  }
  else{
  if(pos1 <= 0){
    servo1Rotate0 = true;
    servo1Rotate180 = false;
  }
  if(pos1 >= 180){
    servo1Rotate0 = false;
    servo1Rotate180 = true;
  }
    if(pos2 <= 0){
      servo2Rotate0 = true;
      servo2Rotate180 = false;
  }
    if(pos2 >= 180){
      servo2Rotate0 = false;
      servo2Rotate180 = true;
  }    
  }
}

void RotateOmAndOm(){
   //Rotate om&om
  if(pos1 >= 180){
    servo1Rotate0 = false;
    servo2Rotate180 = true;
  }
  if(pos2 <= 0){
    servo2Rotate180 = false;
    servo1Rotate180 = true;
  }
  if(pos1 <= 0){
    servo1Rotate180 = false;
    servo2Rotate0 = true;
  }
  if(pos2 >= 180){
    servo2Rotate0 = false;
    servo1Rotate0 = true;
  } 
}

//On And Off Script  
void Power(){
//Power On
if(PowerOn == false && buttonState == 1 && buttonUp == true){
  PowerOn = true;
  buttonUp = false;
}

if(PowerOn == true){
    digitalWrite(led, LOW);  
}
else{
  digitalWrite(led, HIGH);
  if(pos1 != 90){
   pos1 = 90;
  }
  if(pos2 != 90){
  pos2 = 90;   
  }
}  
  
//ButtonUp Check
if(buttonState == 0){
  buttonUp = true;
}
//Power Off
if(PowerOn == true && buttonState == 1 && buttonUp == true){
  buttonCount += 1;
  if(buttonState == 1){
    if(buttonCount >= buttonTimer){
        PowerOn = false;
        buttonUp = false;
        massageState = 0;
        servo1Rotate0 = true;
        servo1Rotate180 = false;
        servo2Rotate0 = false;
        servo2Rotate180 = true;
    }
  }
}
//Check Button massageState Switch
else if(buttonState == 0 && buttonCount > 0 && buttonCount < buttonTimer){
  if(massageState == 0){
    servo1Rotate0 = true;
    servo1Rotate180 = false;
    servo2Rotate0 = true;
    servo2Rotate180 = false;
    massageState = 1;
     doOnce = true;    
  }
  else if(massageState == 1){
    servo1Rotate0 = true;
    servo1Rotate180 = false;
    servo2Rotate0 = false;
    servo2Rotate180 = false;
    massageState = 2;
        doOnce = true;
  }
    else if(massageState == 2){
    servo1Rotate0 = true;
    servo1Rotate180 = false;
    servo2Rotate0 = false;
    servo2Rotate180 = true;
    massageState = 0;
        doOnce = true;
  }
  buttonCount = 0;
}
else{
    buttonCount = 0;
}
}