#include <SPI.h>
  #include <Ethernet.h>
  #include <OneWire.h>
  #include <DallasTemperature.h>
  #include <ESP32Servo.h>

  // WIZ820io Ethernet Module Pins for ESP32-C3
  #define SS_PIN 2  // Chip Select
  #define MOSI_PIN 10
  #define MISO_PIN 9
  #define SCK_PIN 8

  // Servo and Sensor Pins
  #define SERVO_DIVE_PIN 3
  #define SERVO_TURN_PIN 4
  #define WATER_TEMP_PIN 20
  const int relayPin = 5;


  // Temperature Sensor Setup
  OneWire oneWire(WATER_TEMP_PIN);
  DallasTemperature sensors(&oneWire);

  // Servo Setup
  Servo diveServo;
  Servo turnServo;

  // Ethernet Settings (Static IP)
  byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
  IPAddress ip(192, 168, 1, 177); // ESP32-C3 Static IP
  IPAddress subnet(255, 255, 255, 0); // Subnet Mask

  EthernetServer server(80);

  // Servo Positions
  int divePos = 92; 
  int turnPos = 104;
  const int DIVE_MIN = 50, DIVE_MAX = 120;
  const int TURN_MIN = 74, TURN_MAX = 134;

  bool calibrating = false;

  void setup() {
      Serial.begin(115200);
      SPI.begin(SCK_PIN, MISO_PIN, MOSI_PIN, SS_PIN);
      Ethernet.init(SS_PIN);
      Ethernet.begin(mac, ip, subnet);
      server.begin();

      Serial.print("ESP32-C3 IP Address: ");
      Serial.println(Ethernet.localIP());

      sensors.begin();
      diveServo.attach(SERVO_DIVE_PIN);
      turnServo.attach(SERVO_TURN_PIN);
      pinMode(relayPin, OUTPUT);

      resetServos();
      Serial.println("READY");
  }

  void loop() {
      EthernetClient client = server.available();
      if (client) {
          Serial.println("Client connected!");
          String command = "";

          while (client.connected()) {
              if (client.available()) {
                  char c = client.read();
                  if (c == '\n') break;
                  command += c;
              }
          }

          command.trim();
          Serial.printf("Received Command: %s\n", command.c_str());
          processCommand(command);

          client.println("OK:" + command);
          delay(50);
          while (client.available()) client.read();
          Serial.println("Waiting for next command...");
      }
  }

  void processCommand(String cmd) {
      if (cmd == "CALIBRATE") {
          if (!calibrating) {
              calibrating = true;
              calibrateServos();
              calibrating = false;
          }
      } else if (cmd == "RESET") {
          resetServos();
      } else if (cmd == "DIVE_MAX" || cmd == "DIVE") {
          divePos = constrain(divePos + 5, DIVE_MIN, DIVE_MAX);
          diveServo.write(divePos);
          Serial.printf("OK:DIVE (%d)\n", divePos);
      } else if (cmd == "RISE_MAX" || cmd == "RISE") {
          divePos = constrain(divePos - 5, DIVE_MIN, DIVE_MAX);
          diveServo.write(divePos);
          Serial.printf("OK:RISE (%d)\n", divePos);
      } else if (cmd == "LEFT") {
          turnPos = constrain(turnPos - 5, TURN_MIN, TURN_MAX);
          turnServo.write(turnPos);
          Serial.printf("OK:LEFT (%d)\n", turnPos);
      } else if (cmd == "RIGHT") {
          turnPos = constrain(turnPos + 5, TURN_MIN, TURN_MAX);
          turnServo.write(turnPos);
          Serial.printf("OK:RIGHT (%d)\n", turnPos);
      } else if (cmd == "STEERING_RESET") {
          turnPos = 104;
          turnServo.write(turnPos);
          Serial.printf("OK:STEERING RESET (%d)\n", turnPos);
      } else if (cmd == "FORWARD") {
          digitalWrite(relayPin, HIGH);
          Serial.println("OK:FORWARD (Motor ON)");
      } else if (cmd == "FORWARD_STOP") {
          digitalWrite(relayPin, LOW);
          Serial.println("OK:FORWARD_STOP (Motor OFF)");
      } else if (cmd == "DIVE_RESET") {
          divePos = 50;
          diveServo.write(divePos);
          delay(500);
          divePos = 92;
          diveServo.write(divePos);
          Serial.printf("OK:DIVE RESET (%d)\n", divePos);
      } else if (cmd == "READ_TEMP") {
          sensors.requestTemperatures();
          float temp = sensors.getTempCByIndex(0);
          Serial.printf("TEMP: %.2fC\n", temp);
      } else {
          Serial.printf("ERROR: Unknown command (%s)\n", cmd.c_str());
      }
  }

  void resetServos() {
      divePos = 92;
      turnPos = 104;
      diveServo.write(divePos);
      turnServo.write(turnPos);
      Serial.printf("OK:RESET (Dive %d, Turn %d)\n", divePos, turnPos);
  }

  void calibrateServos() {
      Serial.println("CALIBRATION START...");
      diveServo.write(120); delay(500);
      diveServo.write(50); delay(500);
      diveServo.write(92); delay(500);
      turnServo.write(134); delay(500);
      turnServo.write(74); delay(500);
      turnServo.write(104); delay(500);
      Serial.println("CALIBRATION DONE");
  }