int servoPin = A1;         // Servo control pin
int buttonPin = D3;        // Button input pin
int servoPos = 0;          // Initial servo position
bool buttonPressed = false;

void setup() {
    // Set servoPin as output and buttonPin as input
    pinMode(servoPin, OUTPUT);
    pinMode(buttonPin, INPUT);

    // Register Particle functions and variables
    Particle.function("servo", servoControl);
    Particle.variable("servoPos", &servoPos, INT);
}

void loop() {
    // Check if the button is pressed
    if (digitalRead(buttonPin) == HIGH) {
        if (!buttonPressed) {
            // Set servo to 90 degrees when button is pressed
            setServoAngle(90);
            buttonPressed = true;
            delay(500);  // Debounce delay
        }
    } else {
        if (buttonPressed) {
            // Reset servo to 0 degrees when button is released
            setServoAngle(0);
            buttonPressed = false;
        }
    }
}

int servoControl(String command) {
    // Convert command to integer and constrain it between 0 and 180
    int newPos = command.toInt();
    servoPos = constrain(newPos, 0, 180);

    // Set the servo to the new position
    setServoAngle(servoPos);

    return 1;  // Return success
}

// Function to set the servo angle using PWM
void setServoAngle(int angle) {
    // Convert angle (0 to 180) to pulse width (500 to 2500 microseconds)
    int pulseWidth = map(angle, 0, 180, 500, 2500);
    
    // Create a 20ms period PWM signal with the required pulse width
    for (int i = 0; i < 50; i++) {  // 50 cycles of 20ms for stability
        digitalWrite(servoPin, HIGH);
        delayMicroseconds(pulseWidth);
        digitalWrite(servoPin, LOW);
        delay(20 - pulseWidth / 1000);  // Complete the 20ms period
    }
}