#include <Ethernet.h>
#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

// Ethernet configuration (update with your network details)
byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; // MAC address of Ethernet Shield
IPAddress ip(192, 168, 1, 177);  // Arduino's IP address
EthernetServer server(80);      // Web server on port 80

// Servo objects for controlling the steering
Servo servo1; // Left servo
Servo servo2; // Right servo
Servo thruster; // Thruster control

// MPU-6050 object
MPU6050 mpu;

// PID control variables for steering
double desiredHeading = 0; // Desired heading input via web interface
double currentHeading = 0; // Current heading from MPU-6050
double error = 0;          // Difference between desired and current heading
double pidOutput = 0;      // Output from PID controller

// PID constants
double Kp = 1.0, Ki = 0.0, Kd = 100.0;

// PID variables
double previousError = 0;
double integral = 0;
unsigned long lastTime;

// Store current angle for the servos
int currentAngle1 = 90;  // Left servo's current angle (0-180)
int currentAngle2 = 90;  // Right servo's current angle (0-180)

// Store current speed for the thruster
int currentThrusterPWM = 1500; // Thruster PWM value (1100-1900, with 1500 as neutral)

// Set a deadband threshold for small errors (in degrees)
double errorTolerance = 10.0; // Error threshold where we consider the system to be "close enough"

void setup() {
  // Start Ethernet and server
  Ethernet.begin(mac, ip);
  server.begin();

  // Attach the servos to pins
  servo1.attach(7);  // Pin 7 for Left Servo
  servo2.attach(8);  // Pin 10 for Right Servo
  thruster.attach(9); // Pin 11 for Thruster control

  // Initialize MPU-6050
  Wire.begin();
  mpu.initialize();

  // Start serial for debugging
  Serial.begin(9600);
  Serial.println("Server started, waiting for client...");

  // Check MPU connection
  if (mpu.testConnection()) {
    Serial.println("MPU6050 connection successful");
  } else {
    Serial.println("MPU6050 connection failed");
  }

  // Set initial positions of the servos
  servo1.write(currentAngle1);
  servo2.write(currentAngle2);

  // Initialize time tracking for PID
  lastTime = millis();
}

void loop() {
  // Read current heading from MPU-6050
  int16_t accelX, accelY, accelZ;
  int16_t gyroX, gyroY, gyroZ;

  mpu.getMotion6(&accelX, &accelY, &accelZ, &gyroX, &gyroY, &gyroZ);
  
  // Convert raw accelerometer data to heading (using atan2)
  currentHeading = atan2(accelY, accelX) * 180 / PI;
  if (currentHeading < 0) currentHeading += 360;

  // Calculate the error (difference between desired and current heading)
  error = desiredHeading - currentHeading;

  // Handle circular heading (e.g., 0 and 360 degrees should be close)
  if (error > 180) error -= 360;
  if (error < -180) error += 360;

  // Check if the error is within the tolerance (deadband)
  if (abs(error) < errorTolerance) {
    error = 2; // Prevent unnecessary corrections
  }

  // Calculate the PID output manually
  unsigned long now = millis();
  double dt = (now - lastTime) / 1000.0; // Convert to seconds
  lastTime = now;

  integral += error * dt;
  integral = constrain(integral, -100, 100); // Adjust limits based on your system

  double derivative = (error - previousError) / dt;
  pidOutput = Kp * error + Ki * integral + Kd * derivative;
  pidOutput = constrain(pidOutput, -100, 100); // Adjust limits as necessary
  previousError = error; // Store error for next derivative calculation
  // Print the current heading, error, and PID output to the Serial Monitor
  Serial.print("Current Heading: ");
  Serial.println(currentHeading);
  Serial.print("Error: ");
  Serial.println(error);
  Serial.print("pidOutput: ");
  Serial.println(pidOutput);
  Serial.print("Desired Heading: ");
  Serial.println(desiredHeading);

  // Update servo positions based on PID output
  // Servo angles are constrained to between 0 and 180 degrees
  currentAngle1 = constrain(90 + pidOutput, 0, 180); // Left servo
  currentAngle2 = constrain(90 - pidOutput, 0, 180); // Right servo

  servo1.write(currentAngle1);
  servo2.write(currentAngle2);

  // Set the thruster PWM value
  thruster.writeMicroseconds(currentThrusterPWM);

  // Handle client requests
  handleClient();
}

void handleClient() {
  EthernetClient client = server.available();
  if (client) {
    boolean currentLineIsBlank = true;
    String request = "";

    while (client.connected()) {
      if (client.available()) {
        char c = client.read();
        request += c;

        // If the request ends with a blank line, process it
        if (c == '\n' && currentLineIsBlank) {
          // Check if the client is requesting the current heading
          if (request.indexOf("GET /heading") >= 0) {
            // Send the current heading in plain text
            client.println("HTTP/1.1 200 OK");
            client.println("Content-Type: text/plain");
            client.println("Connection: close");
            client.println();
            client.print(currentHeading);
            client.println();
          } else {
            // Send the HTTP header
            client.println("HTTP/1.1 200 OK");
            client.println("Content-Type: text/html");
            client.println("Connection: close");
            client.println();

            // Handle the GET request to update heading, PID, and thruster
            handleRequest(request);

            // Send the HTML page with current values pre-filled
            sendHtmlResponse(client);
          }
          break;
        }
        if (c == '\n') {
          currentLineIsBlank = true;
        } else if (c != '\r') {
          currentLineIsBlank = false;
        }
      }
    }

    // Close the connection
    client.stop();
  }
}

// Function to handle requests and extract parameters
void handleRequest(String request) {
  // Extract desired heading
  int headingIndex = request.indexOf("heading=");
  if (headingIndex >= 0) {
    int headingValue = request.substring(headingIndex + 8, request.indexOf('&', headingIndex)).toInt();
    desiredHeading = constrain(headingValue, 0, 360);  // Constrain desired heading
  }

  // Extract PID values
  int kpIndex = request.indexOf("Kp=");
  if (kpIndex >= 0) {
    Kp = request.substring(kpIndex + 3, request.indexOf('&', kpIndex)).toDouble();
  }
  int kiIndex = request.indexOf("Ki=");
  if (kiIndex >= 0) {
    Ki = request.substring(kiIndex + 3, request.indexOf('&', kiIndex)).toDouble();
  }
  int kdIndex = request.indexOf("Kd=");
  if (kdIndex >= 0) {
    Kd = request.substring(kdIndex + 3, request.indexOf(' ', kdIndex)).toDouble();
  }

  // Extract thruster PWM
  int thrusterIndex = request.indexOf("thruster=");
  if (thrusterIndex >= 0) {
    int pwmValue = request.substring(thrusterIndex + 9, request.indexOf(' ', thrusterIndex)).toInt();
    currentThrusterPWM = constrain(pwmValue, 1100, 1900);  // Ensure PWM is between 1100 and 1900 µs
  }
}

// Function to send HTML response
void sendHtmlResponse(EthernetClient &client) {
  client.println("<html>");
  client.println("<head><title>Boat Control</title></head>");
  client.println("<body>");
  client.println("<h1>Boat Control</h1>");
  client.print("Current Heading: ");
  client.print(currentHeading);
  client.println("<br>");
  client.print("Left Servo Angle: ");
  client.print(currentAngle1);
  client.println("<br>");
  client.print("Right Servo Angle: ");
  client.print(currentAngle2);
  client.println("<br>");
  client.println("<form action=\"/\" method=\"get\">");
  client.print("Desired Heading (0-360): <input type=\"number\" name=\"heading\" min=\"0\" max=\"360\" value=\"");
  client.print(desiredHeading);
  client.println("\"><br>");
  client.print("Kp: <input type=\"number\" name=\"Kp\" step=\"0.01\" value=\"");
  client.print(Kp);
  client.println("\"><br>");
  client.print("Ki: <input type=\"number\" name=\"Ki\" step=\"0.01\" value=\"");
  client.print(Ki);
  client.println("\"><br>");
  client.print("Kd: <input type=\"number\" name=\"Kd\" step=\"0.01\" value=\"");
  client.print(Kd);
  client.println("\"><br>");
  client.print("Thruster PWM: <input type=\"number\" name=\"thruster\" min=\"1100\" max=\"1900\" value=\"");
  client.print(currentThrusterPWM);
  client.println("\"><br>");
  client.println("<input type=\"submit\" value=\"Update\">");
  client.println("</form>");
  client.println("</body>");
  client.println("</html>");
}