//Code for Servo Motor

int solPin = A3;

bool shouldActivate = false;
Servo myServo;
int servoPos = 0;
int vibration = 12;
bool vibrate_up = true;
int level = 0;
int step = 0;


void handleActivateMotor( const char *event, const char *data)
{
    char raw[20];
    strcpy(raw, data);
    
    char *level_raw = strtok(raw, ",");
    char *step_raw = strtok(NULL, ",");
    level = atoi(level_raw);
    step = atoi(step_raw);
    
    
    Serial.print(level);
    Serial.print(",");
    Serial.println(step);
    


}

void setup()
{
    myServo.attach(solPin);
    // Particle.function("servo", servoControl);
    Particle.variable(  "servoPos" , &servoPos , INT );
    Particle.subscribe( "blinkLED", handleActivateMotor );
}

void loop()
{
    if(level == 0) {
        if(step < 50) {
            servoPos = constrain(step + vibration, 0, 130);
        }
        else {
            servoPos = constrain(50 * 2 + vibration, 0, 130);
        }
    }
    else {
        if(vibrate_up) {
            if(step < 100) {
                servoPos = constrain(step + 2 * vibration, 0, 130);
            }
            else {
                servoPos = constrain(50 * 2 + 2 * vibration, 0, 130);
            }
            vibrate_up = false;
        }
        else {
            if(step < 100) {
                servoPos = constrain(step, 0, 130);
            }
            else {
                servoPos = constrain(50 * 2, 0, 130);
            }
                vibrate_up = true;
        }
    }
    myServo.write(servoPos);
    if(level != 0) {
        delay(300 / level);
    }

}