#include <WiFi.h>
#include <pwmWrite.h>

Pwm pwm = Pwm();

const int servoPin1 = 2;
const int servoPin2 = 4;
const int servoPin3 = 6;  // create servo objects to control servos
const char* ssid = "ServoTest"; //Wifi name
const char* password = "Servo123"; //Wifi password
const int port = 16;

WiFiServer server (port);
WiFiClient client;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Serial connected");
  WiFi.begin(ssid, password);
  Serial.println("WiFi beginning...");

  while (WiFi.status() != WL_CONNECTED){
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }
  server.begin();

  pwm.writeServo(servoPin1, 0);
  pwm.writeServo(servoPin2, 0);
  pwm.writeServo(servoPin3, 0);

  Serial.println("Servo 1 angle set to 0 degrees");
  Serial.println("Servo 2 angle set to 0 degrees");
  Serial.println("Servo 3 angle set to 0 degrees");
  Serial.println("Server started");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());

}

void loop() {
  client = server.available();
  
  if (client) {
    if (client.connected()) {
      while (client.available() > 0) {
        String data = client.readStringUntil('\n');
        data.trim();
        
        int degree = data.toInt();
        
        if (degree >= 0 && degree <= 180) {
          pwm.writeServo(servoPin1, degree);  // Set servo angle directly
          Serial.print("Servo 1 angle set to: ");
          Serial.println(degree);
        }
        else if (degree >= 270 && degree <= 450) {
          pwm.writeServo(servoPin2, degree - 270);
          Serial.print("Servo 2 angle set to: ");
          Serial.println(degree - 270);
        }
        else if (degree >= 540 && degree <= 720)
        {
          pwm.writeServo(servoPin3, degree - 540);
          Serial.print("Servo 3 angle set to: ");
          Serial.println(degree - 540);
        }
      }
      
      client.stop();
    }
  }
  
  delay(100);
}